led improvements and try improving motor controller

controller/mixercontroller_w5100_pio/src/main.cpp

@@ -29,6 +29,7 @@ void setMuteChannel(uint8_t i, boolean state);
 void publishCurrentSetVolume();
 void publishAllStates(int pn, String pTopicname, boolean (*pgetBit) (uint8_t));
 void changeRelaisByNumber(uint8_t pn, String pTopicPrefix, String pTopic, String pspayload, void (*psetXChannel) (uint8_t, boolean));
+float getSetVolume();
 
 #define LEDPIN 9  //PB1 = D9 = Pin15
 Adafruit_NeoPixel leds = Adafruit_NeoPixel(9, LEDPIN, NEO_GRB + NEO_KHZ800);
@@ -48,13 +49,17 @@ EthernetClient ethClient;
 PubSubClient mqttClient;
 String mqttdevname="audiomixer/";
 
+bool flag_publishCurrentSetVolume=false;
+
 
 long last_send=0;
+#define MAXIMUMMQTTSENDINTERVAL 100
 #define MQTTRECONNECTDELAY 5000
 unsigned long last_mqttreconnectattempt=0;
 
 
 
+
 //Serial
 long last_serialdebug=0;
 #define INTERVAL_SERIALDEBUG 200
@@ -84,6 +89,7 @@ uint16_t srbits=0;
 #include <Encoder.h>
 Encoder volEnc(PIN_ENCA,PIN_ENCB);
 float encoderMultiplier=4.0;
+int volEncVel=0;
 
 //Servo stuff
 #define PIN_MOTOR_IN1 PD5 //to L293(pin2) Motor IN1 
@@ -91,18 +97,22 @@ float encoderMultiplier=4.0;
 //#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;
+int _motormove;
 
 #define PIN_POT A0 //A0 = PC0, reference potentiometer wiper
 #define DEADZONE_POTI 10 //maximum allowed error. stop when reached this zone
 #define POT_MIN 45 //minimum value pot can reach
 #define POT_MAX 950 //maximum value pot can reach
-#define POTIFILTER 0.6 //0 to 1. 1 means old value stays forever
+#define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever
-#define MAX_MOTOR_PWM 255 //0 to 255. Maximum pwm to output
+#define MAX_MOTOR_PWM 192 //0 to 255. Maximum pwm to output
 
 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
 
+int last_potidifference=0;
+int potidifference=0; //gets overwritten at start of each motorcheck
+
 //#define MOTOR_STOP(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,LOW);
 //#define MOTOR_LEFT(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,HIGH);
 //#define MOTOR_RIGHT(); srWrite(SRPIN_MOTOR_IN1,HIGH); srWrite(SRPIN_MOTOR_IN2,LOW);
@@ -117,24 +127,23 @@ boolean poti_reachedposition=true; //set to true if position reached. after that
 
 //Motorcheck
 long last_motorcheck=0;
-#define INTERVAL_MOTORCHECK 100 //check motor movement every x ms
+#define INTERVAL_MOTORCHECK 50 //check motor movement every x ms
 //int poti_read_last=0;
 //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 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
 
-float motorP=4.0;
+float motorP=2.0;
-float motorI=0.5;
+float motorI=0.05;
+float motorD=1;
 float potidifference_integral=0;
-#define MOTORI_ANTIWINDUP 200 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
+#define MOTORI_ANTIWINDUP 100 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
 //Motor starts moving at about speed=80
 
 long last_potidifferenceLow=0;
 #define DEADZONETIMEUNTILREACHED 250 //time [ms] poti read value has to be inside of deadzone to set reachedposition flag (and stop regulating)
 
-//error
-boolean motorerror=false;
 
 //Menu system
 uint8_t menu_mode=0; //0= volume set mode, 1=mute output selection, 2=output group selection
