From a00c2f004904de50810bd386e0f0c5f046ac87ae Mon Sep 17 00:00:00 2001 From: Fisch Date: Tue, 2 Feb 2021 21:43:02 +0100 Subject: [PATCH] add deviation error check --- src/main.cpp | 88 +++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 73 insertions(+), 15 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index c417720..8638d20 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -40,6 +40,12 @@ unsigned long last_calculate_position_estimate=0; #define PIN_SENSE A0 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 directions for motors #define _UP _CCW #define _DOWN _CW @@ -82,6 +88,8 @@ struct blindmodel int sense_read[SENSE_FILTER_SIZE] = {0}; uint8_t sense_read_pos=0; //position of last element written to + uint8_t pin_sensor_led; //pin to activate sensor. high activates sensor + float set_position=0; unsigned long last_sense_ok=0; //last time sensor measured class ok @@ -98,6 +106,7 @@ 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) + uint8_t error=0; }; blindmodel blind1; @@ -112,16 +121,19 @@ Motor M2(0x30, _MOTOR_B, 1000); //Motor B unsigned long last_motor_send=0; #define MOTOR_UPDATE_INTERVAL 100 +#define DIFF_ERROR_FACTOR 0.35 //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 int getFitered(int* values,uint8_t size); -uint8_t classifySensorValue(blindmodel &blind); +void classifySensorValue(blindmodel &blind); void checkButton(button &btn); void checkModes(blindmodel &blind); void manualMoveHandler(button &btn, blindmodel &blind); -void readSensor(blindmodel &blind); +void readSensor(blindmodel &blind, int value); void estimatePosition(blindmodel &blind); void errorCheck(blindmodel &blind); void updateMotor(blindmodel &blind, Motor motor); +void setError(blindmodel &blind, uint8_t errorcode); void setup() { Serial.begin(115200); @@ -134,17 +146,23 @@ void setup() { pinMode(button2.pin, INPUT_PULLUP); pinMode(PIN_SENSE, INPUT); + pinMode(PIN_SENSE_LED1,OUTPUT); + pinMode(PIN_SENSE_LED2,OUTPUT); + digitalWrite(PIN_SENSE_LED1, LOW); + digitalWrite(PIN_SENSE_LED2, LOW); + M1.setmotor(_STOP); M2.setmotor(_STOP); //settings for blind + blind1.pin_sensor_led=PIN_SENSE_LED1; blind1.length_clear=50; blind1.length_opaque=74; blind1.sense_clear_lower=40; - blind1.sense_clear_upper=100; + blind1.sense_clear_upper=200; blind1.sense_opaque_lower=500; - blind1.sense_opaque_upper=750; + blind1.sense_opaque_upper=850; blind1.sense_end_lower=850; blind1.sense_end_upper=1024; blind1.speedfactor=29; @@ -152,12 +170,13 @@ void setup() { blind1.simulated_acc_dec=-150; blind1.simulated_acc_inc=200; + blind2.pin_sensor_led=PIN_SENSE_LED2; blind2.length_clear=50; blind2.length_opaque=74; blind2.sense_clear_lower=40; - blind2.sense_clear_upper=100; + blind2.sense_clear_upper=200; blind2.sense_opaque_lower=500; - blind2.sense_opaque_upper=750; + blind2.sense_opaque_upper=850; blind2.sense_end_lower=850; blind2.sense_end_upper=1024; blind2.speedfactor=29; @@ -169,8 +188,11 @@ void setup() { 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 + //blind2.mode = MODE_FIND_END; + //blind2.mode_find_end_state=0; //reset mode find state + + + } @@ -189,8 +211,27 @@ void loop() { //Read sensor/encoder - readSensor(blind1); - readSensor(blind2); + if (millis() > last_sensor_led_switch + SENSOR_SWITCH_INTERVAL) { + int rawsensorvalue=analogRead(PIN_SENSE); + switch (sensorreadID) { + case 0: + readSensor(blind1,rawsensorvalue); + sensorreadID++; + digitalWrite(blind1.pin_sensor_led,LOW); //turn self off + digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on + break; + case 1: + readSensor(blind2,rawsensorvalue); + sensorreadID++; + digitalWrite(blind2.pin_sensor_led,LOW); //turn self off + digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on + break; + default: + sensorreadID=0; //reset + break; + } + last_sensor_led_switch=millis(); + } checkModes(blind1); @@ -230,7 +271,7 @@ int getFitered(int* values,uint8_t size) { return copied_values[size/2]; } -uint8_t classifySensorValue(blindmodel &blind) { +void 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; @@ -277,11 +318,11 @@ void manualMoveHandler(button &btn, blindmodel &blind) } } -void readSensor(blindmodel &blind) +void readSensor(blindmodel &blind, int value) { 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); + blind.sense_read[blind.sense_read_pos]=value; classifySensorValue(blind); //writes to blindmodel.sense_status @@ -289,7 +330,7 @@ void readSensor(blindmodel &blind) last_sensor_read=millis(); } - if (millis() > last_print + 500) { + if (millis() > last_print + 500 && blind.speedSimulated!=0) { Serial.print("SenseStatus="); switch(blind.sense_status){ case SENSESTATUS_UNKNOWN: @@ -337,6 +378,8 @@ void errorCheck(blindmodel &blind) { blind.speed=0; } } + + //TODO: led self test. turn off ,should be high value } void estimatePosition(blindmodel &blind) { @@ -388,7 +431,7 @@ void estimatePosition(blindmodel &blind) { 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("posCorrection="); Serial.print(_filterdelay_correction); Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before="); @@ -399,6 +442,10 @@ void estimatePosition(blindmodel &blind) { Serial.print(blind.position); Serial.print(", diff="); Serial.println(blind.position-_position_before); + + if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) { + setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH); + } } blind.last_sense_status = blind.sense_status; //update only if new one is opaque or clear @@ -409,6 +456,9 @@ void estimatePosition(blindmodel &blind) { 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)); @@ -487,3 +537,11 @@ void checkModes(blindmodel &blind) { break; } } + + +void setError(blindmodel &blind, uint8_t errorcode){ + if (blind.error==0) { //only set error if no error appeared before + blind.error=errorcode; + } + Serial.print("ERROR CODE="); Serial.println(errorcode); +} \ No newline at end of file