From 2ef0cf5b69c6040804ccd8b8be5a0cf2b80b4f6b Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 14 Feb 2021 12:29:24 +0100
Subject: [PATCH] implement errorcheck no sensor reading for too long

---
 src/main.cpp | 29 ++++++++++++++++++-----------
 1 file changed, 18 insertions(+), 11 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index c5c959e..86b4f6f 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,9 +6,7 @@
 
 /*
   TODO: 
-  - correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state
   - implement failure detection (timeouts),
-      - 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)
   - manual mover function should work better with iot commands (set position, no errors)
@@ -51,6 +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 PIN_SENSE A0
 unsigned long last_print=0;
@@ -114,8 +113,7 @@ struct blindmodel
   float softlimit_min=0;
   float softlimit_max=1000;  //doubled value for folding bilds
 
-  unsigned long last_sense_ok=0; //last time sensor measured class ok
-  //TODO: implement timeout if last_sense_ok gets too high. 
+  unsigned long position_last_classified_ok=0; //last position sensor measured class was ok
 
   uint8_t mode=MODE_IDLE;
 
@@ -149,6 +147,7 @@ unsigned long last_motor_send=0;
 #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 ERRORCODE_CLASSIFY_LENGTH 4
 
 #define THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm)
 
@@ -161,7 +160,7 @@ void checkModes(blindmodel &blind, HomieNode &node);
 void manualMoveHandler(button &btn, blindmodel &blind);
 void readSensor(blindmodel &blind, int value, HomieNode &node);
 void estimatePosition(blindmodel &blind, HomieNode& node);
-void errorCheck(blindmodel &blind);
+void errorCheck(blindmodel &blind, HomieNode &node);
 void updateMotor(blindmodel &blind, Motor motor);
 void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node);
 String modeNumToString(uint8_t modenum);
@@ -343,8 +342,8 @@ void loopHandler() {
   checkModes(blind1, blind1Node);
   checkModes(blind2, blind2Node);
 
-  errorCheck(blind1);
-  errorCheck(blind2);
+  errorCheck(blind1, blind1Node);
+  errorCheck(blind2, blind2Node);
   
   //Estimate blind position and correct
   estimatePosition(blind1, blind1Node);
@@ -384,13 +383,12 @@ void classifySensorValue(blindmodel &blind) {
   int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
   if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
     blind.sense_status=SENSESTATUS_CLEAR;
-    blind.last_sense_ok=millis();
+    blind.position_last_classified_ok=blind.position;
   } else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
     blind.sense_status=SENSESTATUS_OPAQUE;
-    blind.last_sense_ok=millis();
+    blind.position_last_classified_ok=blind.position;
   } else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
     blind.sense_status=SENSESTATUS_END;
-    blind.last_sense_ok=millis();
   } //if not in these boundaries, keep last class
   
 }
@@ -454,13 +452,17 @@ void readSensor(blindmodel &blind, int value, HomieNode &node)
   }
 }
 
-void errorCheck(blindmodel &blind) {
+void errorCheck(blindmodel &blind, HomieNode &node) {
   if (blind.sense_status==SENSESTATUS_END) {
     if (blind.speed<0) { //stop driving up
       blind.speed=0;
     }
   }
 
+  if (abs(blind.position_last_classified_ok-blind.position) > MAX_ALLOWED_CLASSIFY_LENGTH ) { //sensor reading havent been classified for too far
+    setError(blind, ERRORCODE_CLASSIFY_LENGTH, node);
+  }
+
   //TODO: led self test. turn off ,should be high value
 }
 
@@ -728,6 +730,7 @@ String sensestatusNumToString(uint8_t sensestatusnum){
       return "END";
       break;
   }
+  return "UNDEF";
 }
 
 String errorcodeNumToString(uint8_t errorcode) {
@@ -741,7 +744,11 @@ String errorcodeNumToString(uint8_t errorcode) {
     case ERRORCODE_N_NOT_NEXT:
       return "ERROR_N_NOT_NEXT";
       break;
+    case ERRORCODE_CLASSIFY_LENGTH:
+      return "ERROR_CLASSIFY_LENGTH";
+      break;
   }
+  return "no error";
 }