basic p regulator

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
         }