separate into functions

src/main.cpp

@@ -17,13 +17,16 @@
 
 #define BUTTON_DEBOUNCE 200
 #define PIN_BUTTON1 D5
+#define PIN_BUTTON2 D6
 struct button{
   uint8_t pin;
   unsigned long last_time_read=0;
   bool down=false;
   bool changed=false;
+  bool manual_drive_direction=false; 
 };
 button button1;
+button button2;
 
 unsigned long last_sensor_read=0;
 #define SENSOR_READ_INTERVAL 20 //in ms
@@ -32,7 +35,7 @@ unsigned long last_sensor_read=0;
 #define CALCULATEPOSITIONESTIMATE_INTERVAL 100
 unsigned long last_calculate_position_estimate=0;
 
-bool manual_drive_direction=false;
+
 
 #define PIN_SENSE A0
 unsigned long last_print=0;
@@ -98,6 +101,7 @@ struct blindmodel
 };
 
 blindmodel blind1;
+blindmodel blind2;
 
 
 //Motor shield default I2C Address: 0x30
@@ -111,17 +115,23 @@ unsigned long last_motor_send=0;
 
 int getFitered(int* values,uint8_t size);
 uint8_t classifySensorValue(blindmodel &blind);
+void checkButton(button &btn);
 void checkModes(blindmodel &blind);
