diff --git a/src/main.cpp b/src/main.cpp index db18bd9..41fd86b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,6 +1,14 @@ -#include +#include #include +/* + TODO: + - correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state + - implement second blind + - implement drive to position + - implement failure detection + - implement homie +*/ #include "WEMOS_Motor.h" @@ -21,43 +29,40 @@ unsigned long last_sensor_read=0; #define SENSOR_READ_INTERVAL 20 //in ms #define SENSE_FILTER_SIZE 20 //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; + bool manual_drive_direction=false; #define PIN_SENSE A0 unsigned long last_print=0; -#define SENSE_CLEAR_LOWER 40 //adc value lower limit for clear part. clear is around 70 -#define SENSE_CLEAR_UPPER 100 //adc value upper limit for clear part - -#define SENSE_OPAQUE_LOWER 600 //adc value lower limit for opaque part. opaque is around 675 -#define SENSE_OPAQUE_UPPER 750 //adc value upper limit for opaque part - -#define SENSE_END_LOWER 850 //adc value lower limit for end marker -#define SENSE_END_UPPER 1024 //adc value upper limit for end marker //define directions for motors -#define _M1_UP _CCW; -#define _M1_DOWN _CW; - -#define _M2_UP _CCW; -#define _M2_DOWN _CW; - -#define SENSESTATUS_CLEAR 1; -#define SENSESTATUS_OPAQUE 2; -#define SENSESTATUS_END 3; -#define SENSESTATUS_UNKNOWN 0; +#define _UP _CCW +#define _DOWN _CW +#define SENSESTATUS_CLEAR 1 +#define SENSESTATUS_OPAQUE 2 +#define SENSESTATUS_END 3 +#define SENSESTATUS_UNKNOWN 0 +#define MODE_IDLE 0 +#define MODE_FIND_END 1 +#define MODE_MEASURE_SPEED 2 +#define MODE_GO_TO_POS 3 //model parameters/variables struct blindmodel { unsigned long lastreadtime=0; - float position=0; //0 is furthest open. positive is down (closing). unit is mm + float position=0; //0 is furthest open. positive is down (closing). unit is mm. estimated position float length_clear; //length of clear part in position units float length_opaque; //lengt of opaque part in position units + float start_first_clear; //distance from end marker to beginning of first clear section + //end marker should be on the opaque section. So that a full clear section follows - float speed_estimate=1; //how much position units (mm) per second at pwm=100 + float speedfactor=1; //how much position units (mm) per second at pwm=100 int sense_clear_lower; //adc value lower limit for clear part. clear is around 70 int sense_clear_upper; //adc value upper limit for clear part @@ -67,8 +72,10 @@ struct blindmodel int sense_end_lower; //adc value lower limit for end marker int sense_end_upper; //adc value upper limit for end marker + uint8_t sense_status=0; + uint8_t last_sense_status=0; int sense_read[SENSE_FILTER_SIZE] = {0}; uint8_t sense_read_pos=0; //position of last element written to @@ -76,6 +83,18 @@ struct blindmodel unsigned long last_sense_ok=0; //last time sensor measured class ok //TODO: implement timeout if last_sense_ok gets too high. + + uint8_t mode=MODE_IDLE; + + uint8_t mode_find_end_state=0; + uint8_t mode_measure_speed_state=0; + unsigned long timing_start; + + int speed=0; //-100 to 100. commanded speed to motor. negative means upwards + int speedSimulated=0; //software simulated motor speed up and slow down + 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) + }; blindmodel blind1; @@ -86,15 +105,18 @@ blindmodel blind1; Motor M1(0x30, _MOTOR_A, 1000); //Motor A Motor M2(0x30, _MOTOR_B, 1000); //Motor B +unsigned long last_motor_send=0; +#define MOTOR_UPDATE_INTERVAL 100 int getFitered(int* values,uint8_t size); uint8_t classifySensorValue(blindmodel &blind); +void checkModes(blindmodel &blind); void setup() { - Serial.begin(57600); + Serial.begin(115200); Serial.println("Starting"); button1.pin=PIN_BUTTON1; @@ -111,11 +133,18 @@ void setup() { blind1.length_opaque=74; blind1.sense_clear_lower=40; blind1.sense_clear_upper=100; - blind1.sense_opaque_lower=600; + blind1.sense_opaque_lower=500; blind1.sense_opaque_upper=750; blind1.sense_end_lower=850; blind1.sense_end_upper=1024; - blind1.speed_estimate=20; + blind1.speedfactor=29; + blind1.start_first_clear=27; + blind1.simulated_acc_dec=-150; + blind1.simulated_acc_inc=200; + + //Test + blind1.mode = MODE_FIND_END; + blind1.mode_find_end_state=0; //reset mode find state } @@ -135,19 +164,20 @@ void loop() { //Manual movement by button if (button1.changed) { + blind1.mode=MODE_IDLE; if (button1.down) { //changed to pressed if (manual_drive_direction) { - M1.setmotor( _CW, 100); - Serial.print("CW PWM: "); + //M1.setmotor( _CW, 100); + blind1.speed=-100; + //Serial.print("CW PWM: "); }else{ - M1.setmotor( _CCW, 100); - Serial.print("CCW PWM: "); + blind1.speed=100; + //Serial.print("CCW PWM: "); } - Serial.println(100); manual_drive_direction=!manual_drive_direction; //switch direction every new press }else{ //changed to released - Serial.println("Motor STOP"); - M1.setmotor(_STOP); + //Serial.println("Motor STOP"); + blind1.speed=0; } } @@ -164,10 +194,134 @@ void loop() { last_sensor_read=millis(); } - if (millis() > last_print + 100) { - Serial.println(blind1.sense_status); + if (millis() > last_print + 500) { + Serial.print("SenseStatus="); + switch(blind1.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("("); Serial.print(getFitered(blind1.sense_read, SENSE_FILTER_SIZE)); Serial.print(")"); + Serial.print(", mode="); + switch(blind1.mode){ + case MODE_IDLE: + Serial.print("IDL"); + break; + case MODE_FIND_END: + Serial.print("FIN"); + break; + case MODE_GO_TO_POS: + Serial.print("POS"); + break; + case MODE_MEASURE_SPEED: + Serial.print("MSP"); + break; + } + Serial.print(", speed="); + Serial.print(blind1.speed); + Serial.print(", speedSim="); + Serial.print(blind1.speedSimulated); + Serial.print(", pos="); + Serial.println(blind1.position); last_print=millis(); } + + + checkModes(blind1); + + + if (blind1.sense_status==SENSESTATUS_END) { + if (blind1.speed<0) { //stop driving up + blind1.speed=0; + } + } + //Estimate blind position and correct + if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) { + float _actualTime=(millis()-last_calculate_position_estimate)/1000.0; //in seconds + blind1.speedSimulated+= constrain(blind1.speed - blind1.speedSimulated, blind1.simulated_acc_dec*_actualTime, blind1.simulated_acc_inc*_actualTime); + + blind1.position+= blind1.speedSimulated/100.0*blind1.speedfactor * _actualTime; + last_calculate_position_estimate=millis(); + } + + if (blind1.mode!= MODE_FIND_END) { + if (blind1.sense_status == SENSESTATUS_END && blind1.last_sense_status != SENSESTATUS_END) { //entered end marker + blind1.position=0; + float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0; + blind1.position += _filterdelay_correction; //correct for filter delay + Serial.print("Reached End marker. set Position="); Serial.println(blind1.position); + } + + if (blind1.sense_status == SENSESTATUS_CLEAR || blind1.sense_status == SENSESTATUS_OPAQUE || blind1.sense_status == SENSESTATUS_END) { //either opaque or clear (or end, only for last update used) + if (blind1.sense_status != blind1.last_sense_status) { //status changed + if (blind1.last_sense_status == SENSESTATUS_CLEAR || blind1.last_sense_status == SENSESTATUS_OPAQUE) { //only changes from clear to opaque or opaque to clear + + float _position_before=blind1.position;//only for text output debugging + + if ((blind1.speedSimulated>0 && blind1.sense_status==SENSESTATUS_OPAQUE) || (blind1.speedSimulated<0 && blind1.sense_status==SENSESTATUS_CLEAR)) { //moving down from and clear to opaque OR moving up and from opaque to clear + + //estimated_position_difference = -blind1.position + blind1.start_first_clear+blind1.length_clear + _n*(blind1.length_clear+blind1.length_opaque); //possible occurances. let estimated_position=0 , solve for n + + int _n = round((blind1.position -blind1.start_first_clear - blind1.length_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest n + blind1.position = blind1.start_first_clear+blind1.length_clear + _n*(blind1.length_clear+blind1.length_opaque); //calculate position from _n + float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0; + blind1.position += _filterdelay_correction; //correct for filter delay + Serial.print("n="); + Serial.println(_n); + + Serial.print("posCorrection="); Serial.print(_filterdelay_correction); + + Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before="); + + + }else if((blind1.speedSimulated>0 && blind1.sense_status==SENSESTATUS_CLEAR) || (blind1.speedSimulated<0 && blind1.sense_status==SENSESTATUS_OPAQUE)) { //the other transition + //estimated_position_difference = -blind1.position + blind1.start_first_clear + _n*(blind1.length_clear+blind1.length_opaque); //possible occurances. let estimated_position=0 , solve for n + + int _n = round(( blind1.position - blind1.start_first_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest n + Serial.print("n="); + Serial.println(_n); + blind1.position = blind1.start_first_clear + _n*(blind1.length_clear+blind1.length_opaque); //calculate position from _n + float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0; + blind1.position += _filterdelay_correction; //correct for filter delay + + Serial.print("posCorrectoin="); Serial.print(_filterdelay_correction); + + Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before="); + + } + + Serial.print(_position_before); + Serial.print(", after="); + Serial.print(blind1.position); + Serial.print(", diff="); + Serial.println(blind1.position-_position_before); + } + + blind1.last_sense_status = blind1.sense_status; //update only if new one is opaque or clear + } + } + } + + //Update Motor Driver + if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) { + if(blind1.speed<0){ + M1.setmotor( _UP, abs(blind1.speed)); + }else if(blind1.speed>0){ + M1.setmotor( _DOWN, abs(blind1.speed)); + }else{ + M1.setmotor(_STOP); + } + last_motor_send=millis(); + } } int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort @@ -178,7 +332,7 @@ int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort // The comparison return a > b ? -1 : (a < b ? 1 : 0); // A simpler, probably faster way: - //return b - a; + //return b - a; } int getFitered(int* values,uint8_t size) { @@ -206,6 +360,73 @@ uint8_t classifySensorValue(blindmodel &blind) { } } +void checkModes(blindmodel &blind) { + 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 (blind1.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=10; //down slow + if (blind1.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 + + } + break; + } + + break; + + case MODE_MEASURE_SPEED: + switch(blind.mode_measure_speed_state) { + case 0: //drive down, start timing at clear + blind.speed=100; //down + if (blind1.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 + 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 + blind.speedfactor=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second + Serial.print("speedfactor="); + Serial.print(blind.speedfactor); + Serial.println(" mm/s"); + blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position + blind.speed=0; //stop + blind.mode_measure_speed_state=0; // reset + blind.mode=MODE_IDLE; + } + break; + + + } + + break; + + case MODE_GO_TO_POS: + + break; + } +} + /* void loop() {