interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

implement two sensor throttle with failsafe

This commit is contained in:
interfisch 2023-08-27 18:16:51 +02:00
parent 6add81cf69
commit 2586b95bbc
3 changed files with 79 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
controller_teensy/include/definitions.h
View File

@ -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;

2
controller_teensy/include/temperature.h
View File

@ -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.");

76
controller_teensy/src/main.cpp
View File

@ -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;
}