implement PI controller for motor movement

controller/mixercontroller_w5100_pio/src/main.cpp

@@ -1,3 +1,11 @@
+/*
+  * Ideas/TODO:
+  * POT_MIN, POT_MAX as variable with calibration procedure. Drive slowly to both ends until value does not get lower.
+  * Motor error checking. Timeout overall (if regulation fails or stuck). Timeout movement (motor is tunring but no change in poti value detected). Move right direction.
+  * Relais switching (selection and muting)
+  * MQTT topics
+*/
+
 #include <Arduino.h>
 
 #include <Adafruit_NeoPixel.h>
@@ -67,6 +75,7 @@ float encoderMultiplier=4.0;
 #define PIN_MOTOR_IN2 PD6 //to L293(pin7) Motor IN2
 //#define SRPIN_MOTOR_IN1 1 //L293(pin2) Motor IN1   -- moved to atmega pin
 //#define SRPIN_MOTOR_IN2 2 //L293(pin7) Motor IN2   -- moved to atmega pin
+uint8_t motorspeed=0;
 
 #define PIN_POT A0 //A0 = PC0, reference potentiometer wiper
 #define DEADZONE_POTI 5 //maximum allowed error. stop when reached this zone
@@ -74,7 +83,7 @@ float encoderMultiplier=4.0;
 #define POT_MAX 1010 //maximum value pot can reach
 #define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever
 
-int poti_set=512; //set value
+int poti_set; //set value, initial value will be read from poti
 int poti_read=0; //read value from poti
 boolean poti_reachedposition=true; //set to true if position reached. after that stop turning
 
@@ -85,22 +94,30 @@ boolean poti_reachedposition=true; //set to true if position reached. after that
 #define MOTOR_STOP(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,LOW);
 #define MOTOR_LEFT(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,HIGH);
 #define MOTOR_RIGHT(); digitalWrite(PIN_MOTOR_IN1,HIGH); digitalWrite(PIN_MOTOR_IN2,LOW);
+#define MOTOR_LEFT_PWM(); digitalWrite(PIN_MOTOR_IN1,LOW); analogWrite(PIN_MOTOR_IN2,motorspeed);
+#define MOTOR_RIGHT_PWM(); analogWrite(PIN_MOTOR_IN1,motorspeed); digitalWrite(PIN_MOTOR_IN2,LOW);
 #define MOTOR_TURNING() (digitalRead(PIN_MOTOR_IN1) != digitalRead(PIN_MOTOR_IN2))
 
 
 //Motorcheck
 long last_motorcheck=0;
 #define INTERVAL_MOTORCHECK 100 //check motor movement every x ms
-int poti_read_last=0;
+//int poti_read_last=0;
-int motor_vel=0; //analog read units per second
+//int motor_vel=0; //analog read units per second //TODO: reintroduce into code or remove
-#define MINIMUM_MOTORVEL 20 //minimum velocity motor should turn wenn active
+//#define MINIMUM_MOTORVEL 20 //minimum velocity motor should turn wenn active
-#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate
+//#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate
-long last_motorTooSlow=0; //typically 0
+//long last_motorTooSlow=0; //typically 0
+
+float motorP=1.0;
+float motorI=0.1;
+float potidifference_integral=0;
+#define MOTORI_ANTIWINDUP 32 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
+
+long last_potidifferenceLow=0;
+#define DEADZONETIMEUNTILREACHED 500 //time [ms] poti read value has to be inside of deadzone to set reachedposition flag (and stop regulating)
 
 //error
