diff --git a/src/main.cpp b/src/main.cpp index 34f7d37..205857c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,14 +11,12 @@ - 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 + - manual mover function should work better with iot commands (set position, no errors) */ -//BIG TODO: fix sensor reading right blind. left blind too -//right blind kind off delayed to commands?? +//right blind kind off delayed to commands?? //// #define FW_NAME "blindctrl" #define FW_VERSION "1.0.0" @@ -47,11 +45,12 @@ button button1; button button2; unsigned long last_sensor_read=0; -#define SENSOR_READ_INTERVAL 5 //in ms -#define SENSE_FILTER_SIZE 20 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL +#define SENSOR_READ_INTERVAL 50 //in ms + +#define SENSE_FILTER_SIZE 10 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL #define CALCULATEPOSITIONESTIMATE_INTERVAL 100 -unsigned long last_calculate_position_estimate=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) @@ -62,8 +61,9 @@ unsigned long last_print=0; #define PIN_SENSE_LED1 D7 #define PIN_SENSE_LED2 D8 uint8_t sensorreadID=0; -unsigned long last_sensor_led_switch=0; -#define SENSOR_SWITCH_INTERVAL 20 //should be lower or equal than SENSOR_READ_INTERVAL + +//#define SENSOR_SWITCH_INTERVAL 20 //should be lower or equal than SENSOR_READ_INTERVAL +//unsigned long last_sensor_led_switch=0; //define directions for motors #define _UP _CCW @@ -131,6 +131,8 @@ struct blindmodel float simulated_acc_dec=-100; //pwm/sec^2, speed getting more negative (accelerating up). this value should be negative. better choose too small absolute values float simulated_acc_inc=100; //pwm/sec^2, speed getting more positive (accelerating down) + unsigned long last_calculate_position_estimate=0; + uint8_t error=0; }; @@ -146,7 +148,7 @@ Motor M2(0x30, _MOTOR_B, 1000); //Motor B unsigned long last_motor_send=0; #define MOTOR_UPDATE_INTERVAL 100 -#define DIFF_ERROR_FACTOR 0.4 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed +#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? @@ -165,12 +167,14 @@ void errorCheck(blindmodel &blind); 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); bool blind_l_positionHandler(const HomieRange& range, const String& value); bool blind_r_positionHandler(const HomieRange& range, const String& value); bool blind_l_cmdHandler(const HomieRange& range, const String& value); bool blind_r_cmdHandler(const HomieRange& range, const String& value); +bool blind_cmdHandler(blindmodel &blind, HomieNode& node, const String& value); //referenced by blind_l and r _cmdHandler void loopHandler(); @@ -199,13 +203,13 @@ void setup() { blind1.length_clear=50; blind1.length_opaque=74; blind1.sense_clear_lower=40; - blind1.sense_clear_upper=100; - blind1.sense_opaque_lower=150; + blind1.sense_clear_upper=200; + blind1.sense_opaque_lower=400; blind1.sense_opaque_upper=800; blind1.sense_end_lower=850; blind1.sense_end_upper=1024; - blind1.speedfactorLow=29; - blind1.speedfactorHigh=24.5; + blind1.speedfactorLow=28.7; + blind1.speedfactorHigh=25.3; blind1.start_first_clear=27; blind1.simulated_acc_dec=-150; blind1.simulated_acc_inc=200; @@ -216,13 +220,13 @@ void setup() { blind2.length_clear=50; blind2.length_opaque=74; blind2.sense_clear_lower=40; - blind2.sense_clear_upper=100; - blind2.sense_opaque_lower=150; + blind2.sense_clear_upper=200; + blind2.sense_opaque_lower=400; blind2.sense_opaque_upper=800; blind2.sense_end_lower=850; blind2.sense_end_upper=1024; - blind2.speedfactorLow=28; - blind2.speedfactorHigh=24.5; + blind2.speedfactorLow=27.6; + blind2.speedfactorHigh=23.5; blind2.start_first_clear=27; blind2.simulated_acc_dec=-150; blind2.simulated_acc_inc=200; @@ -276,15 +280,22 @@ void loopHandler() { //Read sensor/encoder - if (millis() > last_sensor_led_switch + SENSOR_SWITCH_INTERVAL) { + if (millis() > last_sensor_read + SENSOR_READ_INTERVAL/2) { int rawsensorvalue=analogRead(PIN_SENSE); + switch (sensorreadID) { case 0: + if (millis() > last_print + 500 && blind1.speedSimulated!=0) { //when readSensor would print its status + Serial.print("1: "); + } readSensor(blind1,rawsensorvalue, blind1Node); digitalWrite(blind1.pin_sensor_led,LOW); //turn self off digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on break; case 1: + if (millis() > last_print + 500 && blind2.speedSimulated!=0) { //when readSensor would print its status + Serial.print("2: "); + } readSensor(blind2,rawsensorvalue, blind2Node); digitalWrite(blind2.pin_sensor_led,LOW); //turn self off digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on @@ -293,7 +304,7 @@ void loopHandler() { sensorreadID=0; //reset, failsafe break; } - last_sensor_led_switch=millis(); + last_sensor_read=millis(); sensorreadID++; sensorreadID=sensorreadID%2; } @@ -339,8 +350,11 @@ void loopHandler() { estimatePosition(blind2, blind2Node); //Update Motor Driver - updateMotor(blind1, M1); - updateMotor(blind2, M2); + if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) { + updateMotor(blind1, M1); + updateMotor(blind2, M2); + last_motor_send=millis(); + } } int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort @@ -415,32 +429,16 @@ void manualMoveHandler(button &btn, blindmodel &blind) void readSensor(blindmodel &blind, int value, HomieNode &node) { - if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) { - blind.sense_read_pos=(blind.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element - blind.sense_read[blind.sense_read_pos]=value; + blind.sense_read_pos=(blind.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element + blind.sense_read[blind.sense_read_pos]=value; - classifySensorValue(blind); //writes to blindmodel.sense_status + classifySensorValue(blind); //writes to blindmodel.sense_status - - last_sensor_read=millis(); - } if (millis() > last_print + 500 && blind.speedSimulated!=0) { Serial.print("SenseStatus="); - switch(blind.sense_status){ - case SENSESTATUS_UNKNOWN: - Serial.print("UNK"); - break; - case SENSESTATUS_CLEAR: - Serial.print("CLE"); - break; - case SENSESTATUS_OPAQUE: - Serial.print("OPA"); - break; - case SENSESTATUS_END: - Serial.print("END"); - break; - } + Serial.print(sensestatusNumToString(blind.sense_status)); + Serial.print("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")"); Serial.print(", mode="); Serial.print(modeNumToString(blind.mode)); @@ -467,12 +465,12 @@ void errorCheck(blindmodel &blind) { void estimatePosition(blindmodel &blind, HomieNode& node) { float positional_speedfactor = blind.speedfactorLow + constrain(blind.position, blind.softlimit_min, blind.softlimit_max)*(blind.speedfactorHigh-blind.speedfactorLow)/POSITION_SPEEDMEASURE_HIGH; //interpolate/extrapolate speedfactor (speed is different when rolled up) - if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) { - float _actualTime=(millis()-last_calculate_position_estimate)/1000.0; //in seconds + if (millis() > blind.last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) { + float _actualTime=(millis()-blind.last_calculate_position_estimate)/1000.0; //in seconds blind.speedSimulated+= constrain(blind.speed - blind.speedSimulated, blind.simulated_acc_dec*_actualTime, blind.simulated_acc_inc*_actualTime); blind.position+= blind.speedSimulated/100.0*positional_speedfactor * _actualTime; - last_calculate_position_estimate=millis(); + blind.last_calculate_position_estimate=millis(); } //if (blind.mode!= MODE_FIND_END) { @@ -535,9 +533,9 @@ void estimatePosition(blindmodel &blind, HomieNode& node) { Serial.print(", diff="); Serial.println(blind.position-_position_before); //node.setProperty("debug").send("diff="+(String)(blind.position-_position_before)); - node.setProperty("estimationerror").send((String)(blind.position-_position_before)); + node.setProperty("estimationerror").send((String)(blind.position-_position_before)); //positive value means estimation position was too low (estimated speed to low), when driving down (positive dir) - if (blind.mode!= MODE_FIND_END) { //only check error if not in find_end mode + if (blind.mode != MODE_FIND_END && blind.mode != MODE_MEASURE_SPEED ) { //only check error if not in find_end mode or measure speed if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) { setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH, node); } @@ -555,16 +553,15 @@ void updateMotor(blindmodel &blind, Motor motor) if (blind.error!=0) { //error appeared blind.speed=0; //set speed to 0 } - if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) { - if(blind.speed<0){ - motor.setmotor( _UP, abs(blind.speed)); - }else if(blind.speed>0){ - motor.setmotor( _DOWN, abs(blind.speed)); - }else{ - motor.setmotor(_STOP); - } - last_motor_send=millis(); + + if(blind.speed<0){ + motor.setmotor( _UP, abs(blind.speed)); + }else if(blind.speed>0){ + motor.setmotor( _DOWN, abs(blind.speed)); + }else{ + motor.setmotor(_STOP); } + } void checkModes(blindmodel &blind, HomieNode &node) { @@ -580,7 +577,7 @@ void checkModes(blindmodel &blind, HomieNode &node) { break; case 1: //drive down slowly until passed end maker - blind.speed=10; //down slow + blind.speed=25; //down slow if (blind.sense_status!=SENSESTATUS_END) { blind.speed=0; //stop blind.position=0; @@ -708,6 +705,24 @@ String modeNumToString(uint8_t modenum){ return "MODE_ERROR"; break; } + return "UNDEF"; +} + +String sensestatusNumToString(uint8_t sensestatusnum){ + switch(sensestatusnum){ + case SENSESTATUS_UNKNOWN: + return "UNK"; + break; + case SENSESTATUS_CLEAR: + return "CLE"; + break; + case SENSESTATUS_OPAQUE: + return "OPA"; + break; + case SENSESTATUS_END: + return "END"; + break; + } } @@ -729,28 +744,6 @@ bool blind_l_positionHandler(const HomieRange& range, const String& value) { return true; } -bool blind_l_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_l_cmdHandler" << ": " << value << endl; - - if (value=="END") - { - blind1.mode = MODE_FIND_END; - blind1.mode_find_end_state=0; //reset mode find state - }else if (value=="SPEEDFACTOR") - { - blind1.mode=MODE_MEASURE_SPEED; //measure speed - }else{ - Homie.getLogger() << "command not known" << endl; - return false; - } - blind1Node.setProperty("cmd").send(value); //can be done in main loop - - return true; -} - bool blind_r_positionHandler(const HomieRange& range, const String& value) { if (range.isRange) { @@ -763,12 +756,27 @@ bool blind_r_positionHandler(const HomieRange& range, const String& value) { return true; } + + +bool blind_l_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_l_cmdHandler" << ": " << value << endl; + + return blind_cmdHandler(blind1,blind1Node, value); + +} + + 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; + return blind_cmdHandler(blind2,blind2Node, value); + /* if (value=="END") { blind2.mode = MODE_FIND_END; @@ -776,11 +784,49 @@ bool blind_r_cmdHandler(const HomieRange& range, const String& value) { }else if (value=="SPEEDFACTOR") { blind2.mode=MODE_MEASURE_SPEED; //measure speed + blind1Node.setProperty("cmd").send("cmd measure speed"); }else{ - Homie.getLogger() << "command not known" << endl; + Homie.getLogger() << "unknown command" << endl; return false; } - blind2Node.setProperty("cmd").send(value); //can be done in main loop + + + return true;*/ +} + + +bool blind_cmdHandler(blindmodel &blind, HomieNode& node, const String& value) { + if (value=="END") + { + blind.mode = MODE_FIND_END; + blind.mode_find_end_state=0; //reset mode find state + node.setProperty("cmd").send("cmd end"); + }else if (value=="SPEEDFACTOR") + { + blind.mode=MODE_MEASURE_SPEED; //measure speed + node.setProperty("cmd").send("cmd measure speed"); + }else if (value=="SENSOR") //return adc values + { + Serial.print("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")"); + Serial.print(", mode="); + Serial.print(modeNumToString(blind.mode)); + node.setProperty("cmd").send(""+String(getFitered(blind.sense_read, SENSE_FILTER_SIZE))+" "+sensestatusNumToString(blind.sense_status)); + }else if (value=="STOP") //stop. go back to idle mode and stop driving + { + blind.speed=0; //stop + blind.mode=MODE_IDLE; + blind.set_position=blind.position; //use current position as set pos to stay there + node.setProperty("cmd").send("stop"); + }else if (value=="RESET") //reset from error + { + blind.error=0; + node.setProperty("cmd").send("reset"); + }else{ + Homie.getLogger() << "unknown command" << endl; + return false; + } + return true; } +