implement sensor filtering
This commit is contained in:
parent
90e160228f
commit
ea71f7dd2c
168
src/main.cpp
168
src/main.cpp
|
|
@ -6,11 +6,20 @@
|
|||
|
||||
//follow firmware flash guide for new wemos motor shield v1.0 https://github.com/thomasfredericks/wemos_motor_shield
|
||||
|
||||
|
||||
#define BUTTON_DEBOUNCE 200
|
||||
#define PIN_BUTTON D5
|
||||
unsigned long last_pin_button=0;
|
||||
bool pin_button_down=0;
|
||||
bool last_pin_button_down=0;
|
||||
#define PIN_BUTTON1 D5
|
||||
struct button{
|
||||
uint8_t pin;
|
||||
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;
|
||||
|
||||
|
|
@ -26,13 +35,51 @@ unsigned long last_print=0;
|
|||
#define SENSE_END_UPPER 1024 //adc value upper limit for end marker
|
||||
|
||||
//define directions for motors
|
||||
#define _M1_UP = _CCW;
|
||||
#define _M1_DOWN = _CW;
|
||||
#define _M1_UP _CCW;
|
||||
#define _M1_DOWN _CW;
|
||||
|
||||
#define _M2_UP = _CCW;
|
||||
#define _M2_DOWN = _CW;
|
||||
#define _M2_UP _CCW;
|
||||
#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
|
||||
//PWM frequency: 1000Hz(1kHz)
|
||||
|
|
@ -40,60 +87,125 @@ Motor M1(0x30, _MOTOR_A, 1000); //Motor A
|
|||
Motor M2(0x30, _MOTOR_B, 1000); //Motor B
|
||||
|
||||
|
||||
|
||||
int getFitered(int* values,uint8_t size);
|
||||
uint8_t classifySensorValue(blindmodel &blind);
|
||||
|
||||
void setup() {
|
||||
|
||||
|
||||
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);
|
||||
|
||||
M1.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() {
|
||||
|
||||
if (millis() > last_pin_button + BUTTON_DEBOUNCE) {
|
||||
bool new_pin_button_down=!digitalRead(PIN_BUTTON);
|
||||
if (pin_button_down != new_pin_button_down) { //changed
|
||||
pin_button_down = new_pin_button_down; //update
|
||||
last_pin_button=millis(); //delay next check
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
M1.setmotor( _CW, pwm);
|
||||
M1.setmotor( _CW, 100);
|
||||
Serial.print("CW PWM: ");
|
||||
}else{
|
||||
M1.setmotor( _CCW, pwm);
|
||||
M1.setmotor( _CCW, 100);
|
||||
Serial.print("CCW PWM: ");
|
||||
}
|
||||
Serial.println(pwm);
|
||||
Serial.println(100);
|
||||
manual_drive_direction=!manual_drive_direction; //switch direction every new press
|
||||
}
|
||||
}else{
|
||||
if (last_pin_button_down!=pin_button_down) { //value changed
|
||||
}else{ //changed to released
|
||||
Serial.println("Motor 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) {
|
||||
int sense=analogRead(PIN_SENSE);
|
||||
Serial.println(sense);
|
||||
Serial.println(blind1.sense_status);
|
||||
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() {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue