add second blind. some sensor reading problems

This commit is contained in:
interfisch 2021-02-12 22:16:09 +01:00
parent 0b2e8ba4c0
commit 34c7134239
1 changed files with 65 additions and 15 deletions

View File

@ -7,11 +7,19 @@
/* /*
TODO: TODO:
- correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state - 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
- has to stop when driving upwards (or in general) for too long (for example end marker missing)
- homie commands (reset error, etc) - homie commands (reset error, etc)
- command to activate sensro raw value print over topics
- do not allow certain commands if error mode
*/ */
//BIG TODO: fix sensor reading right blind. left blind too
//right blind kind off delayed to commands??
#define FW_NAME "blindctrl" #define FW_NAME "blindctrl"
#define FW_VERSION "1.0.0" #define FW_VERSION "1.0.0"
@ -237,7 +245,13 @@ void setup() {
blind1Node.advertise("estimationerror"); blind1Node.advertise("estimationerror");
blind1Node.advertise("mode"); blind1Node.advertise("mode");
blind1Node.advertise("cmd").settable(blind_l_cmdHandler); blind1Node.advertise("cmd").settable(blind_l_cmdHandler);
//TODO: same for blind2
blind2Node.advertise("position").settable(blind_r_positionHandler); //function inputHandler gets called on new message on topic/input/set
blind2Node.advertise("debug");
blind2Node.advertise("estimationerror");
blind2Node.advertise("mode");
blind2Node.advertise("cmd").settable(blind_r_cmdHandler);
Homie.setup(); Homie.setup();
@ -267,21 +281,21 @@ void loopHandler() {
switch (sensorreadID) { switch (sensorreadID) {
case 0: case 0:
readSensor(blind1,rawsensorvalue, blind1Node); readSensor(blind1,rawsensorvalue, blind1Node);
sensorreadID++;
digitalWrite(blind1.pin_sensor_led,LOW); //turn self off digitalWrite(blind1.pin_sensor_led,LOW); //turn self off
digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on
break; break;
case 1: case 1:
readSensor(blind2,rawsensorvalue, blind2Node); readSensor(blind2,rawsensorvalue, blind2Node);
sensorreadID++;
digitalWrite(blind2.pin_sensor_led,LOW); //turn self off digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
break; break;
default: default:
sensorreadID=0; //reset sensorreadID=0; //reset, failsafe
break; break;
} }
last_sensor_led_switch=millis(); last_sensor_led_switch=millis();
sensorreadID++;
sensorreadID=sensorreadID%2;
} }
static float last_blind1_position=0; static float last_blind1_position=0;
@ -291,7 +305,6 @@ void loopHandler() {
last_blind1_position=blind1.position; last_blind1_position=blind1.position;
lastsend_blind1_position=millis(); lastsend_blind1_position=millis();
} }
//TODO: same for blind2
static uint8_t last_blind1_mode=100; static uint8_t last_blind1_mode=100;
if (last_blind1_mode!=blind1.mode) if (last_blind1_mode!=blind1.mode)
@ -300,6 +313,21 @@ void loopHandler() {
last_blind1_mode=blind1.mode; last_blind1_mode=blind1.mode;
} }
static float last_blind2_position=0;
static unsigned long lastsend_blind2_position=0; //time
if (last_blind2_position != blind2.position && millis()-lastsend_blind2_position>MINTIMEINTERVAL_POSITION) {
blind2Node.setProperty("position").send((String)blind2.position);
last_blind2_position=blind2.position;
lastsend_blind2_position=millis();
}
static uint8_t last_blind2_mode=100;
if (last_blind2_mode!=blind2.mode)
{
blind2Node.setProperty("mode").send(modeNumToString(blind2.mode));
last_blind2_mode=blind2.mode;
}
checkModes(blind1, blind1Node); checkModes(blind1, blind1Node);
checkModes(blind2, blind2Node); checkModes(blind2, blind2Node);
@ -348,7 +376,8 @@ void classifySensorValue(blindmodel &blind) {
} 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(); blind.last_sense_ok=millis();
} } //if not in these boundaries, keep last class
} }
void checkButton(button &btn) { void checkButton(button &btn) {
@ -552,7 +581,7 @@ void checkModes(blindmodel &blind, HomieNode &node) {
case 1: //drive down slowly until passed end maker case 1: //drive down slowly until passed end maker
blind.speed=10; //down slow blind.speed=10; //down slow
if (blind1.sense_status!=SENSESTATUS_END) { if (blind.sense_status!=SENSESTATUS_END) {
blind.speed=0; //stop blind.speed=0; //stop
blind.position=0; blind.position=0;
blind.mode=MODE_IDLE; blind.mode=MODE_IDLE;
@ -576,20 +605,20 @@ void checkModes(blindmodel &blind, HomieNode &node) {
break; break;
} }
blind.speed=100; //down blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) { if (blind.sense_status==SENSESTATUS_CLEAR) {
blind.timing_start=millis(); blind.timing_start=millis();
blind.mode_measure_speed_state++; blind.mode_measure_speed_state++;
} }
break; break;
case 1: //on clear section case 1: //on clear section
blind.speed=100; //down blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
blind.mode_measure_speed_state++; blind.mode_measure_speed_state++;
} }
break; break;
case 2: //on opaque section case 2: //on opaque section
blind.speed=100; //down blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section 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.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) 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("speedfactorLow=");
@ -608,20 +637,20 @@ void checkModes(blindmodel &blind, HomieNode &node) {
case 4: //drive further down, start timing at clear case 4: //drive further down, start timing at clear
blind.speed=100; //down blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) { if (blind.sense_status==SENSESTATUS_CLEAR) {
blind.timing_start=millis(); blind.timing_start=millis();
blind.mode_measure_speed_state++; blind.mode_measure_speed_state++;
} }
break; break;
case 5: //on clear section case 5: //on clear section
blind.speed=100; //down blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
blind.mode_measure_speed_state++; blind.mode_measure_speed_state++;
} }
break; break;
case 6: //on opaque section case 6: //on opaque section
blind.speed=100; //down blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section 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 blind.speedfactorHigh=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
Serial.print("speedfactorHigh="); Serial.print("speedfactorHigh=");
Serial.print(blind.speedfactorHigh); Serial.print(blind.speedfactorHigh);
@ -734,3 +763,24 @@ bool blind_r_positionHandler(const HomieRange& range, const String& value) {
return true; return true;
} }
bool blind_r_cmdHandler(const HomieRange& range, const String& value) {
if (range.isRange) {
return false; //if range is given but index is not in allowed range
}
Homie.getLogger() << "blind_r_cmdHandler" << ": " << value << endl;
if (value=="END")
{
blind2.mode = MODE_FIND_END;
blind2.mode_find_end_state=0; //reset mode find state
}else if (value=="SPEEDFACTOR")
{
blind2.mode=MODE_MEASURE_SPEED; //measure speed
}else{
Homie.getLogger() << "command not known" << endl;
return false;
}
blind2Node.setProperty("cmd").send(value); //can be done in main loop
return true;
}