@@ -327,10 +336,10 @@ void loop() {
 
 
   //Read Encoder to velocity "volEncVel"
-  int volEncVel=0;
+  
   int _volEnc=volEnc.read();
   if (_volEnc!=0){ //encoder moved
-    volEncVel=_volEnc;
+    volEncVel+=_volEnc;
     volEnc.write(0); //reset value
   }
   
@@ -342,20 +351,23 @@ void loop() {
         poti_set+=volEncVel*encoderMultiplier; //change poti set value
         poti_set=constrain(poti_set, POT_MIN,POT_MAX);
         poti_reachedposition=false;
-        publishCurrentSetVolume();
+        flag_publishCurrentSetVolume=true;
+        volEncVel=0; //reset vel for next loop
         break;
 
       case 1: case 2: //mute or group selection 
         menu_selectedChannel+=127; //offset to compensate negative values
-        menu_selectedChannel+=volEncVel;
+        menu_selectedChannel+=volEncVel/2; //every encoder detend is +-2
         if (menu_selectedChannel<127){
           menu_selectedChannel=127; //lower limit (0)
-        }else if (menu_selectedChannel > MENU_MAXCHANNEL+1) { //max channel and one extra for "nothing selected"
+        }else if (menu_selectedChannel > 127+MENU_MAXCHANNEL+1) { //max channel and one extra for "nothing selected"
           menu_selectedChannel=127+MENU_MAXCHANNEL+1; //upper limit
         }
-
         menu_selectedChannel-=127; //take out offset
-        
+
+        if (volEncVel/2 != 0) { //if value change was at least one detend
+          volEncVel=0; //reset vel for next loop
+        }
         
         break;
     }
@@ -363,58 +375,93 @@ void loop() {
   }
 
 
+  if (flag_publishCurrentSetVolume && loopmillis-last_send>MAXIMUMMQTTSENDINTERVAL){
+    flag_publishCurrentSetVolume=false;
+    last_send = loopmillis;
+    publishCurrentSetVolume();
+  }
+
+
   //Motor Movement Routine ################# 
 
-  if (!motorerror) //motor not stuck etc
+  if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) 
   {
-    if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) 
+    last_motorcheck=loopmillis;
-    {
-      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;
-        potidifference_integral=constrain(potidifference_integral,-MOTORI_ANTIWINDUP,MOTORI_ANTIWINDUP); //constrain
-        _motormove=potidifference*motorP+potidifference_integral;
-        motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM);
-        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) {
+    last_potidifference = potidifference; //save last difference
-          if (last_potidifferenceLow==0){
+    potidifference=poti_set-poti_read; //positive means poti needs to be moved higher. max poti value is 1023
-            last_potidifferenceLow=loopmillis; //save millis first time entered deadzone
+    
-          }
+    
-          if (loopmillis-last_potidifferenceLow>DEADZONETIMEUNTILREACHED) {
+    
-            poti_reachedposition=true;
+    if (poti_reachedposition) {
-          }
+      motorspeed=0;
-          
+      potidifference_integral=0;
+      MOTOR_STOP();
+    }else{ //not reached position
+      _motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255
+
+      potidifference_integral+=potidifference*motorI;
+      potidifference_integral=constrain(potidifference_integral,-MOTORI_ANTIWINDUP,MOTORI_ANTIWINDUP); //constrain
+      _motormove=potidifference*motorP + potidifference_integral + motorD*(last_potidifference-potidifference);
+      motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM);
+      
+      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{
-          last_potidifferenceLow = 0;
+          MOTOR_STOP();
         }
       }
+
+      if ( (potidifference>0 && last_potidifference<0) || (potidifference<0 && last_potidifference>0) ) { //different signs. potidifference has crossed 0. set value overshoot
+        potidifference_integral=0; //reset integral to stop further overshoot
+      }
+
+
+      /*
+      Serial.print(" diff=");
+      Serial.print(potidifference);
+      Serial.print(" iVal=");
+      Serial.print(potidifference_integral);
+      Serial.print(" dVal=");
+      Serial.print((last_potidifference-potidifference)*motorD);
+      Serial.print(" motormove=");
+      Serial.print(_motormove);
+      if (poti_reachedposition) {
+        Serial.print("!");
+      }
+      Serial.println("");
+      */
+      
+
+      
+      
+
+      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;
+      }
+
     }
   }
+  
 
   
 
