implement two sensor throttle with failsafe

controller_teensy/include/definitions.h

@@ -29,23 +29,34 @@ bool controllers_connected=false;
 #define PIN_THROTTLE A7
 //const uint16_t calib_throttle_min = 420; //better a bit too high than too low
 //const uint16_t calib_throttle_max = 790;
-const uint16_t failsafe_throttle_min = 4900; //if adc value falls below this failsafe is triggered. old 20
+const uint16_t failsafe_throttle_min_A = 4900; //if adc value falls below this failsafe is triggered.
-const uint16_t failsafe_throttle_max = 14500; //if adc value goes above this failsafe is triggered. old 1000
+const uint16_t failsafe_throttle_max_A = 14500; //if adc value goes above this failsafe is triggered.
+const uint16_t failsafe_throttle_min_B = 3900; //if adc value falls below this failsafe is triggered.
+const uint16_t failsafe_throttle_max_B = 12500; //if adc value goes above this failsafe is triggered.
+const uint16_t failsafe_throttle_maxDiff = 200;//maximum adc value difference between both sensors A and B. value range 0-1000. choose value at least 2x higher than maximum difference when moving throttle slowly
 //const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel
-const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel
+//const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel. config used until 20230826
+
+const uint16_t throttleCurvePerMM_A[] = {11800,11130,10300,9990,9650,9470,9370,9240,9130,9030,8950,8850,8700,8560,8350,8040,7750,7150,6520}; //adc values for every unit (mm) of linear travel
+const bool throttleCurvePerMM_A_Descending=true; //set true if corresponding array is descending
+const uint16_t throttleCurvePerMM_B[] = {6200,6700,7420,7710,8030,8200,8310,8440,8560,8640,8740,8840,8990,9130,9330,9630,9900,10440,10990}; //adc values for every unit (mm) of linear travel
+const bool throttleCurvePerMM_B_Descending=false; //set true if corresponding array is descending
 #define PIN_BRAKE A8
 const uint16_t calib_brake_min = 2000;//better a bit too high than too low
 const uint16_t calib_brake_max = 11000;
 const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered
-const uint16_t failsafe_brake_max = 13200; //if adc value goes above this failsafe is triggered
+const uint16_t failsafe_brake_max = 13700; //if adc value goes above this failsafe is triggered
 
 uint16_t ads_throttle_A_raw=0;
 uint16_t ads_throttle_B_raw=0;
 uint16_t ads_brake_raw=failsafe_brake_min;
 uint16_t ads_control_raw=0;
 
-int16_t throttle_pos=0;
+
-int16_t brake_pos=0;
+int16_t throttle_posA; //scaled and clamped throttle position for sensor A
+int16_t throttle_posB; //scaled and clamped throttle position for sensor B
+int16_t throttle_pos=0; //combined and filtered throttle position
+int16_t brake_pos=0; //filtered throttle position
 
 unsigned long loopmillis;
 unsigned long looptime_duration;
@@ -55,12 +66,13 @@ unsigned long looptime_duration;
 #define ADCREADPERIOD 10
 #define BUTTONREADPERIOD 20
 unsigned long last_adsread=0; //needed for failcheck
-uint16_t throttle_raw=failsafe_throttle_min;  //start at min so that failsafe is not triggered
+uint16_t throttle_rawA=failsafe_throttle_min_A;  //start at min so that failsafe is not triggered
+uint16_t throttle_rawB=failsafe_throttle_min_B;  //start at min so that failsafe is not triggered
 #define THROTTLE_ADC_FILTER 0.4 //higher value = faster response
 uint16_t brake_raw=failsafe_brake_min;  //start at min so that failsafe is not triggered
-#define ADC_OUTOFRANGE_TIME 100
+#define ADC_OUTOFRANGE_TIME 100 //for failsafe_throttle_min_X. how long values need to stay bad to trigger failsafe
-unsigned long throttle_ok_time=0;
+#define ADC_DIFFHIGH_TIME 500 //for failsafe_throttle_maxDiff. how long values need to stay bad to trigger failsafe
-unsigned long brake_ok_time=0;
+
 bool error_throttle_outofrange=false;
 bool error_brake_outofrange=false;
 bool error_ads_max_read_interval=false;

controller_teensy/include/temperature.h

@@ -31,7 +31,7 @@ void initTemperature() {
   delay(1000);
 
 
-  Serial.print("Locating devices...");
+  Serial.print("Locating 1Wire devices...");
   Serial.print("Found ");
   Serial.print(sensors.getDeviceCount(), DEC);
   Serial.println(" devices.");

controller_teensy/src/main.cpp

@@ -59,7 +59,7 @@ void leds();
 
 void readButtons();
 
-uint16_t linearizeThrottle(uint16_t v);
+uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending);
 
 
 time_t getTeensy3Time();
