add adc failsafe check

controller_teensy/src/main.cpp

@@ -36,15 +36,26 @@ bool controllers_connected=false;
 #define PIN_THROTTLE A7
 const uint16_t calib_throttle_min = 400; //better a bit too high than too low
 const uint16_t calib_throttle_max = 790;
+const uint16_t failsafe_throttle_min = 20; //if adc value falls below this failsafe is triggered
+const uint16_t failsafe_throttle_max = 1000; //if adc value goes above this failsafe is triggered
 #define PIN_BRAKE A8
 const uint16_t calib_brake_min = 100;//better a bit too high than too low
 const uint16_t calib_brake_max = 600;
+const uint16_t failsafe_brake_min = 20; //if adc value falls below this failsafe is triggered
+const uint16_t failsafe_brake_max = 1000; //if adc value goes above this failsafe is triggered
 
 int16_t throttle_pos=0;
 int16_t brake_pos=0;
 
 unsigned long last_adcread=0;
 #define ADCREADPERIOD 10
+uint16_t throttle_raw=0; 
+uint16_t brake_raw=0;
+#define ADC_OUTOFRANGE_TIME 100
+unsigned long throttle_ok_time=0;
+unsigned long brake_ok_time=0;
+bool error_throttle_outofrange=false;
+bool error_brake_outofrange=false;
 
 #define PIN_START A9
 #define PIN_LED_START 2 //Enginge start led
@@ -137,6 +148,7 @@ void readADC();
 void failChecks();
 void sendCMD();
 void checkLog();
+void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
 
 void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
 {
@@ -251,6 +263,13 @@ void loop() {
   bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
   bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
 
+  if (newData2) {
+    updateMotorparams(motorparamsFront,FeedbackFront);
+  }
+  if (newData3) {
+    updateMotorparams(motorparamsRear,FeedbackRear);
+  }
+
   
   if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
     last_adcread=loopmillis;
@@ -260,37 +279,20 @@ void loop() {
 
   failChecks();  
 
-  
-  
-
-  if (newData2) {
-    motorparamsFront.cur_pos++;
-    motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
-    motorparamsFront.curL_DC[motorparamsFront.cur_pos] = -FeedbackFront.curL_DC; //invert so positive current is consumed current. negative then means regenerated
-    motorparamsFront.curR_DC[motorparamsFront.cur_pos] = -FeedbackFront.curR_DC;
-    motorparamsFront.millis=loopmillis;
-    log_update=true;
-  }
-  if (newData3) {
-    motorparamsRear.cur_pos++;
-    motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
-    motorparamsRear.curL_DC[motorparamsRear.cur_pos] = -FeedbackRear.curL_DC;
-    motorparamsRear.curR_DC[motorparamsRear.cur_pos] = -FeedbackRear.curR_DC;
-    motorparamsRear.millis=loopmillis;
-    log_update=true;
-  }
-
   if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers    
     last_send=loopmillis;
     sendCMD();    
   }
 
-
+  //If needed write log to serial port
-  // ######## LOG ########
-
   checkLog();
+
 }
 
+
+// ##### HELPFUNCTIONS
+
+
 int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
 {
   float a = *((float *)cmp1);
@@ -360,10 +362,13 @@ void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
 
 
 
+// #### LOOPFUNCTIONS
+
+
 void readADC() {
-  uint16_t throttle_raw = analogRead(PIN_THROTTLE);
+  throttle_raw = analogRead(PIN_THROTTLE);
   throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
-    uint16_t brake_raw = analogRead(PIN_BRAKE);
+  brake_raw = analogRead(PIN_BRAKE);
   brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
 
   int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
@@ -406,7 +411,29 @@ void failChecks() {
 
   controllers_connected=controllerFront_connected & controllerRear_connected;
 
-  if (!controllers_connected) { //controllers not available
+  //ADC Range Check
+  if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //outside safe range. maybe wire got disconnected
+    throttle_ok_time=loopmillis;
+  }
+  if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
+    if (!error_throttle_outofrange) {
+      error_throttle_outofrange=true;
+      writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
+    }
+  }
+  if ((brake_raw >= failsafe_brake_min) & (brake_raw <= failsafe_brake_max)) { //outside safe range. maybe wire got disconnected
+    brake_ok_time=loopmillis;
+  }
+  if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
+    if(!error_brake_outofrange) {
+      error_brake_outofrange=true;
+      writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
+    }
+  }
+
+
+
+  if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange) { //controllers not available
     throttle_pos=0;
     brake_pos=0;
   }
@@ -473,3 +500,12 @@ void checkLog() {
     writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
   }
 }
+
+void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
+  mp.cur_pos++;
+  mp.cur_pos%=CURRENT_FILTER_SIZE;
+  mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
+  mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
+  mp.millis=loopmillis;
+  log_update=true;
+}