implement homie

platformio.ini

@@ -13,7 +13,8 @@ platform = espressif8266
 board = d1_mini
 framework = arduino
 
-monitor_speed = 57600
+monitor_speed = 115200
 
 lib_deps =
-    https://github.com/thomasfredericks/wemos_motor_shield
+    https://github.com/thomasfredericks/wemos_motor_shield
+    Homie@3.0.0

src/main.cpp

@@ -1,14 +1,25 @@
 #include <Arduino.h>
 #include <Wire.h>
+#include <Homie.h>
+
+//Upload config: platformio run --target uploadfs
 
 /*
   TODO: 
   - correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state
-  - implement drive to position
-  - implement failure detection
-  - implement homie
+  - implement failure detection (timeouts)
+  - homie commands (find zero, test speed, etc)
 */
 
+
+#define FW_NAME "blindctrl"
+#define FW_VERSION "1.0.0"
+
+
+HomieNode blind1Node("blindl", "Blind Left", "blind"); //paramters: topic, $name, $type
+HomieNode blind2Node("blindr", "Blind Right", "blind"); //paramters: topic, $name, $type
+
+
 #include "WEMOS_Motor.h"
 
 //follow firmware flash guide for new wemos motor shield v1.0  https://github.com/thomasfredericks/wemos_motor_shield
@@ -28,12 +39,13 @@ button button1;
 button button2;
 
 unsigned long last_sensor_read=0;
-#define SENSOR_READ_INTERVAL 20 //in ms
+#define SENSOR_READ_INTERVAL 5 //in ms
 #define SENSE_FILTER_SIZE 20 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL
 
 #define CALCULATEPOSITIONESTIMATE_INTERVAL 100
 unsigned long last_calculate_position_estimate=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 PIN_SENSE A0
@@ -58,6 +70,7 @@ unsigned long last_sensor_led_switch=0;
 #define MODE_FIND_END 1
 #define MODE_MEASURE_SPEED 2
 #define MODE_GO_TO_POS 3
+#define MODE_ERROR 4
 
 //model parameters/variables
 struct blindmodel
@@ -72,7 +85,8 @@ struct blindmodel
   float start_first_clear; //distance from end marker to beginning of first clear section
   //end marker should be on the opaque section. So that a full clear section follows
 
-  float speedfactor=1; //how much position units (mm) per second at pwm=100
+  float speedfactorLow=1; //how much position units (mm) per second at pwm=100. speedfactorLow for position at 0
+  float speedfactorHigh=1; //speedfactorHigh for position at 1000 mm. gets extrapolated above
 
   int sense_clear_lower; //adc value lower limit for clear part. clear is around 70
   int sense_clear_upper; //adc value upper limit for clear part
@@ -92,6 +106,8 @@ struct blindmodel
   uint8_t pin_sensor_led; //pin to activate sensor. high activates sensor
 
   float set_position=0;
+  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. 
@@ -126,16 +142,29 @@ 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 THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm)
+
+#define MINTIMEINTERVAL_POSITION 500 //how fast new position should be send via mqtt when moving
+
 int getFitered(int* values,uint8_t size);
 void classifySensorValue(blindmodel &blind);
 void checkButton(button &btn);
-void checkModes(blindmodel &blind);
+void checkModes(blindmodel &blind, HomieNode &node);
 void manualMoveHandler(button &btn, blindmodel &blind);
-void readSensor(blindmodel &blind, int value);
-void estimatePosition(blindmodel &blind);
+void readSensor(blindmodel &blind, int value, HomieNode &node);
+void estimatePosition(blindmodel &blind, HomieNode& node);
 void errorCheck(blindmodel &blind);
 void updateMotor(blindmodel &blind, Motor motor);
