start with position undefined error. do not allow commands if position unknown

src/main.cpp

@@ -11,13 +11,10 @@
       - has to stop when no sensor change detected for some time
       - in classifySensorValue() only allow a certain time of unclassified reading
       - has to stop when driving upwards (or in general) for too long (for example end marker missing)
-  - do not allow certain commands if error mode
   - manual mover function should work better with iot commands (set position, no errors)
 */
 
 
-//right blind kind off delayed to commands?? ////
-
 #define FW_NAME "blindctrl"
 #define FW_VERSION "1.0.0"
 
@@ -151,6 +148,7 @@ unsigned long last_motor_send=0;
 #define DIFF_ERROR_FACTOR 0.6 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed
 #define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction
 #define ERRORCODE_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
+#define ERRORCODE_UNDEFINED_POSITION 3
 
 #define THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm)
 
@@ -168,6 +166,7 @@ void updateMotor(blindmodel &blind, Motor motor);
 void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node);
 String modeNumToString(uint8_t modenum);
 String sensestatusNumToString(uint8_t sensestatusnum);
+String errorcodeNumToString(uint8_t errorcode);
 
 
 bool blind_l_positionHandler(const HomieRange& range, const String& value);
@@ -215,6 +214,7 @@ void setup() {
   blind1.simulated_acc_inc=200;
   blind1.softlimit_min=0;
   blind1.softlimit_max=2500;
+  setError(blind1,ERRORCODE_UNDEFINED_POSITION,blind1Node);
 
   blind2.pin_sensor_led=PIN_SENSE_LED2;
   blind2.length_clear=50;
@@ -232,6 +232,7 @@ void setup() {
   blind2.simulated_acc_inc=200;
   blind2.softlimit_min=0;
   blind2.softlimit_max=2500;
+  setError(blind2,ERRORCODE_UNDEFINED_POSITION,blind2Node);
 
   //Test
   //blind1.mode = MODE_FIND_END;
@@ -565,115 +566,118 @@ void updateMotor(blindmodel &blind, Motor motor)
 }
 
 void checkModes(blindmodel &blind, HomieNode &node) {
-  switch(blind.mode) {
+  if (blind.error==0)  //only if no errors
-    case MODE_FIND_END:
+  {
-      switch(blind.mode_find_end_state) {
+    switch(blind.mode) {
-        case 0: //drive up until end sensed
+      case MODE_FIND_END:
-          blind.speed=-100; //up
+        switch(blind.mode_find_end_state) {
-          if (blind.sense_status==SENSESTATUS_END) {
+          case 0: //drive up until end sensed
-            blind.speed=0; //stop. for safety
+            blind.speed=-100; //up
-            blind.mode_find_end_state++;
+            if (blind.sense_status==SENSESTATUS_END) {
-          }
+              blind.speed=0; //stop. for safety
-          break;
+              blind.mode_find_end_state++;
-
+            }
-        case 1: //drive down slowly until passed end maker
-          blind.speed=25; //down slow
-          if (blind.sense_status!=SENSESTATUS_END) {
-            blind.speed=0; //stop
-            blind.position=0;
-            blind.mode=MODE_IDLE;
-            //blind.mode=MODE_MEASURE_SPEED; //next measure speed
-            blind.mode_find_end_state=0; //reset
-            blind.last_n=-1; //unknown
-          }
-          break;
-      }
-      
-      break;
-
-    case MODE_MEASURE_SPEED:
-      switch(blind.mode_measure_speed_state) {
-        case 0: //drive down, start timing at clear
-          if (blind.position>100) { //cancel
-            node.setProperty("cmd").send("canceled. Position>100: pos="+(String)blind.position);
-            blind.speed=0; //stop
-            blind.mode_measure_speed_state=0; // reset
-            blind.mode=MODE_IDLE;
             break;
-          }
-          blind.speed=100; //down
-          if (blind.sense_status==SENSESTATUS_CLEAR) {
-            blind.timing_start=millis();
-            blind.mode_measure_speed_state++;
-          }
-          break;
-        case 1: //on clear section
-          blind.speed=100; //down
-          if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
-            blind.mode_measure_speed_state++;
-          }
-          break;
-        case 2: //on opaque section
-          blind.speed=100; //down
-          if (blind.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
-            blind.speedfactorLow=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
-            blind.speedfactorHigh=blind.speedfactorLow; //use speedfactorLow for a first rough estimate (needed to know when position for speedfactorHigh measure reached)
-            Serial.print("speedfactorLow=");
-            Serial.print(blind.speedfactorLow);
-            Serial.println(" mm/s");
-            node.setProperty("cmd").send("speedfactorLow="+(String)blind.speedfactorLow);
-            blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position
-            blind.mode_measure_speed_state++;
-          }
-          break;
-        case 3: // past first measurement. drive to lower position for speedfactorHigh
-          if (blind.position>=POSITION_SPEEDMEASURE_HIGH) { //position reached
-            blind.mode_measure_speed_state++;
-          }
-          break;
 
-        case 4: //drive further down, start timing at clear
+          case 1: //drive down slowly until passed end maker
-          blind.speed=100; //down
+            blind.speed=25; //down slow
-          if (blind.sense_status==SENSESTATUS_CLEAR) {
+            if (blind.sense_status!=SENSESTATUS_END) {
-            blind.timing_start=millis();
+              blind.speed=0; //stop
-            blind.mode_measure_speed_state++;
+              blind.position=0;
-          }
+              blind.mode=MODE_IDLE;
-          break;
+              //blind.mode=MODE_MEASURE_SPEED; //next measure speed
-        case 5: //on clear section
+              blind.mode_find_end_state=0; //reset
-          blind.speed=100; //down
+              blind.last_n=-1; //unknown
-          if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
+            }
-            blind.mode_measure_speed_state++;
+            break;
-          }
-          break;
-        case 6: //on opaque section
-          blind.speed=100; //down
-          if (blind.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
-            blind.speedfactorHigh=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
-            Serial.print("speedfactorHigh=");
-            Serial.print(blind.speedfactorHigh);
-            Serial.println(" mm/s");
-            node.setProperty("cmd").send("speedfactorHigh="+(String)blind.speedfactorHigh);
-            blind.speed=0; //stop
-            blind.mode_measure_speed_state=0; // reset
-            blind.mode=MODE_IDLE;
-          }
-          break;          
-      }
-
-      break;
-
-    case MODE_GO_TO_POS:
-      if (abs(blind.position-blind.set_position)<=THRESHOLD_GO_TO_POS) {
-        blind.speed=0; //stop
-        blind.mode=MODE_IDLE;
-      }else{
-        if (blind.set_position<blind.position) {
-          blind.speed=-100; //drive up
-        }else if (blind.set_position>blind.position) {
-          blind.speed=100; //drive down
         }
-      }
+        
-      break;
+        break;
+
+      case MODE_MEASURE_SPEED:
+        switch(blind.mode_measure_speed_state) {
+          case 0: //drive down, start timing at clear
+            if (blind.position>100) { //cancel
+              node.setProperty("cmd").send("canceled. Position>100: pos="+(String)blind.position);
+              blind.speed=0; //stop
+              blind.mode_measure_speed_state=0; // reset
+              blind.mode=MODE_IDLE;
+              break;
+            }
+            blind.speed=100; //down
+            if (blind.sense_status==SENSESTATUS_CLEAR) {
+              blind.timing_start=millis();
+              blind.mode_measure_speed_state++;
+            }
+            break;
+          case 1: //on clear section
+            blind.speed=100; //down
+            if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
+              blind.mode_measure_speed_state++;
+            }
+            break;
+          case 2: //on opaque section
+            blind.speed=100; //down
+            if (blind.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
+              blind.speedfactorLow=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
+              blind.speedfactorHigh=blind.speedfactorLow; //use speedfactorLow for a first rough estimate (needed to know when position for speedfactorHigh measure reached)
+              Serial.print("speedfactorLow=");
+              Serial.print(blind.speedfactorLow);
+              Serial.println(" mm/s");
+              node.setProperty("cmd").send("speedfactorLow="+(String)blind.speedfactorLow);
+              blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position
+              blind.mode_measure_speed_state++;
+            }
+            break;
+          case 3: // past first measurement. drive to lower position for speedfactorHigh
+            if (blind.position>=POSITION_SPEEDMEASURE_HIGH) { //position reached
+              blind.mode_measure_speed_state++;
+            }
+            break;
+
+          case 4: //drive further down, start timing at clear
+            blind.speed=100; //down
+            if (blind.sense_status==SENSESTATUS_CLEAR) {
+              blind.timing_start=millis();
+              blind.mode_measure_speed_state++;
+            }
+            break;
+          case 5: //on clear section
+            blind.speed=100; //down
+            if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
+              blind.mode_measure_speed_state++;
+            }
+            break;
+          case 6: //on opaque section
+            blind.speed=100; //down
+            if (blind.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
+              blind.speedfactorHigh=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
+              Serial.print("speedfactorHigh=");
+              Serial.print(blind.speedfactorHigh);
+              Serial.println(" mm/s");
+              node.setProperty("cmd").send("speedfactorHigh="+(String)blind.speedfactorHigh);
+              blind.speed=0; //stop
+              blind.mode_measure_speed_state=0; // reset
+              blind.mode=MODE_IDLE;
+            }
+            break;          
+        }
+
+        break;
+
+      case MODE_GO_TO_POS:
+        if (abs(blind.position-blind.set_position)<=THRESHOLD_GO_TO_POS) {
+          blind.speed=0; //stop
+          blind.mode=MODE_IDLE;
+        }else{
+          if (blind.set_position<blind.position) {
+            blind.speed=-100; //drive up
+          }else if (blind.set_position>blind.position) {
+            blind.speed=100; //drive down
+          }
+        }
+        break;
+    }
   }
 }
 
