separate into functions

master
interfisch 2 years ago
parent fd43be8326
commit 7529403d6c
  1. 314
      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,53 +152,131 @@ 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) {
manualMoveHandler(button1, blind1);
//Read sensor/encoder
readSensor(blind1);
checkModes(blind1);
errorCheck(blind1);
//Estimate blind position and correct
estimatePosition(blind1);
//Update Motor Driver
updateMotor(blind1, M1);
}
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
// Need to cast the void * to int *
int a = *((int *)cmp1);
int b = *((int *)cmp2);
// The comparison
return a > b ? -1 : (a < b ? 1 : 0);
// A simpler, probably faster way:
//return b - a;
}
int getFitered(int* values,uint8_t size) {
int copied_values[size];
for(int i=0;i<size;i++) {
copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
}
int copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
return copied_values[size/2];
}
uint8_t classifySensorValue(blindmodel &blind) {
int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
blind.sense_status=SENSESTATUS_CLEAR;
blind.last_sense_ok=millis();
} else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
blind.sense_status=SENSESTATUS_OPAQUE;
blind.last_sense_ok=millis();
} else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
blind.sense_status=SENSESTATUS_END;
blind.last_sense_ok=millis();
}
}
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);
blind1.speed=-100;
blind.speed=-100;
//Serial.print("CW PWM: ");
}else{
blind1.speed=100;
blind.speed=100;
//Serial.print("CCW PWM: ");
}
manual_drive_direction=!manual_drive_direction; //switch direction every new press
btn.manual_drive_direction=!btn.manual_drive_direction; //switch direction every new press
}else{ //changed to released
//Serial.println("Motor STOP");
blind1.speed=0;
blind.speed=0;
}
}
}
//Read sensor/encoder
void readSensor(blindmodel &blind)
{
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);
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(blind1); //writes to blindmodel.sense_status
classifySensorValue(blind); //writes to blindmodel.sense_status
last_sensor_read=millis();
@ -196,7 +284,7 @@ void loop() {
if (millis() > last_print + 500) {
Serial.print("SenseStatus=");
switch(blind1.sense_status){
switch(blind.sense_status){
case SENSESTATUS_UNKNOWN:
Serial.print("UNK");
break;
@ -210,9 +298,9 @@ void loop() {
Serial.print("END");
break;
}
Serial.print("("); Serial.print(getFitered(blind1.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
Serial.print("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
Serial.print(", mode=");
switch(blind1.mode){
switch(blind.mode){
case MODE_IDLE:
Serial.print("IDL");
break;
@ -227,54 +315,54 @@ void loop() {
break;
}
Serial.print(", speed=");
Serial.print(blind1.speed);
Serial.print(blind.speed);
Serial.print(", speedSim=");
Serial.print(blind1.speedSimulated);
Serial.print(blind.speedSimulated);
Serial.print(", pos=");
Serial.println(blind1.position);
Serial.println(blind.position);
last_print=millis();
}
}
checkModes(blind1);
if (blind1.sense_status==SENSESTATUS_END) {
if (blind1.speed<0) { //stop driving up
blind1.speed=0;
void errorCheck(blindmodel &blind) {
if (blind.sense_status==SENSESTATUS_END) {
if (blind.speed<0) { //stop driving up
blind.speed=0;
}
}
//Estimate blind position and correct
}
void estimatePosition(blindmodel &blind) {
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);
blind.speedSimulated+= constrain(blind.speed - blind.speedSimulated, blind.simulated_acc_dec*_actualTime, blind.simulated_acc_inc*_actualTime);
blind1.position+= blind1.speedSimulated/100.0*blind1.speedfactor * _actualTime;
blind.position+= blind.speedSimulated/100.0*blind.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 (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 (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
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=blind1.position;//only for text output debugging
float _position_before=blind.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
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 = -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
//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((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
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);
@ -283,15 +371,15 @@ void loop() {
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
}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(( blind1.position - blind1.start_first_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest 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);
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
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);
@ -301,72 +389,38 @@ void loop() {
Serial.print(_position_before);
Serial.print(", after=");
Serial.print(blind1.position);
Serial.print(blind.position);
Serial.print(", diff=");
Serial.println(blind1.position-_position_before);
Serial.println(blind.position-_position_before);
}
blind1.last_sense_status = blind1.sense_status; //update only if new one is opaque or clear
blind.last_sense_status = blind.sense_status; //update only if new one is opaque or clear
}
}
}
}
//Update Motor Driver
void updateMotor(blindmodel &blind, Motor motor)
{
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));
if(blind.speed<0){
motor.setmotor( _UP, abs(blind.speed));
}else if(blind.speed>0){
motor.setmotor( _DOWN, abs(blind.speed));
}else{
M1.setmotor(_STOP);
motor.setmotor(_STOP);
}
last_motor_send=millis();
}
}
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
// Need to cast the void * to int *
int a = *((int *)cmp1);
int b = *((int *)cmp2);
// The comparison
return a > b ? -1 : (a < b ? 1 : 0);
// A simpler, probably faster way:
//return b - a;
}
int getFitered(int* values,uint8_t size) {
int copied_values[size];
for(int i=0;i<size;i++) {
copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
}
int copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
return copied_values[size/2];
}
uint8_t classifySensorValue(blindmodel &blind) {
int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
blind.sense_status=SENSESTATUS_CLEAR;
blind.last_sense_ok=millis();
} else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
blind.sense_status=SENSESTATUS_OPAQUE;
blind.last_sense_ok=millis();
} else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
blind.sense_status=SENSESTATUS_END;
blind.last_sense_ok=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…
Cancel
Save