From 836d2e3e6eb2deda93ae1ae6c3bc45c73b6ead02 Mon Sep 17 00:00:00 2001
From: Eggert Jung <eggert.s.jung@wischhof13.de>
Date: Mon, 4 Apr 2022 15:39:54 +0200
Subject: [PATCH] add pid controller

---
 pid_controller.h | 34 ++++++++++++++++++++++++++++++++++
 taenzer.c        | 51 +++++++++++++++++++++++++++++++++++++++++----------
 2 files changed, 75 insertions(+), 10 deletions(-)
 create mode 100644 pid_controller.h

diff --git a/pid_controller.h b/pid_controller.h
new file mode 100644
index 0000000..56bf0f2
--- /dev/null
+++ b/pid_controller.h
@@ -0,0 +1,34 @@
+/*
+ * pid_controller.h
+ *
+ *  Created on: Apr 24, 2016
+ *      Author: subham roy
+ */
+
+#ifndef __PID_CONTROLLER_H
+#define __PID_CONTROLLER_H
+
+typedef struct {
+  /* PID controller parameters */
+  double Kp;
+  double Ki;
+  double Kd;
+  
+  /* max output limits for the PID controller */
+  double output_max;
+  double output_min;
+  
+  /* below are session variables for the PID controller */
+  double _integral_sum;
+  double _prev_err;
+  double _dt;
+} PID_vars;
+
+
+#define PID_VARS_INIT(x) PID_vars x = {.Kp=1.0,.Ki=0.00,.Kd=0.1,.output_max=15000.0, \
+                       .output_min=-15000.0,._integral_sum=0.0,._prev_err=0.0,._dt=1.0}
+
+/* Function Prototypes */
+double pid(PID_vars *vars, double current_err);
+
+#endif
diff --git a/taenzer.c b/taenzer.c
index 65b7909..d04f6ba 100644
--- a/taenzer.c
+++ b/taenzer.c
@@ -3,7 +3,11 @@
 #include "taenzer.h"
 #include "avrIOhelper/io-helper.h"
 #include "kraftsensor.h"
+#include "notaus.h"
+#include "pid_controller.h"
 
+#include <stdint.h>
+#include <stdio.h>
 #include <stdlib.h>
 
 #define TAENZER_KRAFT_SETPOINT 12000
@@ -11,6 +15,7 @@
 
 
 taenzer_state_t taenzer_state;
+PID_VARS_INIT(regler);
 
 taenzer_state_t taenzer_state = {
     .homed = 0,
@@ -18,6 +23,28 @@ taenzer_state_t taenzer_state = {
     .force_setpoint = TAENZER_KRAFT_SETPOINT,
 };
 
+double pid(PID_vars *vars, double current_err) {
+
+  /* current_error = setpoint - current_process_variable */
+
+  vars->_integral_sum += current_err*(vars->_dt);
+
+  double output = (vars->Kp)*current_err                      \
+                + (vars->Ki)*(vars->_integral_sum)            \
+                + (vars->Kd)*((current_err-(vars->_prev_err)) \
+                              /(vars->_dt));
+
+  vars->_prev_err = current_err;
+
+  /* limit output within output_min and output_max */
+  if (output>(vars->output_max))
+    output = vars->output_max;
+  else if (output<(vars->output_min))
+    output = vars->output_min;
+
+  return output;
+}
+
 void send_info(void);
 void do_taenzer(){
 
@@ -49,19 +76,23 @@ void do_taenzer(){
     if(kraftsensor_valid && taenzer_state.homed && taenzer_state.pos >= 0 && taenzer_state.pos < 500000){
         int16_t err = (kraftsensor_value - taenzer_state.force_setpoint);
 
-        if(err > 8000)
-            err=8000;
+        double pid_out = pid(&regler, err);
+        int16_t out = (int16_t)pid_out;
 
-        ICR4 = 400000/err;
+        ICR4 = 400000/abs(out);
         OCR4A = ICR4/2;
 
-        if(kraftsensor_value < TAENZER_KRAFT_SETPOINT - (TAENZER_KRAFT_HYST/2)){
-            set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
-            TCCR4B |= _BV(CS41); //TURN ON
-        }
-        else if(kraftsensor_value > TAENZER_KRAFT_SETPOINT + (TAENZER_KRAFT_HYST/2)){
-            set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
-            TCCR4B |= _BV(CS41); //TURN ON
+        if(kraftsensor_valid && notaus_state == POWER_ON){
+            if(out < -TAENZER_KRAFT_HYST/2 && taenzer_state.pos < 300000UL){
+                set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
+                TCCR4B |= _BV(CS41); //TURN ON
+            }
+            else if(out > TAENZER_KRAFT_HYST/2 && taenzer_state.pos > 0){
+                set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
+                TCCR4B |= _BV(CS41); //TURN ON
+            }
+            else
+                TCCR4B &= ~_BV(CS41); //TURN OFF
         }
         else
             TCCR4B &= ~_BV(CS41); //TURN OFF