@@ -683,7 +687,8 @@ void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node){
     blind.error=errorcode;
   }
   Serial.print("ERROR CODE="); Serial.println(errorcode);
-  node.setProperty("debug").send("Error="+(String)errorcode);
+  node.setProperty("debug").send("Error="+errorcodeNumToString(errorcode));
+  
   blind.mode=MODE_ERROR;
 }
 
@@ -725,6 +730,20 @@ String sensestatusNumToString(uint8_t sensestatusnum){
   }
 }
 
+String errorcodeNumToString(uint8_t errorcode) {
+  switch(errorcode){
+    case ERRORCODE_UNDEFINED_POSITION:
+      return "ERROR_UNDEFINED_POSITION";
+      break;
+    case ERRORCODE_POSITIONDIFFTOOHIGH:
+      return "ERROR_POSITIONDIFFTOOHIGH";
+      break;
+    case ERRORCODE_N_NOT_NEXT:
+      return "ERROR_N_NOT_NEXT";
+      break;
+  }
+}
+
 
 bool blind_l_positionHandler(const HomieRange& range, const String& value) {
   if (range.isRange) {
@@ -800,6 +819,10 @@ bool blind_cmdHandler(blindmodel &blind, HomieNode& node, const String& value) {
   {
     blind.mode = MODE_FIND_END;
     blind.mode_find_end_state=0; //reset mode find state
+    
+    if (blind.error==ERRORCODE_UNDEFINED_POSITION) {
+      blind.error=0; //reset
+    }
     node.setProperty("cmd").send("cmd end");
   }else if (value=="SPEEDFACTOR") 
   {