From 7529403d6cdd7300b44ad64a941e3e2391b27705 Mon Sep 17 00:00:00 2001 From: Fisch Date: Tue, 2 Feb 2021 20:54:35 +0100 Subject: [PATCH] separate into functions --- src/main.cpp | 414 ++++++++++++++++++++++++++------------------------- 1 file changed, 214 insertions(+), 200 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 41fd86b..4424c8e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,13 +17,16 @@ #define BUTTON_DEBOUNCE 200 #define PIN_BUTTON1 D5 +#define PIN_BUTTON2 D6 struct button{ uint8_t pin; unsigned long last_time_read=0; bool down=false; bool changed=false; + bool manual_drive_direction=false; }; button button1; +button button2; unsigned long last_sensor_read=0; #define SENSOR_READ_INTERVAL 20 //in ms @@ -32,7 +35,7 @@ unsigned long last_sensor_read=0; #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; @@ -98,6 +101,7 @@ struct blindmodel }; blindmodel blind1; +blindmodel blind2; //Motor shield default I2C Address: 0x30 @@ -111,17 +115,23 @@ unsigned long last_motor_send=0; int getFitered(int* values,uint8_t size); uint8_t classifySensorValue(blindmodel &blind); +void checkButton(button &btn); void checkModes(blindmodel &blind); +void manualMoveHandler(button &btn, blindmodel &blind); +void readSensor(blindmodel &blind); +void estimatePosition(blindmodel &blind); +void errorCheck(blindmodel &blind); +void updateMotor(blindmodel &blind, Motor motor); void setup() { - - Serial.begin(115200); Serial.println("Starting"); button1.pin=PIN_BUTTON1; + button2.pin=PIN_BUTTON2; pinMode(button1.pin, INPUT_PULLUP); + pinMode(button2.pin, INPUT_PULLUP); pinMode(PIN_SENSE, INPUT); M1.setmotor(_STOP); @@ -142,186 +152,53 @@ void setup() { blind1.simulated_acc_dec=-150; blind1.simulated_acc_inc=200; + blind2.length_clear=50; + blind2.length_opaque=74; + blind2.sense_clear_lower=40; + blind2.sense_clear_upper=100; + blind2.sense_opaque_lower=500; + blind2.sense_opaque_upper=750; + blind2.sense_end_lower=850; + blind2.sense_end_upper=1024; + blind2.speedfactor=29; + blind2.start_first_clear=27; + blind2.simulated_acc_dec=-150; + blind2.simulated_acc_inc=200; + //Test blind1.mode = MODE_FIND_END; blind1.mode_find_end_state=0; //reset mode find state + + blind2.mode = MODE_FIND_END; + blind2.mode_find_end_state=0; //reset mode find state } void loop() { - button1.changed=false; - if (millis() > button1.last_time_read + BUTTON_DEBOUNCE) { - bool new_pin_button_down=!digitalRead(button1.pin); - if (button1.down != new_pin_button_down) { //changed - button1.down = new_pin_button_down; //update - button1.changed=true; - button1.last_time_read=millis(); //delay next check - } - } + checkButton(button1); + //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); - blind1.speed=-100; - //Serial.print("CW PWM: "); - }else{ - blind1.speed=100; - //Serial.print("CCW PWM: "); - } - manual_drive_direction=!manual_drive_direction; //switch direction every new press - }else{ //changed to released - //Serial.println("Motor STOP"); - blind1.speed=0; - } - } + manualMoveHandler(button1, blind1); //Read sensor/encoder - if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) { - blind1.sense_read_pos=(blind1.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element - blind1.sense_read[blind1.sense_read_pos]=analogRead(PIN_SENSE); - - classifySensorValue(blind1); //writes to blindmodel.sense_status - - - last_sensor_read=millis(); - } - - 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(); - } + readSensor(blind1); checkModes(blind1); + errorCheck(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 - } - } - } + estimatePosition(blind1); //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(); - } + updateMotor(blind1, M1); } int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort @@ -360,13 +237,190 @@ uint8_t classifySensorValue(blindmodel &blind) { } } +void checkButton(button &btn) { + btn.changed=false; + if (millis() > btn.last_time_read + BUTTON_DEBOUNCE) { + bool new_pin_button_down=!digitalRead(btn.pin); + if (btn.down != new_pin_button_down) { //changed + btn.down = new_pin_button_down; //update + btn.changed=true; + btn.last_time_read=millis(); //delay next check + } + } +} + +void manualMoveHandler(button &btn, blindmodel &blind) +{ + if (btn.changed) { + blind.mode=MODE_IDLE; + if (btn.down) { //changed to pressed + if (btn.manual_drive_direction) { + //M1.setmotor( _CW, 100); + blind.speed=-100; + //Serial.print("CW PWM: "); + }else{ + blind.speed=100; + //Serial.print("CCW PWM: "); + } + btn.manual_drive_direction=!btn.manual_drive_direction; //switch direction every new press + }else{ //changed to released + //Serial.println("Motor STOP"); + blind.speed=0; + } + } +} + +void readSensor(blindmodel &blind) +{ + 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]=analogRead(PIN_SENSE); + + classifySensorValue(blind); //writes to blindmodel.sense_status + + + last_sensor_read=millis(); + } + + if (millis() > last_print + 500) { + 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("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")"); + Serial.print(", mode="); + switch(blind.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(blind.speed); + Serial.print(", speedSim="); + Serial.print(blind.speedSimulated); + Serial.print(", pos="); + Serial.println(blind.position); + last_print=millis(); + } +} + +void errorCheck(blindmodel &blind) { + if (blind.sense_status==SENSESTATUS_END) { + if (blind.speed<0) { //stop driving up + blind.speed=0; + } + } +} + +void estimatePosition(blindmodel &blind) { + if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) { + float _actualTime=(millis()-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*blind.speedfactor * _actualTime; + last_calculate_position_estimate=millis(); + } + + if (blind.mode!= MODE_FIND_END) { + if (blind.sense_status == SENSESTATUS_END && blind.last_sense_status != SENSESTATUS_END) { //entered end marker + blind.position=0; + float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0; + blind.position += _filterdelay_correction; //correct for filter delay + Serial.print("Reached End marker. set Position="); Serial.println(blind.position); + } + + if (blind.sense_status == SENSESTATUS_CLEAR || blind.sense_status == SENSESTATUS_OPAQUE || blind.sense_status == SENSESTATUS_END) { //either opaque or clear (or end, only for last update used) + if (blind.sense_status != blind.last_sense_status) { //status changed + if (blind.last_sense_status == SENSESTATUS_CLEAR || blind.last_sense_status == SENSESTATUS_OPAQUE) { //only changes from clear to opaque or opaque to clear + + float _position_before=blind.position;//only for text output debugging + + if ((blind.speedSimulated>0 && blind.sense_status==SENSESTATUS_OPAQUE) || (blind.speedSimulated<0 && blind.sense_status==SENSESTATUS_CLEAR)) { //moving down from and clear to opaque OR moving up and from opaque to clear + + //estimated_position_difference = -blind.position + blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //possible occurances. let estimated_position=0 , solve for n + + int _n = round((blind.position -blind.start_first_clear - blind.length_clear)/(blind.length_clear+blind.length_opaque)); //find closest n + blind.position = blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n + float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0; + blind.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((blind.speedSimulated>0 && blind.sense_status==SENSESTATUS_CLEAR) || (blind.speedSimulated<0 && blind.sense_status==SENSESTATUS_OPAQUE)) { //the other transition + //estimated_position_difference = -blind.position + blind.start_first_clear + _n*(blind.length_clear+blind.length_opaque); //possible occurances. let estimated_position=0 , solve for n + + int _n = round(( blind.position - blind.start_first_clear)/(blind.length_clear+blind.length_opaque)); //find closest n + Serial.print("n="); + Serial.println(_n); + blind.position = blind.start_first_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n + float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0; + blind.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(blind.position); + Serial.print(", diff="); + Serial.println(blind.position-_position_before); + } + + blind.last_sense_status = blind.sense_status; //update only if new one is opaque or clear + } + } + } +} + +void updateMotor(blindmodel &blind, Motor motor) +{ + 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(); + } +} + 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) { + if (blind.sense_status==SENSESTATUS_END) { blind.speed=0; //stop. for safety blind.mode_find_end_state++; } @@ -426,43 +480,3 @@ void checkModes(blindmodel &blind) { break; } } - -/* -void loop() { - - int pwm; - - for (pwm = 0; pwm <= 100; pwm++) - { - M1.setmotor( _CW, pwm); - M2.setmotor(_CW, pwm); - Serial.print("Clockwise PWM: "); - Serial.println(pwm); - delay(100); - } - - Serial.println("Motor STOP"); - M1.setmotor(_STOP); - M2.setmotor( _STOP); - - delay(1000); - - - for (pwm = 0; pwm <= 100; pwm++) - { - M1.setmotor(_CCW, pwm); - //delay(1); - M2.setmotor(_CCW, pwm); - Serial.print("Counterclockwise PWM: "); - Serial.println(pwm); - delay(100); - - } - - Serial.println("Motor A&B STANDBY"); - M1.setmotor(_STANDBY); - M2.setmotor( _STANDBY); - delay(1000); - -} -*/ \ No newline at end of file