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:
- 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)
- 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_VERSION "1.0.0"
@ -237,7 +245,13 @@ void setup() {
blind1Node.advertise("estimationerror");
blind1Node.advertise("mode");
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();
@ -267,21 +281,21 @@ void loopHandler() {
switch (sensorreadID) {
case 0:
readSensor(blind1,rawsensorvalue, blind1Node);
sensorreadID++;
digitalWrite(blind1.pin_sensor_led,LOW); //turn self off
digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on
break;
case 1:
readSensor(blind2,rawsensorvalue, blind2Node);
sensorreadID++;
digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
break;
default:
sensorreadID=0; //reset
sensorreadID=0; //reset, failsafe
break;
}
last_sensor_led_switch=millis();
sensorreadID++;
sensorreadID=sensorreadID%2;
}
static float last_blind1_position=0;
@ -291,7 +305,6 @@ void loopHandler() {
last_blind1_position=blind1.position;
lastsend_blind1_position=millis();
}
//TODO: same for blind2
static uint8_t last_blind1_mode=100;
if (last_blind1_mode!=blind1.mode)
@ -300,6 +313,21 @@ void loopHandler() {
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(blind2, blind2Node);
@ -348,7 +376,8 @@ void classifySensorValue(blindmodel &blind) {
} else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
blind.sense_status=SENSESTATUS_END;
blind.last_sense_ok=millis();
}
} //if not in these boundaries, keep last class
}
void checkButton(button &btn) {
@ -552,7 +581,7 @@ void checkModes(blindmodel &blind, HomieNode &node) {
case 1: //drive down slowly until passed end maker
blind.speed=10; //down slow
if (blind1.sense_status!=SENSESTATUS_END) {
if (blind.sense_status!=SENSESTATUS_END) {
blind.speed=0; //stop
blind.position=0;
blind.mode=MODE_IDLE;
@ -576,20 +605,20 @@ void checkModes(blindmodel &blind, HomieNode &node) {
break;
}
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) {
if (blind.sense_status==SENSESTATUS_CLEAR) {
blind.timing_start=millis();
blind.mode_measure_speed_state++;
}
break;
case 1: //on clear section
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++;
}
break;
case 2: //on opaque section
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.speedfactorHigh=blind.speedfactorLow; //use speedfactorLow for a first rough estimate (needed to know when position for speedfactorHigh measure reached)
Serial.print("speedfactorLow=");
@ -608,20 +637,20 @@ void checkModes(blindmodel &blind, HomieNode &node) {
case 4: //drive further down, start timing at clear
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) {
if (blind.sense_status==SENSESTATUS_CLEAR) {
blind.timing_start=millis();
blind.mode_measure_speed_state++;
}
break;
case 5: //on clear section
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++;
}
break;
case 6: //on opaque section
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
Serial.print("speedfactorHigh=");
Serial.print(blind.speedfactorHigh);
@ -728,9 +757,30 @@ bool blind_r_positionHandler(const HomieRange& range, const String& value) {
return false; //if range is given but index is not in allowed range
}
Homie.getLogger() << "blind_r_positionHandler" << ": " << value << endl;
blind2.set_position=constrain(value.toFloat(), blind2.softlimit_min, blind2.softlimit_max);
blind2.mode=MODE_GO_TO_POS;
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;
}