change error classify change length

This commit is contained in:
interfisch 2021-02-14 13:24:27 +01:00
parent a6e668f263
commit 5463f1bb25
1 changed files with 22 additions and 10 deletions

View File

@ -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{