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

recalibrate throttle and minor display fixes

This commit is contained in:
interfisch 2024-07-22 14:29:36 +02:00
parent b3ab5841a7
commit e20a3eb040
3 changed files with 86 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
controller_teensy/include/definitions.h
View File

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

53
controller_teensy/include/display.h
View File

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

58
controller_teensy/src/main.cpp
View File

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