interfisch
/
blinds_motorcontrol
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

243
src/main.cpp
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,115 +566,118 @@ void updateMotor(blindmodel &blind, Motor motor)
}
void checkModes(blindmodel &blind, HomieNode &node) {
switch(blind.mode) {
case MODE_FIND_END:
switch(blind.mode_find_end_state) {
case 0: //drive up until end sensed
blind.speed=-100; //up
if (blind.sense_status==SENSESTATUS_END) {
blind.speed=0; //stop. for safety
blind.mode_find_end_state++;
}
break;
case 1: //drive down slowly until passed end maker
blind.speed=25; //down slow
if (blind.sense_status!=SENSESTATUS_END) {
blind.speed=0; //stop
blind.position=0;
blind.mode=MODE_IDLE;
//blind.mode=MODE_MEASURE_SPEED; //next measure speed
blind.mode_find_end_state=0; //reset
blind.last_n=-1; //unknown
}
break;
}
break;
case MODE_MEASURE_SPEED:
switch(blind.mode_measure_speed_state) {
case 0: //drive down, start timing at clear
if (blind.position>100) { //cancel
node.setProperty("cmd").send("canceled. Position>100: pos="+(String)blind.position);
blind.speed=0; //stop
blind.mode_measure_speed_state=0; // reset
blind.mode=MODE_IDLE;
if (blind.error==0) //only if no errors
{
switch(blind.mode) {
case MODE_FIND_END:
switch(blind.mode_find_end_state) {
case 0: //drive up until end sensed
blind.speed=-100; //up
if (blind.sense_status==SENSESTATUS_END) {
blind.speed=0; //stop. for safety
blind.mode_find_end_state++;
}
break;
}
blind.speed=100; //down
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 (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 (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=");
Serial.print(blind.speedfactorLow);
Serial.println(" mm/s");
node.setProperty("cmd").send("speedfactorLow="+(String)blind.speedfactorLow);
blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position
blind.mode_measure_speed_state++;
}
break;
case 3: // past first measurement. drive to lower position for speedfactorHigh
if (blind.position>=POSITION_SPEEDMEASURE_HIGH) { //position reached
blind.mode_measure_speed_state++;
}
break;
case 4: //drive further down, start timing at clear
blind.speed=100; //down
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 (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 (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);
Serial.println(" mm/s");
node.setProperty("cmd").send("speedfactorHigh="+(String)blind.speedfactorHigh);
blind.speed=0; //stop
blind.mode_measure_speed_state=0; // reset
blind.mode=MODE_IDLE;
}
break;
}
break;
case MODE_GO_TO_POS:
if (abs(blind.position-blind.set_position)<=THRESHOLD_GO_TO_POS) {
blind.speed=0; //stop
blind.mode=MODE_IDLE;
}else{
if (blind.set_position<blind.position) {
blind.speed=-100; //drive up
}else if (blind.set_position>blind.position) {
blind.speed=100; //drive down
case 1: //drive down slowly until passed end maker
blind.speed=25; //down slow
if (blind.sense_status!=SENSESTATUS_END) {
blind.speed=0; //stop
blind.position=0;
blind.mode=MODE_IDLE;
//blind.mode=MODE_MEASURE_SPEED; //next measure speed
blind.mode_find_end_state=0; //reset
blind.last_n=-1; //unknown
}
break;
}
}
break;
break;
case MODE_MEASURE_SPEED:
switch(blind.mode_measure_speed_state) {
case 0: //drive down, start timing at clear
if (blind.position>100) { //cancel
node.setProperty("cmd").send("canceled. Position>100: pos="+(String)blind.position);
blind.speed=0; //stop
blind.mode_measure_speed_state=0; // reset
blind.mode=MODE_IDLE;
break;
}
blind.speed=100; //down
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 (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 (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=");
Serial.print(blind.speedfactorLow);
Serial.println(" mm/s");
node.setProperty("cmd").send("speedfactorLow="+(String)blind.speedfactorLow);
blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position
blind.mode_measure_speed_state++;
}
break;
case 3: // past first measurement. drive to lower position for speedfactorHigh
if (blind.position>=POSITION_SPEEDMEASURE_HIGH) { //position reached
blind.mode_measure_speed_state++;
}
break;
case 4: //drive further down, start timing at clear
blind.speed=100; //down
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 (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 (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);
Serial.println(" mm/s");
node.setProperty("cmd").send("speedfactorHigh="+(String)blind.speedfactorHigh);
blind.speed=0; //stop
blind.mode_measure_speed_state=0; // reset
blind.mode=MODE_IDLE;
}
break;
}
break;
case MODE_GO_TO_POS:
if (abs(blind.position-blind.set_position)<=THRESHOLD_GO_TO_POS) {
blind.speed=0; //stop
blind.mode=MODE_IDLE;
}else{
if (blind.set_position<blind.position) {
blind.speed=-100; //drive up
}else if (blind.set_position>blind.position) {
blind.speed=100; //drive down
}
}
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")
{