interfisch
/
blinds_motorcontrol
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add deviation error check

Browse Source
master
interfisch 3 years ago
parent fe21d1b79f
commit a00c2f0049
1 changed files with 73 additions and 15 deletions
Show Stats Download Patch File Download Diff File

88
src/main.cpp
Unescape Escape View File

@ -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);
}
Write Preview
Loading…
Cancel
Save