From 2586b95bbc0778fc561a6528319f0608ae16efb9 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 27 Aug 2023 18:16:51 +0200 Subject: [PATCH] implement two sensor throttle with failsafe --- controller_teensy/include/definitions.h | 32 +++++++---- controller_teensy/include/temperature.h | 2 +- controller_teensy/src/main.cpp | 76 ++++++++++++++++++------- 3 files changed, 79 insertions(+), 31 deletions(-) diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index a961335..f490f7f 100644 --- a/controller_teensy/include/definitions.h +++ b/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_max = 14500; //if adc value goes above this failsafe is triggered. old 1000 +const uint16_t failsafe_throttle_min_A = 4900; //if adc value falls below this failsafe is triggered. +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 -unsigned long throttle_ok_time=0; -unsigned long brake_ok_time=0; +#define ADC_OUTOFRANGE_TIME 100 //for failsafe_throttle_min_X. how long values need to stay bad to trigger failsafe +#define ADC_DIFFHIGH_TIME 500 //for failsafe_throttle_maxDiff. how long values need to stay bad to trigger failsafe + bool error_throttle_outofrange=false; bool error_brake_outofrange=false; bool error_ads_max_read_interval=false; diff --git a/controller_teensy/include/temperature.h b/controller_teensy/include/temperature.h index e21201d..681e4f6 100644 --- a/controller_teensy/include/temperature.h +++ b/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."); diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 1e9a9ef..acad2c3 100644 --- a/controller_teensy/src/main.cpp +++ b/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_B_raw); Serial.print('\t'); - Serial.print(ads_brake_raw); Serial.print('\t'); - 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 + + //Serial.print(ads_throttle_A_raw); Serial.print('\t'); + //Serial.print(ads_throttle_B_raw); Serial.print('\t'); + //Serial.print(ads_brake_raw); Serial.print('\t'); + //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_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 - //array has to be sorted ! + //uses pthrottleCurvePerMM array to find linear approximation of actual throttle travel + //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]); - while (_searchpos < arraysize && v>throttleCurvePerMM[_searchpos]) { //find arraypos with value above input value + //uint8_t arraysize = sizeof(pthrottleCurvePerMM)/sizeof(pthrottleCurvePerMM[0]); + 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 nextHigher=throttleCurvePerMM[_searchpos]; + uint16_t nextLower=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?0:1)]; + 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; } \ No newline at end of file