start with position undefined error. do not allow commands if position unknown

This commit is contained in:
interfisch 2021-02-14 12:17:58 +01:00
parent e3c918edb8
commit fab73616cb
1 changed files with 133 additions and 110 deletions

View File

@ -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,6 +566,8 @@ void updateMotor(blindmodel &blind, Motor motor)
}
void checkModes(blindmodel &blind, HomieNode &node) {
if (blind.error==0) //only if no errors
{
switch(blind.mode) {
case MODE_FIND_END:
switch(blind.mode_find_end_state) {
@ -675,6 +678,7 @@ void checkModes(blindmodel &blind, HomieNode &node) {
}
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")
{