implement sensor filtering

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
+#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;
 
@@ -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_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
 //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) {
+  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) {
-        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{ //changed to released
-  }else{
-    if (last_pin_button_down!=pin_button_down) { //value changed
       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(blind1.sense_status);
-    Serial.println(sense);
     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() {