From e20a3eb040fd535ae875973b468d91a40cd83b4c Mon Sep 17 00:00:00 2001 From: Fisch Date: Mon, 22 Jul 2024 14:29:36 +0200 Subject: [PATCH] recalibrate throttle and minor display fixes --- controller_teensy/include/definitions.h | 16 +++---- controller_teensy/include/display.h | 53 +++++++++++++++++++--- controller_teensy/src/main.cpp | 58 ++++++++++++++----------- 3 files changed, 86 insertions(+), 41 deletions(-) diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index 8689efb..cc91fd5 100644 --- a/controller_teensy/include/definitions.h +++ b/controller_teensy/include/definitions.h @@ -41,10 +41,12 @@ const uint16_t failsafe_throttle_max_A = 13000; //if adc value goes above this f const uint16_t failsafe_throttle_min_B = 5000; //if adc value falls below this failsafe is triggered. const uint16_t failsafe_throttle_max_B = 14500; //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 failsafe_throttle_maxDiff = 200;//maximum throttle pos value difference between both sensors A and B (after linearization). value range 0-1000. choose value at least 2x higher than maximum difference when moving throttle slowly + +//Throttle Calibration: First value pair should be 1-2mm pressed down (start of throttle=1). Last value pair should be a bit before fully pressed. all values in between need to be equidistant (for example every 1mm). Use define CALIBRATION_THROTTLE_CURVE for easy calibration output +const uint16_t throttleCurvePerMM_A[] = {9996,9718,9507,9357,9232,9162,9071,8958,8838,8703,8577,8396,8192,7845,7407,6800,5923}; //adc values for every unit (mm) of linear travel +const uint16_t throttleCurvePerMM_B[] = {7698,7948,8133,8277,8403,8503,8588,8695,8791,8899,9034,9196,9400,9711,10110,10657,1146}; //adc values for every unit (mm) of linear travel -const uint16_t throttleCurvePerMM_A[] = {10000,9610,9590,9400,9250,9160,9070,8970,8860,8750,8610,8435,8230,7960,7520, 7000, 6340, 5230}; //adc values for every unit (mm) of linear travel -const uint16_t throttleCurvePerMM_B[] = {7730, 8060,8070,8250,8380,8490,8580,8670,8780,8870,9000,9160,9350,9610,10020,10500,11120,12160}; //adc values for every unit (mm) of linear travel const bool throttleCurvePerMM_A_Descending=true; //set true if corresponding array is descending const bool throttleCurvePerMM_B_Descending=false; //set true if corresponding array is descending @@ -65,12 +67,10 @@ uint16_t ads_brake_raw=failsafe_brake_min; uint16_t ads_control_raw=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 +int16_t brake_pos=0; //filtered and constrained throttle position bool control_buttonA=false; bool control_buttonB=false; @@ -84,10 +84,8 @@ unsigned long looptime_duration_max; #define ADCREADPERIOD 10 #define BUTTONREADPERIOD 20 unsigned long last_adsread=0; //needed for failcheck -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 +int16_t brake_linear=0; //linearized bake position #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 diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index aa6d64d..80e93bd 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -16,7 +16,7 @@ Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); uint8_t standingDisplayScreen=0; -#define NUM_STANDINGDISPLAYSCREEN 3 +#define NUM_STANDINGDISPLAYSCREEN 4 #define DISPLAYSTANDSTILLTIME 5000 @@ -328,17 +328,21 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc //Row2 display.print(F("Throttle ")); - dtostrf(ads_throttle_A_raw,1,0,buf); + dtostrf((throttle_posA+throttle_posB)/2.0,4,0,buf); display.print((String)buf); - display.print(F("/")); - dtostrf(ads_throttle_B_raw,1,0,buf); + display.print(F(" (")); + if (throttle_posA>throttle_posB) { //use values acutally taken for diff check + display.print(F("+")); //add + for better formatting + } + dtostrf((throttle_posA-throttle_posB),1,0,buf); display.print((String)buf); + display.print(F(")")); display.println(); //Row3 display.print("Brake "); - dtostrf(ads_brake_raw,1,0,buf); + dtostrf(brake_linear,4,0,buf); //use only linearized values (not constrained) display.print((String)buf); display.setCursor(8*10,2*8); @@ -347,7 +351,7 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc display.print(F("V")); display.println(); - //TODO: show deviation (and max deviation), show resulting throttle and brake pos + /*display.print(" c="); @@ -401,6 +405,43 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc break; + case 3: + //Raw inputs and Input Debug + + //TODO: show deviation (and max deviation), show resulting throttle and brake pos + + + //Row1 + + display.print(F("Throttle ")); + dtostrf(ads_throttle_A_raw,1,0,buf); + display.print((String)buf); + display.print(F("/")); + dtostrf(ads_throttle_B_raw,1,0,buf); + display.print((String)buf); + display.println(); + + //Row2 + static int16_t _throttle_maxdiff = 0; + if (control_buttonA || control_buttonB) { //any button press + _throttle_maxdiff=0; //reset maxdiff for this display + } + _throttle_maxdiff=max(_throttle_maxdiff,abs(throttle_posA-throttle_posB)); + display.print(F("Thr.maxdiff ")); + dtostrf(_throttle_maxdiff,1,0,buf); + display.print((String)buf); + display.println(); + + //Row3 + + display.print("Brake "); + dtostrf(ads_brake_raw,1,0,buf); + display.print((String)buf); + display.println(); + + + break; + } //Put User Inputs in last row diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index cf4d14b..8df6acf 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -25,7 +25,7 @@ bool getDatalogging(); #include "ADS1X15.h" - +//#define CALIBRATION_THROTTLE_CURVE //uncomment to enable throttleA and B serial output on button press @@ -382,24 +382,9 @@ 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 - - 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_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'); + throttle_posA=max(0,min(1000, linearizeThrottle(ads_throttle_A_raw, throttleCurvePerMM_A, sizeof(throttleCurvePerMM_A)/sizeof(throttleCurvePerMM_A[0]), throttleCurvePerMM_A_Descending ) )); //linearize and constrain (linear function already constrains) + throttle_posB=max(0,min(1000, linearizeThrottle(ads_throttle_B_raw, throttleCurvePerMM_B, sizeof(throttleCurvePerMM_B)/sizeof(throttleCurvePerMM_B[0]), throttleCurvePerMM_B_Descending ) )); //linearize and constrain (linear function already constrains + int16_t throttle_posMean = (throttle_posA+throttle_posB)/2.0; @@ -407,8 +392,8 @@ void readADC() { 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 + brake_linear=map(ads_brake_raw,calib_brake_min,calib_brake_max,0,1000); + brake_pos=max(0,min(1000,brake_linear)); //map and constrain //brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000); @@ -457,6 +442,7 @@ void failChecks() { 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; @@ -465,10 +451,12 @@ void failChecks() { //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; } +#ifndef CALIBRATION_THROTTLE_CURVE //do not check if throttle calibration enabled if (loopmillis>throttlediff_ok_time+ADC_DIFFHIGH_TIME) { //not ok for too long if (!error_throttle_difftoohigh) { error_throttle_difftoohigh=true; @@ -476,16 +464,17 @@ void failChecks() { } //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw); } +#endif 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 + if ((ads_brake_raw >= failsafe_brake_min) & (ads_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. ADC="+(String)brake_raw); + writeLogComment(loopmillis, "Error Brake ADC Out of Range. ADC="+(String)ads_brake_raw); } //Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw); } @@ -559,16 +548,20 @@ void calculateSetSpeed(unsigned long timediff){ - +#ifdef DRIVING_TANKSTEERING // ## Experimental Tank steering + /* Notes from Drivetest 20240722 + When driving slow and pressing button steering wheel is turned more violently. + When driving faster (>10km/h) steering wheel is not forced as strong, but bobbycar turns better around itself. + As cmd increase in higher ranges is more pronounces in motor torque, the same tanksteering_max_speed=200 is way too strong when driving >20km/h, but okay when driving <10km/h. + Disabled this function for now, because if button press is triggered erroneously during fast driving it could be dangerous. Steering manually against it is not possible. + */ static float tanksteering_differential=0; //to ramp up slowly. value between -1.0 and 1.0 //Parameters: int16_t tanksteering_max_speed=200; float tanksteering_rate_increase=2.0; //increase units per second float tanksteering_rate_decrease=3.0; //decrease units per second - - if (control_buttonA && !control_buttonB && !reverse_enabled && throttle_pos>0) { //Right button (A) only. and throttle touched tanksteering_differential+=tanksteering_rate_increase*(timediff/1000.0); tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1 @@ -584,10 +577,12 @@ void calculateSetSpeed(unsigned long timediff){ tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1 } + cmd_send_toMotor_FL+=tanksteering_differential*tanksteering_max_speed; cmd_send_toMotor_FR-=tanksteering_differential*tanksteering_max_speed; cmd_send_toMotor_RL+=tanksteering_differential*tanksteering_max_speed; cmd_send_toMotor_RR-=tanksteering_differential*tanksteering_max_speed; +#endif @@ -717,6 +712,7 @@ void readButtons() { writeLogComment(loopmillis, "Disarmed by button"); } if (button_start_longpress_flag) { +#ifndef CALIBRATION_THROTTLE_CURVE //disable arming when throttle calibration enabled (because errorcheck is disabled) if (escFront.getControllerConnected() && escRear.getControllerConnected()) { armed=true; //arm if button pressed long enough writeLogComment(loopmillis, "Armed by button"); @@ -737,6 +733,7 @@ void readButtons() { }else{ writeLogComment(loopmillis, "Unable to arm"); } +#endif } @@ -766,6 +763,15 @@ void readADSButtons() { if (standingDisplayScreen