From ea71f7dd2ccab07827c3915f3b2616f9bc08c089 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 31 Jan 2021 17:59:57 +0100 Subject: [PATCH] implement sensor filtering --- src/main.cpp | 168 ++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 140 insertions(+), 28 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 44dcbb9..db18bd9 100644 --- a/src/main.cpp +++ b/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=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() {