@@ -423,9 +470,15 @@ void loop() {
 
     switch(menu_mode) {
       case 0: //volume
-        for(uint8_t i=0;i<leds.numPixels();i++){ //set color of all leds
+        if (poti_reachedposition) { //idle
-          leds.setPixelColor(i, Wheel(wheelpos+i*10));
+          for(uint8_t i=0;i<leds.numPixels();i++){ //set color of all leds
+            leds.setPixelColor(i, Wheel(wheelpos+i*10));
+          }
+        }else{ //motor is currently moving
+          leds.clear();
+          leds.setPixelColor(min(getSetVolume()*8/100, 7), Wheel(wheelpos)); //show volume as bargraph
         }
+
         wheelpos+=1;
         break;
 
@@ -436,9 +489,9 @@ void loop() {
         uint8_t _g=0;
         uint8_t _b=0;
 
-        for(uint8_t i=0;i<leds.numPixels();i++){ //set color of all leds
+        for(uint8_t i=0;i<=MENU_MAXCHANNEL;i++){ //set color of all channel leds
           if (menu_mode==1) { //in mute mode
-            if (!getMute(i)) { //not muted
+            if (getMute(i)) { //not muted
               _r = 0; _g = 50; _b = 0;
             }else{ //muted
               _r = 50; _g = 0; _b = 50;
@@ -456,7 +509,9 @@ void loop() {
             _b*=4;
           }
           if (menu_selectedChannel==MENU_MAXCHANNEL+1) { //nothing selected
-            _r=50; _g=50; _b=50;
+            _r*=4; //make color brighter
+            _g*=4;
+            _b*=4;
           }
           leds.setPixelColor(i, leds.Color(_r,_g,_b));
         }
@@ -465,12 +520,11 @@ void loop() {
         break;
     }
 
-    
-  
     leds.show();
   }
   
 
+  /*
   if ( loopmillis > last_serialdebug+INTERVAL_SERIALDEBUG){
     last_serialdebug=loopmillis;
 
@@ -478,8 +532,10 @@ void loop() {
     Serial.print(poti_set);
     Serial.print(" is=");
     Serial.print(poti_read);
-    Serial.print(" mspeed=");
+    //Serial.print(" mspeed=");
-    Serial.print(motorspeed);
+    //Serial.print(motorspeed);
+    Serial.print(" motormove=");
+    Serial.print(_motormove);
     Serial.print(" iVal=");
     Serial.print(potidifference_integral);
     if (poti_reachedposition) {
@@ -487,6 +543,7 @@ void loop() {
     }
     Serial.println("");
   }
+  */
 
   if (loopmillis%5001==0) { //TODO: remove when working
     Serial.println(loopmillis); //alive print. for debugging
@@ -595,7 +652,7 @@ void setMuteInt(uint8_t i) {
   srbits = ((i<<NUMSELECTCHANNELS) & mask) | (srbits & ~mask);
   srShiftOut();
 }
-boolean getMute(uint8_t pbit) {
+boolean getMute(uint8_t pbit) { //low is muted
   return srbits & (1<<(pbit+NUMSELECTCHANNELS)); //check bit at position
 }
 void setSelectionInt(uint8_t i) {
@@ -621,7 +678,7 @@ void setMuteChannel(uint8_t i, boolean state){
 
 void publishCurrentSetVolume()
 {
-  float _setpercentage=map(poti_set,POT_MIN,POT_MAX, 0.0,100.0); //get percentage from set poti value
+  float _setpercentage=getSetVolume();
   char pub_payload[8]; // Buffer big enough for 7-character float
   dtostrf(_setpercentage, 1, 2, pub_payload);
   mqttClient.publish("audiomixer/volume", pub_payload);
@@ -660,4 +717,8 @@ void changeRelaisByNumber(uint8_t pn, String pTopicPrefix, String pTopic, String
       mqttClient.publish((char*) pub_topic.c_str(), "true");
     }
   }
+}
+
+float getSetVolume() {
+  return map(poti_set,POT_MIN,POT_MAX, 0.0,100.0); //get percentage from set poti value
 }