From 696c9eac0ee704ba62373c25df267615aa4f5b42 Mon Sep 17 00:00:00 2001
From: Eggert Jung <eggert.s.jung@wischhof13.de>
Date: Mon, 21 Feb 2022 05:20:29 +0100
Subject: [PATCH] basic p regulator

---
 taenzer.c | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/taenzer.c b/taenzer.c
index de16c8e..d5603ee 100644
--- a/taenzer.c
+++ b/taenzer.c
@@ -4,6 +4,8 @@
 #include "avrIOhelper/io-helper.h"
 #include "kraftsensor.h"
 
+#include <stdlib.h>
+
 taenzer_state_t taenzer_state;
 
 void do_taenzer(){
@@ -21,11 +23,19 @@ void do_taenzer(){
 
     /* Force regualtion */
     if(kraftsensor_valid && taenzer_state.homed){
-        if(kraftsensor_value < 9000){
+        int16_t err = abs(kraftsensor_value - 10000);
+
+        if(err > 8000)
+            err=8000;
+
+        ICR4 = 400000/err;
+        OCR4A = ICR4/2;
+
+        if(kraftsensor_value < 19000){
             set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
             TCCR4B |= _BV(CS41); //TURN ON
         }
-        else if(kraftsensor_value > 10000){
+        else if(kraftsensor_value > 20000){
             set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
             TCCR4B |= _BV(CS41); //TURN ON
         }