recalibrate throttle and minor display fixes
This commit is contained in:
parent
b3ab5841a7
commit
e20a3eb040
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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<NUM_STANDINGDISPLAYSCREEN-1){
|
||||
standingDisplayScreen++;
|
||||
}
|
||||
|
||||
|
||||
#ifdef CALIBRATION_THROTTLE_CURVE
|
||||
if (standingDisplayScreen==(NUM_STANDINGDISPLAYSCREEN-1)) { //on last page and button A pressed
|
||||
Serial.print(ads_throttle_A_raw); Serial.print(',');
|
||||
Serial.println(ads_throttle_B_raw);
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue