add deviation error check

src/main.cpp

@@ -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 = MODE_FIND_END;
-  blind2.mode_find_end_state=0; //reset mode find state
+  //blind2.mode_find_end_state=0; //reset mode find state
+
+
+  
   
 }
 
@@ -189,8 +211,27 @@ void loop() {
 
 
   //Read sensor/encoder
-  readSensor(blind1);
+  if (millis() > last_sensor_led_switch + SENSOR_SWITCH_INTERVAL) {
-  readSensor(blind2);
+    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);
+}