+void manualMoveHandler(button &btn, blindmodel &blind);
+void readSensor(blindmodel &blind);
+void estimatePosition(blindmodel &blind);
+void errorCheck(blindmodel &blind);
+void updateMotor(blindmodel &blind, Motor motor);
 
 void setup() {
-  
-  
   Serial.begin(115200);
   Serial.println("Starting");
 
   button1.pin=PIN_BUTTON1;
+  button2.pin=PIN_BUTTON2;
 
   pinMode(button1.pin, INPUT_PULLUP);
+  pinMode(button2.pin, INPUT_PULLUP);
   pinMode(PIN_SENSE, INPUT);
 
   M1.setmotor(_STOP);
@@ -142,186 +152,53 @@ void setup() {
   blind1.simulated_acc_dec=-150;
   blind1.simulated_acc_inc=200;
 
+  blind2.length_clear=50;
+  blind2.length_opaque=74;
+  blind2.sense_clear_lower=40;
+  blind2.sense_clear_upper=100;
+  blind2.sense_opaque_lower=500;
+  blind2.sense_opaque_upper=750;
+  blind2.sense_end_lower=850;
+  blind2.sense_end_upper=1024;
+  blind2.speedfactor=29;
+  blind2.start_first_clear=27;
+  blind2.simulated_acc_dec=-150;
+  blind2.simulated_acc_inc=200;
+
   //Test
   blind1.mode = MODE_FIND_END;
   blind1.mode_find_end_state=0; //reset mode find state
+
+  blind2.mode = MODE_FIND_END;
+  blind2.mode_find_end_state=0; //reset mode find state
   
 }
 
 
 void loop() {
 
-  button1.changed=false;
+  checkButton(button1);
-  if (millis() > button1.last_time_read + BUTTON_DEBOUNCE) {
+  
-    bool new_pin_button_down=!digitalRead(button1.pin);
-    if (button1.down != new_pin_button_down) { //changed
-      button1.down = new_pin_button_down; //update
-      button1.changed=true;
-      button1.last_time_read=millis(); //delay next check
-    }    
-  }
 
 
   //Manual movement by button
-  if (button1.changed) {
+  manualMoveHandler(button1, blind1);
-    blind1.mode=MODE_IDLE;
-    if (button1.down) { //changed to pressed
-      if (manual_drive_direction) {
-        //M1.setmotor( _CW, 100);
-        blind1.speed=-100;
-        //Serial.print("CW PWM: ");
-      }else{
-        blind1.speed=100;
-        //Serial.print("CCW PWM: ");
-      }
-      manual_drive_direction=!manual_drive_direction; //switch direction every new press
-    }else{ //changed to released
-      //Serial.println("Motor STOP");
-      blind1.speed=0;
-    }
-  }
 
 
 
   //Read sensor/encoder
-  if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) {
+  readSensor(blind1);
-    blind1.sense_read_pos=(blind1.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
-    blind1.sense_read[blind1.sense_read_pos]=analogRead(PIN_SENSE);
-
-    classifySensorValue(blind1); //writes to blindmodel.sense_status
-
-    
-    last_sensor_read=millis();
-  }
-
-  if (millis() > last_print + 500) {
-    Serial.print("SenseStatus=");
-    switch(blind1.sense_status){
-      case SENSESTATUS_UNKNOWN:
-        Serial.print("UNK");
-        break;
-      case SENSESTATUS_CLEAR:
-        Serial.print("CLE");
-        break;
-      case SENSESTATUS_OPAQUE:
-        Serial.print("OPA");
-        break;
-      case SENSESTATUS_END:
-        Serial.print("END");
-        break;
-    }
-    Serial.print("("); Serial.print(getFitered(blind1.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
-    Serial.print(", mode=");
-    switch(blind1.mode){
-      case MODE_IDLE:
-        Serial.print("IDL");
-        break;
-      case MODE_FIND_END:
-        Serial.print("FIN");
-        break;
-      case MODE_GO_TO_POS:
-        Serial.print("POS");
-        break;
-      case MODE_MEASURE_SPEED:
-        Serial.print("MSP");
-        break;
-    }
-    Serial.print(", speed=");
-    Serial.print(blind1.speed);
-    Serial.print(", speedSim=");
-    Serial.print(blind1.speedSimulated);    
-    Serial.print(", pos=");
-    Serial.println(blind1.position);
-    last_print=millis();
-  }
 
 
   checkModes(blind1);
 
+  errorCheck(blind1);
   
-  if (blind1.sense_status==SENSESTATUS_END) {
-    if (blind1.speed<0) { //stop driving up
-      blind1.speed=0;
-    }
-  }
   //Estimate blind position and correct
-  if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) {
+  estimatePosition(blind1);
-    float _actualTime=(millis()-last_calculate_position_estimate)/1000.0; //in seconds
-    blind1.speedSimulated+= constrain(blind1.speed - blind1.speedSimulated, blind1.simulated_acc_dec*_actualTime, blind1.simulated_acc_inc*_actualTime);
-    
-    blind1.position+= blind1.speedSimulated/100.0*blind1.speedfactor * _actualTime;
-    last_calculate_position_estimate=millis();
-  }
-
-  if (blind1.mode!= MODE_FIND_END) {
-    if (blind1.sense_status == SENSESTATUS_END && blind1.last_sense_status != SENSESTATUS_END) { //entered end marker
-      blind1.position=0;
-      float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
-      blind1.position += _filterdelay_correction; //correct for filter delay
-      Serial.print("Reached End marker. set Position="); Serial.println(blind1.position);
-    }
-
-    if (blind1.sense_status == SENSESTATUS_CLEAR || blind1.sense_status == SENSESTATUS_OPAQUE || blind1.sense_status == SENSESTATUS_END) { //either opaque or clear (or end, only for last update used)
-      if (blind1.sense_status != blind1.last_sense_status) { //status changed
-        if (blind1.last_sense_status == SENSESTATUS_CLEAR || blind1.last_sense_status == SENSESTATUS_OPAQUE) { //only changes from clear to opaque or opaque to clear
-
-          float _position_before=blind1.position;//only for text output debugging
-
-          if ((blind1.speedSimulated>0 && blind1.sense_status==SENSESTATUS_OPAQUE) || (blind1.speedSimulated<0 && blind1.sense_status==SENSESTATUS_CLEAR)) { //moving down from and clear to opaque OR moving up and from opaque to clear
-          
-            //estimated_position_difference = -blind1.position + blind1.start_first_clear+blind1.length_clear + _n*(blind1.length_clear+blind1.length_opaque); //possible occurances. let estimated_position=0 , solve for n
-            
-            int _n = round((blind1.position -blind1.start_first_clear - blind1.length_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest n
-            blind1.position = blind1.start_first_clear+blind1.length_clear + _n*(blind1.length_clear+blind1.length_opaque); //calculate position from _n
-            float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
-            blind1.position += _filterdelay_correction; //correct for filter delay
-            Serial.print("n=");
-             Serial.println(_n);
-
-            Serial.print("posCorrection="); Serial.print(_filterdelay_correction); 
-
-            Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before=");            
-          
-
-          }else if((blind1.speedSimulated>0 && blind1.sense_status==SENSESTATUS_CLEAR) || (blind1.speedSimulated<0 && blind1.sense_status==SENSESTATUS_OPAQUE)) { //the other transition
-            //estimated_position_difference = -blind1.position + blind1.start_first_clear + _n*(blind1.length_clear+blind1.length_opaque); //possible occurances. let estimated_position=0 , solve for n
-            
-            int _n = round(( blind1.position - blind1.start_first_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest n
-            Serial.print("n=");
-             Serial.println(_n);
-            blind1.position = blind1.start_first_clear + _n*(blind1.length_clear+blind1.length_opaque); //calculate position from _n
-            float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
-            blind1.position += _filterdelay_correction; //correct for filter delay
-
-            Serial.print("posCorrectoin="); Serial.print(_filterdelay_correction); 
-
-            Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before=");        
-
-          }
-
-          Serial.print(_position_before);
-          Serial.print(", after=");
-          Serial.print(blind1.position);
-          Serial.print(", diff=");
-          Serial.println(blind1.position-_position_before);
-        }
-
-        blind1.last_sense_status = blind1.sense_status; //update only if new one is opaque or clear
-      }
-    }
-  }
 
   //Update Motor Driver
-  if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) {
+  updateMotor(blind1, M1);
-    if(blind1.speed<0){
-      M1.setmotor( _UP, abs(blind1.speed));
-    }else if(blind1.speed>0){
-      M1.setmotor( _DOWN, abs(blind1.speed));
-    }else{
-      M1.setmotor(_STOP);
-    }
-    last_motor_send=millis();
-  }
 }
 
 int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
@@ -360,13 +237,190 @@ uint8_t classifySensorValue(blindmodel &blind) {
   } 
 }
 
+void checkButton(button &btn) {
+  btn.changed=false;
+  if (millis() > btn.last_time_read + BUTTON_DEBOUNCE) {
+    bool new_pin_button_down=!digitalRead(btn.pin);
+    if (btn.down != new_pin_button_down) { //changed
+      btn.down = new_pin_button_down; //update
+      btn.changed=true;
+      btn.last_time_read=millis(); //delay next check
+    }    
+  }
+}
+
+void manualMoveHandler(button &btn, blindmodel &blind)
+{
+  if (btn.changed) {
+    blind.mode=MODE_IDLE;
+    if (btn.down) { //changed to pressed
+      if (btn.manual_drive_direction) {
+        //M1.setmotor( _CW, 100);
+        blind.speed=-100;
+        //Serial.print("CW PWM: ");
+      }else{
+        blind.speed=100;
+        //Serial.print("CCW PWM: ");
+      }
+      btn.manual_drive_direction=!btn.manual_drive_direction; //switch direction every new press
+    }else{ //changed to released
+      //Serial.println("Motor STOP");
+      blind.speed=0;
+    }
+  }
+}
+
+void readSensor(blindmodel &blind)
+{
+  if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) {
+    blind.sense_read_pos=(blind.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
+    blind.sense_read[blind.sense_read_pos]=analogRead(PIN_SENSE);
+
+    classifySensorValue(blind); //writes to blindmodel.sense_status
+
+    
+    last_sensor_read=millis();
+  }
+
+  if (millis() > last_print + 500) {
+    Serial.print("SenseStatus=");
+    switch(blind.sense_status){
+      case SENSESTATUS_UNKNOWN:
+        Serial.print("UNK");
+        break;
+      case SENSESTATUS_CLEAR:
+        Serial.print("CLE");
+        break;
+      case SENSESTATUS_OPAQUE:
+        Serial.print("OPA");
+        break;
+      case SENSESTATUS_END:
+        Serial.print("END");
+        break;
+    }
+    Serial.print("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
+    Serial.print(", mode=");
+    switch(blind.mode){
+      case MODE_IDLE:
+        Serial.print("IDL");
+        break;
+      case MODE_FIND_END:
+        Serial.print("FIN");
+        break;
+      case MODE_GO_TO_POS:
+        Serial.print("POS");
+        break;
+      case MODE_MEASURE_SPEED:
+        Serial.print("MSP");
+        break;
+    }
+    Serial.print(", speed=");
+    Serial.print(blind.speed);
+    Serial.print(", speedSim=");
+    Serial.print(blind.speedSimulated);    
+    Serial.print(", pos=");
+    Serial.println(blind.position);
+    last_print=millis();
+  }
+}
+
+void errorCheck(blindmodel &blind) {
+  if (blind.sense_status==SENSESTATUS_END) {
+    if (blind.speed<0) { //stop driving up
+      blind.speed=0;
+    }
+  }
+}
+
+void estimatePosition(blindmodel &blind) {
+  if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) {
+    float _actualTime=(millis()-last_calculate_position_estimate)/1000.0; //in seconds
+    blind.speedSimulated+= constrain(blind.speed - blind.speedSimulated, blind.simulated_acc_dec*_actualTime, blind.simulated_acc_inc*_actualTime);
+    
+    blind.position+= blind.speedSimulated/100.0*blind.speedfactor * _actualTime;
+    last_calculate_position_estimate=millis();
+  }
+
+  if (blind.mode!= MODE_FIND_END) {
+    if (blind.sense_status == SENSESTATUS_END && blind.last_sense_status != SENSESTATUS_END) { //entered end marker
+      blind.position=0;
+      float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
+      blind.position += _filterdelay_correction; //correct for filter delay
+      Serial.print("Reached End marker. set Position="); Serial.println(blind.position);
+    }
+
+    if (blind.sense_status == SENSESTATUS_CLEAR || blind.sense_status == SENSESTATUS_OPAQUE || blind.sense_status == SENSESTATUS_END) { //either opaque or clear (or end, only for last update used)
+      if (blind.sense_status != blind.last_sense_status) { //status changed
+        if (blind.last_sense_status == SENSESTATUS_CLEAR || blind.last_sense_status == SENSESTATUS_OPAQUE) { //only changes from clear to opaque or opaque to clear
+
+          float _position_before=blind.position;//only for text output debugging
+
+          if ((blind.speedSimulated>0 && blind.sense_status==SENSESTATUS_OPAQUE) || (blind.speedSimulated<0 && blind.sense_status==SENSESTATUS_CLEAR)) { //moving down from and clear to opaque OR moving up and from opaque to clear
+          
+            //estimated_position_difference = -blind.position + blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //possible occurances. let estimated_position=0 , solve for n
+            
+            int _n = round((blind.position -blind.start_first_clear - blind.length_clear)/(blind.length_clear+blind.length_opaque)); //find closest n
+            blind.position = blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n
+            float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
+            blind.position += _filterdelay_correction; //correct for filter delay
+            Serial.print("n=");
+             Serial.println(_n);
+
+            Serial.print("posCorrection="); Serial.print(_filterdelay_correction); 
+
+            Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before=");            
+          
+
+          }else if((blind.speedSimulated>0 && blind.sense_status==SENSESTATUS_CLEAR) || (blind.speedSimulated<0 && blind.sense_status==SENSESTATUS_OPAQUE)) { //the other transition
+            //estimated_position_difference = -blind.position + blind.start_first_clear + _n*(blind.length_clear+blind.length_opaque); //possible occurances. let estimated_position=0 , solve for n
+            
+            int _n = round(( blind.position - blind.start_first_clear)/(blind.length_clear+blind.length_opaque)); //find closest n
+            Serial.print("n=");
+             Serial.println(_n);
+            blind.position = blind.start_first_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n
+            float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
+            blind.position += _filterdelay_correction; //correct for filter delay
+
+            Serial.print("posCorrectoin="); Serial.print(_filterdelay_correction); 
+
+            Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before=");        
+
+          }
+
+          Serial.print(_position_before);
+          Serial.print(", after=");
+          Serial.print(blind.position);
+          Serial.print(", diff=");
+          Serial.println(blind.position-_position_before);
+        }
+
+        blind.last_sense_status = blind.sense_status; //update only if new one is opaque or clear
+      }
+    }
+  }
+}
+
+void updateMotor(blindmodel &blind, Motor motor)
+{
+  if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) {
+    if(blind.speed<0){
+      motor.setmotor( _UP, abs(blind.speed));
+    }else if(blind.speed>0){
+      motor.setmotor( _DOWN, abs(blind.speed));
+    }else{
+      motor.setmotor(_STOP);
+    }
+    last_motor_send=millis();
+  }
+}
+
 void checkModes(blindmodel &blind) {
   switch(blind.mode) {
     case MODE_FIND_END:
       switch(blind.mode_find_end_state) {
         case 0: //drive up until end sensed
           blind.speed=-100; //up
-          if (blind1.sense_status==SENSESTATUS_END) {
+          if (blind.sense_status==SENSESTATUS_END) {
             blind.speed=0; //stop. for safety
             blind.mode_find_end_state++;
           }
@@ -426,43 +480,3 @@ void checkModes(blindmodel &blind) {
       break;
   }
 }
-
-/*
-void loop() {
-
-  int pwm;
-
-  for (pwm = 0; pwm <= 100; pwm++)
-  {
-    M1.setmotor( _CW, pwm);
-    M2.setmotor(_CW, pwm);
-    Serial.print("Clockwise PWM: ");
-    Serial.println(pwm);
-    delay(100);
-  }
-
-  Serial.println("Motor STOP");
-  M1.setmotor(_STOP);
-  M2.setmotor( _STOP);
-
-  delay(1000);
-
-  
-  for (pwm = 0; pwm <= 100; pwm++)
-  {
-    M1.setmotor(_CCW, pwm);
-    //delay(1);
-    M2.setmotor(_CCW, pwm);
-    Serial.print("Counterclockwise PWM: ");
-    Serial.println(pwm);
-    delay(100);
-
-  }
-
-  Serial.println("Motor A&B STANDBY");
-  M1.setmotor(_STANDBY);
-  M2.setmotor( _STANDBY);
-  delay(1000);
-
-}
-*/