change error classify change length

src/main.cpp

@@ -8,7 +8,8 @@
   TODO: 
   - implement failure detection (timeouts),
       - has to stop when driving upwards (or in general) for too long (for example end marker missing)
-  - manual mover function should work better with iot commands (set position, no errors)
+  - manual mover function should work better with iot commands (set position, no errors). implemented but untested
+  - implement find end mode when only using manual move buttons
 */
 
 
@@ -48,7 +49,7 @@ unsigned long last_sensor_read=0;
 
 #define POSITION_SPEEDMEASURE_HIGH 1000.0 //at which position (in mm) speedfactorHigh should be measured (should be at least two encoder segments beyond that left)
 
-#define MAX_ALLOWED_CLASSIFY_LENGTH 20 //[mm] . for error check. after with length traveled without successfull classification error should be triggered
+#define MAX_ALLOWED_CLASSCHANGE_LENGTH 130 //[mm] . for error check. after with length traveled without classification change error will be triggered. should be at least max(length_opaque,length_clear)
 
 #define PIN_SENSE A0
 unsigned long last_print=0;
@@ -114,7 +115,7 @@ struct blindmodel
   float softlimit_min=0;
   float softlimit_max=1000;  //doubled value for folding bilds
 
-  unsigned long position_last_classified_ok=0; //last position sensor measured class was ok
+  float position_last_classchange=0; //last position sensor measured class was ok
 
   uint8_t mode=MODE_IDLE;
 
@@ -383,11 +384,17 @@ int getFitered(int* values,uint8_t size) {
 void classifySensorValue(blindmodel &blind) {
   int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
   if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
+    if (blind.sense_status==SENSESTATUS_OPAQUE) { //change from opaque to clear
+      blind.position_last_classchange=blind.position;
+      Serial.print("classchange to clear. last="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position);
+    }
     blind.sense_status=SENSESTATUS_CLEAR;
-    blind.position_last_classified_ok=blind.position;
   } else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
+    if (blind.sense_status==SENSESTATUS_CLEAR) { //change from clear to opaque
+      blind.position_last_classchange=blind.position;
+      Serial.print("classchange to opaque. last="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position);
+    }
     blind.sense_status=SENSESTATUS_OPAQUE;
-    blind.position_last_classified_ok=blind.position;
   } else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
     blind.sense_status=SENSESTATUS_END;
   } //if not in these boundaries, keep last class
@@ -466,7 +473,10 @@ void errorCheck(blindmodel &blind, HomieNode &node) {
     }
   }
 
-  if (abs(blind.position_last_classified_ok-blind.position) > MAX_ALLOWED_CLASSIFY_LENGTH ) { //sensor reading havent been classified for too far
+  if (abs(blind.position_last_classchange-blind.position) > MAX_ALLOWED_CLASSCHANGE_LENGTH ) { //sensor reading havent been classified for too far
+    if (blind.error==0) { //only first time
+      Serial.print("classchange error: last_classchange="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position);
+    }
     setError(blind, ERRORCODE_CLASSIFY_LENGTH, node);
   }
 
@@ -486,6 +496,7 @@ void estimatePosition(blindmodel &blind, HomieNode& node) {
   //if (blind.mode!= MODE_FIND_END) {
     if (blind.sense_status == SENSESTATUS_END && blind.last_sense_status != SENSESTATUS_END) { //entered end marker
       blind.position=0;
+      blind.position_last_classchange=blind.position; //also set lastchange position
       float _filterdelay_correction=blind.speedSimulated/100.0*positional_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);
@@ -593,6 +604,7 @@ void checkModes(blindmodel &blind, HomieNode &node) {
             if (blind.sense_status!=SENSESTATUS_END) {
               blind.speed=0; //stop
               blind.position=0;
+              blind.position_last_classchange=blind.position; //also set lastchange position
               blind.mode=MODE_IDLE;
               //blind.mode=MODE_MEASURE_SPEED; //next measure speed
               blind.mode_find_end_state=0; //reset
@@ -698,10 +710,9 @@ void checkModes(blindmodel &blind, HomieNode &node) {
 void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node){
   if (blind.error==0) { //only set error if no error appeared before
     blind.error=errorcode;
+    Serial.print("ERROR CODE="); Serial.println(errorcode);
+    node.setProperty("debug").send("Error="+errorcodeNumToString(errorcode));
   }
-  Serial.print("ERROR CODE="); Serial.println(errorcode);
-  node.setProperty("debug").send("Error="+errorcodeNumToString(errorcode));
-  
   blind.mode=MODE_ERROR;
 }
 
@@ -837,7 +848,7 @@ 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
-    
+    blind.position_last_classchange=blind.position; //otherwise error trips immediately 
     if (blind.error==ERRORCODE_UNDEFINED_POSITION) {
       blind.error=0; //reset
     }
@@ -860,6 +871,7 @@ bool blind_cmdHandler(blindmodel &blind, HomieNode& node, const String& value) {
     node.setProperty("cmd").send("stop");
   }else if (value=="RESET")  //reset from error
   {
+    blind.position_last_classchange=blind.position;
     blind.error=0;
     node.setProperty("cmd").send("reset");
   }else{