@@ -411,15 +411,31 @@ void readADS() { //sequentially read ads and write to variable
 
 
 void readADC() {
-  /*
+  
-  Serial.print(ads_throttle_A_raw); Serial.print('\t'); 
+  //Serial.print(ads_throttle_A_raw); Serial.print('\t'); 
-  Serial.print(ads_throttle_B_raw); Serial.print('\t'); 
+  //Serial.print(ads_throttle_B_raw); Serial.print('\t'); 
-  Serial.print(ads_brake_raw); Serial.print('\t'); 
+  //Serial.print(ads_brake_raw); Serial.print('\t'); 
-  Serial.print(ads_control_raw); Serial.println();*/
+  //Serial.print(ads_control_raw); Serial.println();
-  throttle_raw = (ads_throttle_A_raw+ads_throttle_B_raw)/2.0*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter
+  //throttle_raw = (ads_throttle_A_raw+ads_throttle_B_raw)/2.0*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter
+
+  throttle_rawA=ads_throttle_A_raw;
+  throttle_rawB=ads_throttle_B_raw;
+
 
   //maps throttle curve to be linear
-  throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain
+  //throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain
+  throttle_posA=max(0,min(1000, linearizeThrottle(ads_throttle_A_raw, throttleCurvePerMM_A, sizeof(throttleCurvePerMM_A)/sizeof(throttleCurvePerMM_A[0]), throttleCurvePerMM_A_Descending ) )); //map and constrain
+  throttle_posB=max(0,min(1000, linearizeThrottle(ads_throttle_B_raw, throttleCurvePerMM_B, sizeof(throttleCurvePerMM_B)/sizeof(throttleCurvePerMM_B[0]), throttleCurvePerMM_B_Descending ) )); //map and constrain
+  
+  //Serial.print(throttle_posA); Serial.print('\t'); 
+  //Serial.print(throttle_posB); Serial.print('\t'); 
+  
+  int16_t _throttlediff = (int)throttle_posA-(int)throttle_posB;
+  int16_t throttle_posMean = (throttle_posA+throttle_posB)/2.0;   
+  
+  
+  throttle_pos = throttle_posMean*THROTTLE_ADC_FILTER + throttle_pos*(1-THROTTLE_ADC_FILTER); //apply filter
+ 
     
   brake_raw=ads_brake_raw;
   brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
@@ -500,23 +516,39 @@ void failChecks() {
   controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();
 
   //ADC Range Check
-  if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
+  static unsigned long throttle_ok_time=0;
+  if ((ads_throttle_A_raw >= failsafe_throttle_min_A) & (ads_throttle_A_raw <= failsafe_throttle_max_A) & (ads_throttle_B_raw >= failsafe_throttle_min_B) & (ads_throttle_B_raw <= failsafe_throttle_max_B)) { //inside safe range (to check if 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(loopmillis, "Error Throttle ADC Out of Range");      
+      writeLogComment(loopmillis, "Error Throttle ADC Out of Range. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw);
     }
     //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
   }
+
+  static unsigned long throttlediff_ok_time=0;
+  if (abs(throttle_posA-throttle_posB) <= failsafe_throttle_maxDiff) { //inside safe range (to check if wire got disconnected)
+    throttlediff_ok_time=loopmillis;
+  }
+  if (loopmillis>throttlediff_ok_time+ADC_DIFFHIGH_TIME) { //not ok for too long
+    if (!error_throttle_outofrange) {
+      error_throttle_outofrange=true;
+      writeLogComment(loopmillis, "Error Throttle Diff too High. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw);
+    }
+    //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
+  }
+
+
+  static unsigned long brake_ok_time=0;
   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(loopmillis, "Error Brake ADC Out of Range");
+      writeLogComment(loopmillis, "Error Brake ADC Out of Range. ADC="+(String)brake_raw);
     }
     //Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw);
   }
@@ -771,27 +803,31 @@ void readButtons() {
 
 }
 
-uint16_t linearizeThrottle(uint16_t v) {
+
+uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending) {
   //input is raw adc value from hall sensor
-  //uses throttleCurvePerMM array to find linear approximation of actual throttle travel
+  //uses pthrottleCurvePerMM array to find linear approximation of actual throttle travel
-  //array has to be sorted !
+  //array has to be sorted ! if sorteddescending=false then sorted ascending, if true then array should be sorted descending
   uint8_t _searchpos=0;
-  uint8_t arraysize = sizeof(throttleCurvePerMM)/sizeof(throttleCurvePerMM[0]);
+  //uint8_t arraysize = sizeof(pthrottleCurvePerMM)/sizeof(pthrottleCurvePerMM[0]);
-  while (_searchpos < arraysize && v>throttleCurvePerMM[_searchpos]) { //find arraypos with value above input value
+  while (_searchpos < arraysize && v>pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)]) { //find arraypos with value above input value
     _searchpos++; //try next value
   }
 
   if (_searchpos <=0) { //lower limit
-    return 0;
+    return (sorteddescending?1000:0);
   }
   if (_searchpos >= arraysize) { //upper limit
-    return 1000;
+    return (sorteddescending?0:1000);
   }
 
-  uint16_t nextLower=throttleCurvePerMM[_searchpos-1];
+  uint16_t nextLower=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?0:1)];
-  uint16_t nextHigher=throttleCurvePerMM[_searchpos];
+  uint16_t nextHigher=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?1:0)];
   float _linearThrottle = _searchpos+map(v*1.0,nextLower,nextHigher,0.0,1.0);
   _linearThrottle/=arraysize; //scale to 0-1
   _linearThrottle*=1000; //scale to 0-1000
+  if (sorteddescending){
+    _linearThrottle=1000-_linearThrottle; //invert result
+  }
   return (uint16_t)_linearThrottle;
 }