diff --git a/platformio.ini b/platformio.ini index 427fdcb..f409bd2 100644 --- a/platformio.ini +++ b/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 \ No newline at end of file + https://github.com/thomasfredericks/wemos_motor_shield + Homie@3.0.0 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index cbf536a..ce518ee 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,14 +1,25 @@ #include #include +#include + +//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_positionblind.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); -} \ No newline at end of file + 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; +}