-void setError(blindmodel &blind, uint8_t errorcode);
+void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node);
+String modeNumToString(uint8_t modenum);
+
+
+bool blind_l_positionHandler(const HomieRange& range, const String& value);
+bool blind_r_positionHandler(const HomieRange& range, const String& value);
+bool blind_l_cmdHandler(const HomieRange& range, const String& value);
+bool blind_r_cmdHandler(const HomieRange& range, const String& value);
+
+void loopHandler();
 
 void setup() {
   Serial.begin(115200);
@@ -167,10 +196,13 @@ void setup() {
   blind1.sense_opaque_upper=850;
   blind1.sense_end_lower=850;
   blind1.sense_end_upper=1024;
-  blind1.speedfactor=29;
+  blind1.speedfactorLow=28;
+  blind1.speedfactorHigh=24.5;
   blind1.start_first_clear=27;
   blind1.simulated_acc_dec=-150;
   blind1.simulated_acc_inc=200;
+  blind1.softlimit_min=0;
+  blind1.softlimit_max=2500;
 
   blind2.pin_sensor_led=PIN_SENSE_LED2;
   blind2.length_clear=50;
@@ -181,10 +213,13 @@ void setup() {
   blind2.sense_opaque_upper=850;
   blind2.sense_end_lower=850;
   blind2.sense_end_upper=1024;
-  blind2.speedfactor=29;
+  blind1.speedfactorLow=28;
+  blind1.speedfactorHigh=24.5;
   blind2.start_first_clear=27;
   blind2.simulated_acc_dec=-150;
   blind2.simulated_acc_inc=200;
+  blind2.softlimit_min=0;
+  blind2.softlimit_max=2500;
 
   //Test
   blind1.mode = MODE_FIND_END;
@@ -193,6 +228,18 @@ void setup() {
   //blind2.mode = MODE_FIND_END;
   //blind2.mode_find_end_state=0; //reset mode find state
 
+  Homie_setFirmware(FW_NAME, FW_VERSION);
+  Homie_setBrand(FW_NAME);
+  Homie.setLoopFunction(loopHandler);
+
+  blind1Node.advertise("position").settable(blind_l_positionHandler);  //function inputHandler gets called on new message on topic/input/set
+  blind1Node.advertise("debug");
+  blind1Node.advertise("estimationerror");
+  blind1Node.advertise("mode");
+  blind1Node.advertise("cmd").settable(blind_l_cmdHandler);
+  //TODO: same for blind2
+
+  Homie.setup();
 
   
   
@@ -200,6 +247,9 @@ void setup() {
 
 
 void loop() {
+  Homie.loop();  
+}
+void loopHandler() {
 
   checkButton(button1);
   checkButton(button2);
@@ -216,13 +266,13 @@ void loop() {
     int rawsensorvalue=analogRead(PIN_SENSE);
     switch (sensorreadID) {
       case 0:
-        readSensor(blind1,rawsensorvalue);
+        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);
+        readSensor(blind2,rawsensorvalue, blind2Node);
         sensorreadID++;
         digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
         digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
@@ -234,16 +284,31 @@ void loop() {
     last_sensor_led_switch=millis();
   }  
 
+  static float last_blind1_position=0;
+  static unsigned long lastsend_blind1_position=0; //time
+  if (last_blind1_position != blind1.position && millis()-lastsend_blind1_position>MINTIMEINTERVAL_POSITION) {
+    blind1Node.setProperty("position").send((String)blind1.position);
+    last_blind1_position=blind1.position;
+    lastsend_blind1_position=millis();
+  }
+  //TODO: same for blind2
 
-  checkModes(blind1);
-  checkModes(blind2);
+  static uint8_t last_blind1_mode=100;
+  if (last_blind1_mode!=blind1.mode)
+  {
+    blind1Node.setProperty("mode").send(modeNumToString(blind1.mode));
+    last_blind1_mode=blind1.mode;
+  }
+
+  checkModes(blind1, blind1Node);
+  checkModes(blind2, blind2Node);
 
   errorCheck(blind1);
   errorCheck(blind2);
   
   //Estimate blind position and correct
-  estimatePosition(blind1);
-  estimatePosition(blind2);
+  estimatePosition(blind1, blind1Node);
+  estimatePosition(blind2, blind2Node);
 
   //Update Motor Driver
   updateMotor(blind1, M1);
@@ -319,7 +384,7 @@ void manualMoveHandler(button &btn, blindmodel &blind)
   }
 }
 
-void readSensor(blindmodel &blind, int value)
+void readSensor(blindmodel &blind, int value, HomieNode &node)
 {
   if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) {
     blind.sense_read_pos=(blind.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
@@ -349,20 +414,8 @@ void readSensor(blindmodel &blind, int value)
     }
     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(modeNumToString(blind.mode));
+    
     Serial.print(", speed=");
     Serial.print(blind.speed);
     Serial.print(", speedSim=");
@@ -383,21 +436,23 @@ void errorCheck(blindmodel &blind) {
   //TODO: led self test. turn off ,should be high value
 }
 
-void estimatePosition(blindmodel &blind) {
+void estimatePosition(blindmodel &blind, HomieNode& node) {
+  float positional_speedfactor = blind.speedfactorLow + constrain(blind.position, blind.softlimit_min, blind.softlimit_max)*(blind.speedfactorHigh-blind.speedfactorLow)/POSITION_SPEEDMEASURE_HIGH; //interpolate/extrapolate speedfactor (speed is different when rolled up)
   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;
+    blind.position+= blind.speedSimulated/100.0*positional_speedfactor * _actualTime;
     last_calculate_position_estimate=millis();
   }
 
-  if (blind.mode!= MODE_FIND_END) {
+  //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;
+      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);
+      node.setProperty("debug").send("Reached End marker. set Position="+(String)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)
@@ -412,18 +467,20 @@ void estimatePosition(blindmodel &blind) {
             
             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;
+            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("nearest n="); Serial.println(_n); 
 
-            if (blind.last_n !=-1 && abs(blind.last_n-_n>1)) { //if last_n is known and if n is not the next (error)
-              setError(blind, ERRORCODE_N_NOT_NEXT);
+            if (blind.last_n !=-1 && abs(blind.last_n-_n>1) && blind.mode!= MODE_FIND_END) { //if last_n is known and if n is not the next (error)
+              setError(blind, ERRORCODE_N_NOT_NEXT, node);
             }
             blind.last_n=_n; //for error check
 
             Serial.print("posCorrection="); Serial.print(_filterdelay_correction); 
+            
+            
 
-            Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before=");            
+            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
@@ -433,10 +490,11 @@ void estimatePosition(blindmodel &blind) {
             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;
+            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("posCorrection="); Serial.print(_filterdelay_correction); 
+            
 
             Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before=");        
 
@@ -447,16 +505,20 @@ void estimatePosition(blindmodel &blind) {
           Serial.print(blind.position);
           Serial.print(", diff=");
           Serial.println(blind.position-_position_before);
+          //node.setProperty("debug").send("diff="+(String)(blind.position-_position_before));
+          node.setProperty("estimationerror").send((String)(blind.position-_position_before));
 
-          if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) {
-            setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH);
+          if (blind.mode!= MODE_FIND_END) { //only check error if not in find_end mode
+            if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) {
+              setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH, node);
+            }
           }
         }
 
         blind.last_sense_status = blind.sense_status; //update only if new one is opaque or clear
       }
     }
-  }
+  //}
 }
 
 void updateMotor(blindmodel &blind, Motor motor)
@@ -476,7 +538,7 @@ void updateMotor(blindmodel &blind, Motor motor)
   }
 }
 
-void checkModes(blindmodel &blind) {
+void checkModes(blindmodel &blind, HomieNode &node) {
   switch(blind.mode) {
     case MODE_FIND_END:
       switch(blind.mode_find_end_state) {
@@ -493,8 +555,8 @@ void checkModes(blindmodel &blind) {
           if (blind1.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=MODE_IDLE;
+            //blind.mode=MODE_MEASURE_SPEED; //next measure speed
             blind.mode_find_end_state=0; //reset
             blind.last_n=-1; //unknown
           }
@@ -506,6 +568,13 @@ void checkModes(blindmodel &blind) {
     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 (blind1.sense_status==SENSESTATUS_CLEAR) {
             blind.timing_start=millis();
@@ -521,32 +590,147 @@ void checkModes(blindmodel &blind) {
         case 2: //on opaque section
           blind.speed=100; //down
           if (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
-            blind.speedfactor=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
-            Serial.print("speedfactor=");
-            Serial.print(blind.speedfactor);
+            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 (blind1.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
+            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
+            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;          
       }
 
       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;
   }
 }
 
 
-void setError(blindmodel &blind, uint8_t errorcode){
+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="+(String)errorcode);
+  blind.mode=MODE_ERROR;
+}
+
+String modeNumToString(uint8_t modenum){
+  switch(modenum){
+      case MODE_IDLE:
+        return "MODE_IDLE";
+        break;
+      case MODE_FIND_END:
+        return "MODE_FIND_END";
+        break;
+      case MODE_GO_TO_POS:
+        return "MODE_GO_TO_POS";
+        break;
+      case MODE_MEASURE_SPEED:
+        return "MODE_MEASURE_SPEED";
+        break;
+      case MODE_ERROR:
+        return "MODE_ERROR";
+        break;
+    }
+}
+
+
+bool blind_l_positionHandler(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_l_positionHandler" << ": " << value << endl;
+  //lightNode.setProperty("enable").send(value); //can be done in main loop
+  
+  //if (value.toFloat() >= 0 && value.toFloat() <= 1.0) {
+    //something with value.toFloat()
+    blind1.set_position=constrain(value.toFloat(), blind1.softlimit_min, blind1.softlimit_max);
+    blind1.mode=MODE_GO_TO_POS;
+  /*}else{
+    Homie.getLogger() << "Value outside range" << endl;
+    return false;  
+  }*/
+
+  return true;
+}
+bool blind_l_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_l_cmdHandler" << ": " << value << endl;
+  
+  if (value=="END") 
+  {
+    blind1.mode = MODE_FIND_END;
+    blind1.mode_find_end_state=0; //reset mode find state
+  }else if (value=="SPEEDFACTOR") 
+  {
+    blind1.mode=MODE_MEASURE_SPEED; //measure speed
+  }else{
+    Homie.getLogger() << "command not known" << endl;
+    return false;
+  }
+  blind1Node.setProperty("cmd").send(value); //can be done in main loop
+
+  return true;
+}
+
+
+bool blind_r_positionHandler(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_positionHandler" << ": " << value << endl;
+  
+  blind2.set_position=constrain(value.toFloat(), blind2.softlimit_min, blind2.softlimit_max);
+  blind2.mode=MODE_GO_TO_POS;
+  
+  return true;
+}