diff --git a/src/main.cpp b/src/main.cpp index 205857c..c5c959e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,13 +11,10 @@ - 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) - - do not allow certain commands if error mode - manual mover function should work better with iot commands (set position, no errors) */ -//right blind kind off delayed to commands?? //// - #define FW_NAME "blindctrl" #define FW_VERSION "1.0.0" @@ -151,6 +148,7 @@ unsigned long last_motor_send=0; #define DIFF_ERROR_FACTOR 0.6 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed #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 THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm) @@ -168,6 +166,7 @@ void updateMotor(blindmodel &blind, Motor motor); void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node); String modeNumToString(uint8_t modenum); String sensestatusNumToString(uint8_t sensestatusnum); +String errorcodeNumToString(uint8_t errorcode); bool blind_l_positionHandler(const HomieRange& range, const String& value); @@ -215,6 +214,7 @@ void setup() { blind1.simulated_acc_inc=200; blind1.softlimit_min=0; blind1.softlimit_max=2500; + setError(blind1,ERRORCODE_UNDEFINED_POSITION,blind1Node); blind2.pin_sensor_led=PIN_SENSE_LED2; blind2.length_clear=50; @@ -232,6 +232,7 @@ void setup() { blind2.simulated_acc_inc=200; blind2.softlimit_min=0; blind2.softlimit_max=2500; + setError(blind2,ERRORCODE_UNDEFINED_POSITION,blind2Node); //Test //blind1.mode = MODE_FIND_END; @@ -565,115 +566,118 @@ void updateMotor(blindmodel &blind, Motor motor) } void checkModes(blindmodel &blind, HomieNode &node) { - switch(blind.mode) { - case MODE_FIND_END: - switch(blind.mode_find_end_state) { - case 0: //drive up until end sensed - blind.speed=-100; //up - if (blind.sense_status==SENSESTATUS_END) { - blind.speed=0; //stop. for safety - blind.mode_find_end_state++; - } - break; - - case 1: //drive down slowly until passed end maker - blind.speed=25; //down slow - if (blind.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_find_end_state=0; //reset - blind.last_n=-1; //unknown - } - break; - } - - break; - - 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; + if (blind.error==0) //only if no errors + { + switch(blind.mode) { + case MODE_FIND_END: + switch(blind.mode_find_end_state) { + case 0: //drive up until end sensed + blind.speed=-100; //up + if (blind.sense_status==SENSESTATUS_END) { + blind.speed=0; //stop. for safety + blind.mode_find_end_state++; + } break; - } - blind.speed=100; //down - 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 (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 (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="); - 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 (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 (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 (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); - 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; - - 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 + case 1: //drive down slowly until passed end maker + blind.speed=25; //down slow + if (blind.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_find_end_state=0; //reset + blind.last_n=-1; //unknown + } + break; } - } - break; + + break; + + 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 (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 (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 (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="); + 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 (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 (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 (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); + 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; + + 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; + } } } @@ -683,7 +687,8 @@ void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node){ blind.error=errorcode; } Serial.print("ERROR CODE="); Serial.println(errorcode); - node.setProperty("debug").send("Error="+(String)errorcode); + node.setProperty("debug").send("Error="+errorcodeNumToString(errorcode)); + blind.mode=MODE_ERROR; } @@ -725,6 +730,20 @@ String sensestatusNumToString(uint8_t sensestatusnum){ } } +String errorcodeNumToString(uint8_t errorcode) { + switch(errorcode){ + case ERRORCODE_UNDEFINED_POSITION: + return "ERROR_UNDEFINED_POSITION"; + break; + case ERRORCODE_POSITIONDIFFTOOHIGH: + return "ERROR_POSITIONDIFFTOOHIGH"; + break; + case ERRORCODE_N_NOT_NEXT: + return "ERROR_N_NOT_NEXT"; + break; + } +} + bool blind_l_positionHandler(const HomieRange& range, const String& value) { if (range.isRange) { @@ -800,6 +819,10 @@ 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 + + if (blind.error==ERRORCODE_UNDEFINED_POSITION) { + blind.error=0; //reset + } node.setProperty("cmd").send("cmd end"); }else if (value=="SPEEDFACTOR") {