separate into functions
This commit is contained in:
parent
fd43be8326
commit
7529403d6c
414
src/main.cpp
414
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);
|
||||
|
||||
}
|
||||
*/
|
||||
Loading…
Reference in New Issue