implement sensor filtering

This commit is contained in:
interfisch 2021-01-31 17:59:57 +01:00
parent 90e160228f
commit ea71f7dd2c
1 changed files with 140 additions and 28 deletions

View File

@ -6,11 +6,20 @@
//follow firmware flash guide for new wemos motor shield v1.0 https://github.com/thomasfredericks/wemos_motor_shield //follow firmware flash guide for new wemos motor shield v1.0 https://github.com/thomasfredericks/wemos_motor_shield
#define BUTTON_DEBOUNCE 200 #define BUTTON_DEBOUNCE 200
#define PIN_BUTTON D5 #define PIN_BUTTON1 D5
unsigned long last_pin_button=0; struct button{
bool pin_button_down=0; uint8_t pin;
bool last_pin_button_down=0; unsigned long last_time_read=0;
bool down=false;
bool changed=false;
};
button button1;
unsigned long last_sensor_read=0;
#define SENSOR_READ_INTERVAL 20 //in ms
#define SENSE_FILTER_SIZE 20 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL
bool manual_drive_direction=false; bool manual_drive_direction=false;
@ -26,13 +35,51 @@ unsigned long last_print=0;
#define SENSE_END_UPPER 1024 //adc value upper limit for end marker #define SENSE_END_UPPER 1024 //adc value upper limit for end marker
//define directions for motors //define directions for motors
#define _M1_UP = _CCW; #define _M1_UP _CCW;
#define _M1_DOWN = _CW; #define _M1_DOWN _CW;
#define _M2_UP = _CCW; #define _M2_UP _CCW;
#define _M2_DOWN = _CW; #define _M2_DOWN _CW;
#define SENSESTATUS_CLEAR 1;
#define SENSESTATUS_OPAQUE 2;
#define SENSESTATUS_END 3;
#define SENSESTATUS_UNKNOWN 0;
//model parameters/variables
struct blindmodel
{
unsigned long lastreadtime=0;
float position=0; //0 is furthest open. positive is down (closing). unit is mm
float length_clear; //length of clear part in position units
float length_opaque; //lengt of opaque part in position units
float speed_estimate=1; //how much position units (mm) per second at pwm=100
int sense_clear_lower; //adc value lower limit for clear part. clear is around 70
int sense_clear_upper; //adc value upper limit for clear part
int sense_opaque_lower; //adc value lower limit for opaque part. opaque is around 675
int sense_opaque_upper; //adc value upper limit for opaque part
int sense_end_lower; //adc value lower limit for end marker
int sense_end_upper; //adc value upper limit for end marker
uint8_t sense_status=0;
int sense_read[SENSE_FILTER_SIZE] = {0};
uint8_t sense_read_pos=0; //position of last element written to
float set_position=0;
unsigned long last_sense_ok=0; //last time sensor measured class ok
//TODO: implement timeout if last_sense_ok gets too high.
};
blindmodel blind1;
int pwm;
//Motor shield default I2C Address: 0x30 //Motor shield default I2C Address: 0x30
//PWM frequency: 1000Hz(1kHz) //PWM frequency: 1000Hz(1kHz)
@ -40,60 +87,125 @@ Motor M1(0x30, _MOTOR_A, 1000); //Motor A
Motor M2(0x30, _MOTOR_B, 1000); //Motor B Motor M2(0x30, _MOTOR_B, 1000); //Motor B
int getFitered(int* values,uint8_t size);
uint8_t classifySensorValue(blindmodel &blind);
void setup() { void setup() {
Serial.begin(57600); Serial.begin(57600);
Serial.println("Starting demo"); Serial.println("Starting");
pinMode(PIN_BUTTON, INPUT_PULLUP); button1.pin=PIN_BUTTON1;
pinMode(button1.pin, INPUT_PULLUP);
pinMode(PIN_SENSE, INPUT); pinMode(PIN_SENSE, INPUT);
M1.setmotor(_STOP); M1.setmotor(_STOP);
M2.setmotor(_STOP); M2.setmotor(_STOP);
pwm=100;
//settings for blind
blind1.length_clear=50;
blind1.length_opaque=74;
blind1.sense_clear_lower=40;
blind1.sense_clear_upper=100;
blind1.sense_opaque_lower=600;
blind1.sense_opaque_upper=750;
blind1.sense_end_lower=850;
blind1.sense_end_upper=1024;
blind1.speed_estimate=20;
} }
void loop() { void loop() {
if (millis() > last_pin_button + BUTTON_DEBOUNCE) { button1.changed=false;
bool new_pin_button_down=!digitalRead(PIN_BUTTON); if (millis() > button1.last_time_read + BUTTON_DEBOUNCE) {
if (pin_button_down != new_pin_button_down) { //changed bool new_pin_button_down=!digitalRead(button1.pin);
pin_button_down = new_pin_button_down; //update if (button1.down != new_pin_button_down) { //changed
last_pin_button=millis(); //delay next check button1.down = new_pin_button_down; //update
button1.changed=true;
button1.last_time_read=millis(); //delay next check
} }
} }
if (pin_button_down) { //button currently pressed
if (last_pin_button_down!=pin_button_down) { //value changed. executed one time when entering if //Manual movement by button
if (button1.changed) {
if (button1.down) { //changed to pressed
if (manual_drive_direction) { if (manual_drive_direction) {
M1.setmotor( _CW, pwm); M1.setmotor( _CW, 100);
Serial.print("CW PWM: "); Serial.print("CW PWM: ");
}else{ }else{
M1.setmotor( _CCW, pwm); M1.setmotor( _CCW, 100);
Serial.print("CCW PWM: "); Serial.print("CCW PWM: ");
} }
Serial.println(pwm); Serial.println(100);
manual_drive_direction=!manual_drive_direction; //switch direction every new press manual_drive_direction=!manual_drive_direction; //switch direction every new press
} }else{ //changed to released
}else{
if (last_pin_button_down!=pin_button_down) { //value changed
Serial.println("Motor STOP"); Serial.println("Motor STOP");
M1.setmotor(_STOP); M1.setmotor(_STOP);
} }
} }
last_pin_button_down=pin_button_down;
//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 + 100) { if (millis() > last_print + 100) {
int sense=analogRead(PIN_SENSE); Serial.println(blind1.sense_status);
Serial.println(sense);
last_print=millis(); last_print=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 loop() { void loop() {