implement errorcheck no sensor reading for too long

This commit is contained in:
interfisch 2021-02-14 12:29:24 +01:00
parent fab73616cb
commit 2ef0cf5b69
1 changed files with 18 additions and 11 deletions

View File

@ -6,9 +6,7 @@
/* /*
TODO: 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 - 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) - 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)
@ -51,6 +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 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 PIN_SENSE A0 #define PIN_SENSE A0
unsigned long last_print=0; unsigned long last_print=0;
@ -114,8 +113,7 @@ struct blindmodel
float softlimit_min=0; float softlimit_min=0;
float softlimit_max=1000; //doubled value for folding bilds float softlimit_max=1000; //doubled value for folding bilds
unsigned long last_sense_ok=0; //last time sensor measured class ok unsigned long position_last_classified_ok=0; //last position sensor measured class was ok
//TODO: implement timeout if last_sense_ok gets too high.
uint8_t mode=MODE_IDLE; uint8_t mode=MODE_IDLE;
@ -149,6 +147,7 @@ unsigned long last_motor_send=0;
#define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction #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_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
#define ERRORCODE_UNDEFINED_POSITION 3 #define ERRORCODE_UNDEFINED_POSITION 3
#define ERRORCODE_CLASSIFY_LENGTH 4
#define THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm) #define THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm)
@ -161,7 +160,7 @@ void checkModes(blindmodel &blind, HomieNode &node);
void manualMoveHandler(button &btn, blindmodel &blind); void manualMoveHandler(button &btn, blindmodel &blind);
void readSensor(blindmodel &blind, int value, HomieNode &node); void readSensor(blindmodel &blind, int value, HomieNode &node);
void estimatePosition(blindmodel &blind, HomieNode& node); void estimatePosition(blindmodel &blind, HomieNode& node);
void errorCheck(blindmodel &blind); void errorCheck(blindmodel &blind, HomieNode &node);
void updateMotor(blindmodel &blind, Motor motor); void updateMotor(blindmodel &blind, Motor motor);
void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node); void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node);
String modeNumToString(uint8_t modenum); String modeNumToString(uint8_t modenum);
@ -343,8 +342,8 @@ void loopHandler() {
checkModes(blind1, blind1Node); checkModes(blind1, blind1Node);
checkModes(blind2, blind2Node); checkModes(blind2, blind2Node);
errorCheck(blind1); errorCheck(blind1, blind1Node);
errorCheck(blind2); errorCheck(blind2, blind2Node);
//Estimate blind position and correct //Estimate blind position and correct
estimatePosition(blind1, blind1Node); estimatePosition(blind1, blind1Node);
@ -384,13 +383,12 @@ void classifySensorValue(blindmodel &blind) {
int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE); int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) { if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
blind.sense_status=SENSESTATUS_CLEAR; blind.sense_status=SENSESTATUS_CLEAR;
blind.last_sense_ok=millis(); blind.position_last_classified_ok=blind.position;
} else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) { } else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
blind.sense_status=SENSESTATUS_OPAQUE; blind.sense_status=SENSESTATUS_OPAQUE;
blind.last_sense_ok=millis(); blind.position_last_classified_ok=blind.position;
} else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) { } else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
blind.sense_status=SENSESTATUS_END; blind.sense_status=SENSESTATUS_END;
blind.last_sense_ok=millis();
} //if not in these boundaries, keep last class } //if not in these boundaries, keep last class
} }
@ -454,13 +452,17 @@ void readSensor(blindmodel &blind, int value, HomieNode &node)
} }
} }
void errorCheck(blindmodel &blind) { void errorCheck(blindmodel &blind, HomieNode &node) {
if (blind.sense_status==SENSESTATUS_END) { if (blind.sense_status==SENSESTATUS_END) {
if (blind.speed<0) { //stop driving up if (blind.speed<0) { //stop driving up
blind.speed=0; blind.speed=0;
} }
} }
if (abs(blind.position_last_classified_ok-blind.position) > MAX_ALLOWED_CLASSIFY_LENGTH ) { //sensor reading havent been classified for too far
setError(blind, ERRORCODE_CLASSIFY_LENGTH, node);
}
//TODO: led self test. turn off ,should be high value //TODO: led self test. turn off ,should be high value
} }
@ -728,6 +730,7 @@ String sensestatusNumToString(uint8_t sensestatusnum){
return "END"; return "END";
break; break;
} }
return "UNDEF";
} }
String errorcodeNumToString(uint8_t errorcode) { String errorcodeNumToString(uint8_t errorcode) {
@ -741,7 +744,11 @@ String errorcodeNumToString(uint8_t errorcode) {
case ERRORCODE_N_NOT_NEXT: case ERRORCODE_N_NOT_NEXT:
return "ERROR_N_NOT_NEXT"; return "ERROR_N_NOT_NEXT";
break; break;
case ERRORCODE_CLASSIFY_LENGTH:
return "ERROR_CLASSIFY_LENGTH";
break;
} }
return "no error";
} }