add deviation error check
This commit is contained in:
parent
fe21d1b79f
commit
a00c2f0049
88
src/main.cpp
88
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);
|
||||
}
|
||||
Loading…
Reference in New Issue