diff --git a/src/main.cpp b/src/main.cpp index 996aa0f..e2a1e16 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,7 +8,8 @@ TODO: - implement failure detection (timeouts), - 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) + - manual mover function should work better with iot commands (set position, no errors). implemented but untested + - implement find end mode when only using manual move buttons */ @@ -48,7 +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 MAX_ALLOWED_CLASSCHANGE_LENGTH 130 //[mm] . for error check. after with length traveled without classification change error will be triggered. should be at least max(length_opaque,length_clear) #define PIN_SENSE A0 unsigned long last_print=0; @@ -114,7 +115,7 @@ struct blindmodel float softlimit_min=0; float softlimit_max=1000; //doubled value for folding bilds - unsigned long position_last_classified_ok=0; //last position sensor measured class was ok + float position_last_classchange=0; //last position sensor measured class was ok uint8_t mode=MODE_IDLE; @@ -383,11 +384,17 @@ int getFitered(int* values,uint8_t size) { void classifySensorValue(blindmodel &blind) { int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE); if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) { + if (blind.sense_status==SENSESTATUS_OPAQUE) { //change from opaque to clear + blind.position_last_classchange=blind.position; + Serial.print("classchange to clear. last="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position); + } blind.sense_status=SENSESTATUS_CLEAR; - blind.position_last_classified_ok=blind.position; } else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) { + if (blind.sense_status==SENSESTATUS_CLEAR) { //change from clear to opaque + blind.position_last_classchange=blind.position; + Serial.print("classchange to opaque. last="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position); + } blind.sense_status=SENSESTATUS_OPAQUE; - blind.position_last_classified_ok=blind.position; } else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) { blind.sense_status=SENSESTATUS_END; } //if not in these boundaries, keep last class @@ -466,7 +473,10 @@ void errorCheck(blindmodel &blind, HomieNode &node) { } } - if (abs(blind.position_last_classified_ok-blind.position) > MAX_ALLOWED_CLASSIFY_LENGTH ) { //sensor reading havent been classified for too far + if (abs(blind.position_last_classchange-blind.position) > MAX_ALLOWED_CLASSCHANGE_LENGTH ) { //sensor reading havent been classified for too far + if (blind.error==0) { //only first time + Serial.print("classchange error: last_classchange="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position); + } setError(blind, ERRORCODE_CLASSIFY_LENGTH, node); } @@ -486,6 +496,7 @@ void estimatePosition(blindmodel &blind, HomieNode& node) { //if (blind.mode!= MODE_FIND_END) { if (blind.sense_status == SENSESTATUS_END && blind.last_sense_status != SENSESTATUS_END) { //entered end marker blind.position=0; + blind.position_last_classchange=blind.position; //also set lastchange position 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); @@ -593,6 +604,7 @@ void checkModes(blindmodel &blind, HomieNode &node) { if (blind.sense_status!=SENSESTATUS_END) { blind.speed=0; //stop blind.position=0; + blind.position_last_classchange=blind.position; //also set lastchange position blind.mode=MODE_IDLE; //blind.mode=MODE_MEASURE_SPEED; //next measure speed blind.mode_find_end_state=0; //reset @@ -698,10 +710,9 @@ void checkModes(blindmodel &blind, HomieNode &node) { 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="+errorcodeNumToString(errorcode)); } - Serial.print("ERROR CODE="); Serial.println(errorcode); - node.setProperty("debug").send("Error="+errorcodeNumToString(errorcode)); - blind.mode=MODE_ERROR; } @@ -837,7 +848,7 @@ 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 - + blind.position_last_classchange=blind.position; //otherwise error trips immediately if (blind.error==ERRORCODE_UNDEFINED_POSITION) { blind.error=0; //reset } @@ -860,6 +871,7 @@ bool blind_cmdHandler(blindmodel &blind, HomieNode& node, const String& value) { node.setProperty("cmd").send("stop"); }else if (value=="RESET") //reset from error { + blind.position_last_classchange=blind.position; blind.error=0; node.setProperty("cmd").send("reset"); }else{