add second blind. some sensor reading problems

src/main.cpp

@@ -7,11 +7,19 @@
 /*
   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)
+  - 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)
   - homie commands (reset error, etc)
+  - command to activate sensro raw value print over topics
+  - do not allow certain commands if error mode
 */
 
 
+//BIG TODO: fix sensor reading right blind. left blind too
+//right blind kind off delayed to commands??
+
 #define FW_NAME "blindctrl"
 #define FW_VERSION "1.0.0"
 
@@ -237,7 +245,13 @@ void setup() {
   blind1Node.advertise("estimationerror");
   blind1Node.advertise("mode");
   blind1Node.advertise("cmd").settable(blind_l_cmdHandler);
-  //TODO: same for blind2
+  
+  blind2Node.advertise("position").settable(blind_r_positionHandler);  //function inputHandler gets called on new message on topic/input/set
+  blind2Node.advertise("debug");
+  blind2Node.advertise("estimationerror");
+  blind2Node.advertise("mode");
+  blind2Node.advertise("cmd").settable(blind_r_cmdHandler);
+  
 
   Homie.setup();
 
@@ -267,21 +281,21 @@ void loopHandler() {
     switch (sensorreadID) {
       case 0:
         readSensor(blind1,rawsensorvalue, blind1Node);
-        sensorreadID++;
         digitalWrite(blind1.pin_sensor_led,LOW); //turn self off
         digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on
         break;
       case 1:
         readSensor(blind2,rawsensorvalue, blind2Node);
-        sensorreadID++;
         digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
         digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
         break;
       default:
-        sensorreadID=0; //reset
+        sensorreadID=0; //reset, failsafe
         break;
     }
     last_sensor_led_switch=millis();
+    sensorreadID++;
+    sensorreadID=sensorreadID%2;
   }  
 
   static float last_blind1_position=0;
@@ -291,7 +305,6 @@ void loopHandler() {
     last_blind1_position=blind1.position;
     lastsend_blind1_position=millis();
   }
-  //TODO: same for blind2
 
   static uint8_t last_blind1_mode=100;
   if (last_blind1_mode!=blind1.mode)
@@ -300,6 +313,21 @@ void loopHandler() {
     last_blind1_mode=blind1.mode;
   }
 
+  static float last_blind2_position=0;
+  static unsigned long lastsend_blind2_position=0; //time
+  if (last_blind2_position != blind2.position && millis()-lastsend_blind2_position>MINTIMEINTERVAL_POSITION) {
+    blind2Node.setProperty("position").send((String)blind2.position);
+    last_blind2_position=blind2.position;
+    lastsend_blind2_position=millis();
+  }
+
+  static uint8_t last_blind2_mode=100;
+  if (last_blind2_mode!=blind2.mode)
+  {
+    blind2Node.setProperty("mode").send(modeNumToString(blind2.mode));
+    last_blind2_mode=blind2.mode;
+  }
+
   checkModes(blind1, blind1Node);
   checkModes(blind2, blind2Node);
 
@@ -348,7 +376,8 @@ void classifySensorValue(blindmodel &blind) {
   } 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
+  
 }
 
 void checkButton(button &btn) {
@@ -552,7 +581,7 @@ void checkModes(blindmodel &blind, HomieNode &node) {
 
         case 1: //drive down slowly until passed end maker
           blind.speed=10; //down slow
-          if (blind1.sense_status!=SENSESTATUS_END) {
+          if (blind.sense_status!=SENSESTATUS_END) {
             blind.speed=0; //stop
             blind.position=0;
             blind.mode=MODE_IDLE;
@@ -576,20 +605,20 @@ void checkModes(blindmodel &blind, HomieNode &node) {
             break;
           }
           blind.speed=100; //down
-          if (blind1.sense_status==SENSESTATUS_CLEAR) {
+          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 (blind1.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
+          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 (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
+          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=");
@@ -608,20 +637,20 @@ void checkModes(blindmodel &blind, HomieNode &node) {
 
         case 4: //drive further down, start timing at clear
           blind.speed=100; //down
-          if (blind1.sense_status==SENSESTATUS_CLEAR) {
+          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 (blind1.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
+          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 (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
+          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);
@@ -728,9 +757,30 @@ bool blind_r_positionHandler(const HomieRange& range, const String& value) {
     return false; //if range is given but index is not in allowed range
   }
   Homie.getLogger() << "blind_r_positionHandler" << ": " << value << endl;
-  
+
   blind2.set_position=constrain(value.toFloat(), blind2.softlimit_min, blind2.softlimit_max);
   blind2.mode=MODE_GO_TO_POS;
   
   return true;
 }
+bool blind_r_cmdHandler(const HomieRange& range, const String& value) {
+  if (range.isRange) {
+    return false; //if range is given but index is not in allowed range
+  }
+  Homie.getLogger() << "blind_r_cmdHandler" << ": " << value << endl;
+  
+  if (value=="END") 
+  {
+    blind2.mode = MODE_FIND_END;
+    blind2.mode_find_end_state=0; //reset mode find state
+  }else if (value=="SPEEDFACTOR") 
+  {
+    blind2.mode=MODE_MEASURE_SPEED; //measure speed
+  }else{
+    Homie.getLogger() << "command not known" << endl;
+    return false;
+  }
+  blind2Node.setProperty("cmd").send(value); //can be done in main loop
+
+  return true;
+}