-uint8_t error=0;
+boolean motorerror=false;
-#define NOERROR 0
-#define MOTORDIDNOTTURN 1
 
 void setup() {
   pinMode(PIN_BUTTON,INPUT_PULLUP);
@@ -108,6 +125,7 @@ void setup() {
 
   pinMode(PIN_POT,INPUT);
   
+
   pinMode(SRLATCH, OUTPUT);
   pinMode(SRCLOCK, OUTPUT);
   pinMode(SRDATA, OUTPUT);
@@ -162,6 +180,10 @@ void setup() {
     Serial.println("Ethernet disabled");
   }
 
+
+  poti_set=analogRead(PIN_POT);
+  Serial.print("Poti value="); Serial.println(poti_set);
+
   Serial.println("Ready");
   last_send = millis();
   
@@ -218,20 +240,8 @@ void loop() {
   //Inputs ###################################################
   poti_read=poti_read*POTIFILTER + (1.0-POTIFILTER)*analogRead(PIN_POT); //read poti
   
-  /*
-  if (!digitalRead(PIN_BUTTON)){ //button pressed
-    if (button_released){
-      button_released=false; //flag: not released
-      if(loopmillis-last_button_released > BUTTON_RELEASE_DEBOUNCE){
-        button_flag=true;
-      }
-    }
-  }else if(!button_flag && !button_released){ //button released and flag has been cleared
-    last_button_released=loopmillis;
-    button_released=true;
-  }*/
   
-  button_knob.update(millis(),!digitalRead(PIN_BUTTON));
+  button_knob.update(millis(),!digitalRead(PIN_BUTTON)); //Update routine
   
   if (button_knob.buttonPressed()){
     Serial.println("Button Pressed");
@@ -263,9 +273,10 @@ void loop() {
 
 
   //Motor Movement Routine #################
+  /*
   if (error==0){ //no errors
       
-    if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //error too high
+    if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //difference too high
       if (poti_read-poti_set < 0){
         MOTOR_LEFT();
       }else{
@@ -307,6 +318,71 @@ void loop() {
   }else{ //an error occured. error!=0
     MOTOR_STOP();
   }
+  */
+
+  //Test
+  /*if (poti_set == 512){
+    MOTOR_STOP();
+  }else if (poti_set < 512){
+    motorspeed=(512-poti_set)/2;
+    MOTOR_LEFT_PWM();
+  }else if (poti_set > 512){
+    motorspeed=(poti_set-512)/2;
+    MOTOR_RIGHT_PWM();
+  }*/
+
+  
+
+  if (!motorerror) //motor not stuck etc
+  {
+    if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) 
+    {
+      last_motorcheck=loopmillis;
+      int potidifference=poti_set-poti_read; //positive means poti needs to be moved higher. max poti value is 1023
+      if (poti_reachedposition) {
+        motorspeed=0;
+        potidifference_integral=0;
+        MOTOR_STOP();
+      }else{ //not reached position
+        
+        int _motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255
+        potidifference_integral+=potidifference*motorI;
+        _motormove=potidifference*motorP+potidifference_integral;
+        motorspeed=constrain(abs(_motormove), 0,255);
+        if (poti_read<=POT_MIN && _motormove<0) { //stop motor if soft endstops reached and wants to turn that way
+          MOTOR_STOP();
+          potidifference_integral=0;
+          _motormove=0;
+        }else if (poti_read>=POT_MAX && _motormove>0){  //stop motor if soft endstops reached and wants to turn that way
+          MOTOR_STOP();
+          potidifference_integral=0;
+          _motormove=0;
+        }else{ //no endstop reached
+          if (_motormove<0) {
+            MOTOR_LEFT_PWM();
+          }else if (_motormove>0) {
+            MOTOR_RIGHT_PWM();
+          }else{
+            MOTOR_STOP();
+          }
+        }
+        
+
+        if (abs(potidifference)<DEADZONE_POTI) {
+          if (last_potidifferenceLow==0){
+            last_potidifferenceLow=loopmillis; //save millis first time entered deadzone
+          }
+          if (loopmillis-last_potidifferenceLow>DEADZONETIMEUNTILREACHED) {
+            poti_reachedposition=true;
+          }
+          
+        }else{
+          last_potidifferenceLow = 0;
+        }
+      }
+    }
+  }
+
   
   
 
@@ -318,8 +394,10 @@ void loop() {
     Serial.print(poti_set);
     Serial.print(" is=");
     Serial.print(poti_read);
-    Serial.print(" vel=");
+    Serial.print(" motorspeed=");
-    Serial.print(motor_vel);
+    Serial.print(motorspeed);
+    Serial.print(" iValue=");
+    Serial.print(potidifference_integral);
     Serial.println("");
 
     if (button_flag){ //TODO: remove hier if correct behaviour implemented