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

add driving parameter change via buttons and button display screen toggle

This commit is contained in:
interfisch 2024-03-02 21:55:40 +01:00
parent 87a65ccde5
commit ecbb3bb491
3 changed files with 181 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
controller_teensy/include/definitions.h
View File

@ -47,16 +47,27 @@ const uint16_t calib_brake_max = 11000;
const uint16_t failsafe_brake_min = 4000; //if adc value falls below this failsafe is triggered const uint16_t failsafe_brake_min = 4000; //if adc value falls below this failsafe is triggered
const uint16_t failsafe_brake_max = 14500; //if adc value goes above this failsafe is triggered const uint16_t failsafe_brake_max = 14500; //if adc value goes above this failsafe is triggered
const uint16_t calib_control_buttonA=7170; // adc value for button A pressed
const uint16_t calib_control_buttonB=10402; // adc value for button B pressed
const uint16_t calib_control_buttonAB=5595; // adc value for button A and B pressed
const uint16_t calib_control_treshold=100; // from calibration value adc-thres to adc+thres
const uint16_t calib_control_max=13000; //over which adc value button are both released
uint16_t ads_throttle_A_raw=0; uint16_t ads_throttle_A_raw=0;
uint16_t ads_throttle_B_raw=0; uint16_t ads_throttle_B_raw=0;
uint16_t ads_brake_raw=failsafe_brake_min; uint16_t ads_brake_raw=failsafe_brake_min;
uint16_t ads_control_raw=0; uint16_t ads_control_raw=0;
int16_t throttle_posA; //scaled and clamped throttle position for sensor A 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_posB; //scaled and clamped throttle position for sensor B
int16_t throttle_pos=0; //combined and filtered throttle position int16_t throttle_pos=0; //combined and filtered throttle position
int16_t brake_pos=0; //filtered throttle position int16_t brake_pos=0; //filtered throttle position
bool control_buttonA=false;
bool control_buttonB=false;
unsigned long loopmillis; unsigned long loopmillis;
unsigned long looptime_duration; unsigned long looptime_duration;
@ -80,7 +91,7 @@ bool error_ads_max_read_interval=false;
bool error_sdfile_unavailable=false; bool error_sdfile_unavailable=false;
#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward #define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward
#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1
#define NORMAL_MAX_ACCELERATION_RATE 10000 #define NORMAL_MAX_ACCELERATION_RATE 10000
#define SLOW_MAX_ACCELERATION_RATE 500 #define SLOW_MAX_ACCELERATION_RATE 500
@ -92,6 +103,9 @@ int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant
int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling) int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
float startbrakecurrent=2; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor float startbrakecurrent=2; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
float startbrakecurrent_offset=0.13; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading float startbrakecurrent_offset=0.13; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average)
float reverse_speed=0.25; //reverse driving speed //0 to 1
int16_t throttle_max=1000; //maximum allowed set speed. used for scaling and limiting. [0,1000]
bool reverse_enabled=false; bool reverse_enabled=false;
unsigned long last_notidle=0; //not rolling to fast, no pedal pressed unsigned long last_notidle=0; //not rolling to fast, no pedal pressed

157
controller_teensy/include/display.h
View File

@ -85,8 +85,11 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){
if (loopmillis-last_notidle>5000) { if (loopmillis-last_notidle>5000) {
display_standingDisplay(escFront,escRear); display_standingDisplay(escFront,escRear);
}else{ }else{
display_drivingDisplay(escFront,escRear); if (!control_buttonA) {
//display_debugDisplay(escFront,escRear); display_drivingDisplay(escFront,escRear);
}else{
display_debugDisplay(escFront,escRear);
}
} }
}else{ }else{
@ -147,7 +150,8 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
//## Trip / Current Consumed Display //## Trip / Current Consumed Display
display.setCursor(1,SCREEN_HEIGHT-7); display.setCursor(1,SCREEN_HEIGHT-7);
if (((millis()/2500)%2)==0) { //if (((millis()/2500)%2)==0) {
if (control_buttonB) {
//## Speed statistic //## Speed statistic
display.print("max: "); display.print("max: ");
dtostrf(max_meanSpeed*3.6,1,0,buf); dtostrf(max_meanSpeed*3.6,1,0,buf);
@ -181,7 +185,7 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
//Debug //Debug
display.setTextSize(1); // Normal 1:1 pixel scale display.setTextSize(2); // Normal 1:1 pixel scale
display.setFont(); display.setFont();
display.setTextColor(SSD1306_WHITE); // Draw white text display.setTextColor(SSD1306_WHITE); // Draw white text
char buf[8]; char buf[8];
@ -190,6 +194,32 @@ void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
dtostrf(filtered_currentAll,1,3,buf); dtostrf(filtered_currentAll,1,3,buf);
display.print((String)buf); display.print((String)buf);
display.print(" A > "); display.print(" A > ");
display.println();
//all wheel getCMD
display.setTextSize(1);
display.setCursor(25,SCREEN_HEIGHT-8*2);
display.print(escFront.getCmdL());
display.setCursor(SCREEN_WIDTH-4*7-25,SCREEN_HEIGHT-8*2);
display.print(escFront.getCmdR());
display.setCursor(25,SCREEN_HEIGHT-8*1);
display.print(escRear.getCmdL());
display.setCursor(SCREEN_WIDTH-4*7-25,SCREEN_HEIGHT-8*1);
display.print(escRear.getCmdR());
/*
display.setTextSize(1);
display.setCursor(1,SCREEN_HEIGHT-8*2);
display.print("minCMD=");
display.print(min(min(min(escFront.getCmdL(),escFront.getCmdR()),escRear.getCmdL()),escRear.getCmdR()));
display.print(" thrpos=");
display.print(throttle_pos);
*/
} }
@ -197,55 +227,86 @@ void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
double _displaytrip=trip; double _displaytrip=trip;
//double _displaycurrent=currentConsumed; //double _displaycurrent=currentConsumed;
double _displaywatthours=watthoursConsumed; double _displaywatthours=watthoursConsumed;
bool _displayOverall= ((millis()/3000)%2==0); //bool _displayOverall= ((millis()/3000)%2==0); //switch based on time
if (_displayOverall) { //alternate between this trip and overall trip bool _displayOverall=control_buttonB; //switch with button
_displaytrip=overallTrip; bool _displayParameters=control_buttonA;
//_displaycurrent=overallCurrentConsumed;
_displaywatthours=overallWatthoursConsumed;
}
char buf[8]; char buf[8];
display.setFont(); display.setFont();
display.setCursor(0,0); display.setCursor(0,0);
display.print(F("Vbat:")); display.print(escFront.getFeedback_batVoltage()); if (_displayParameters) {
display.print(F("/")); display.print(escRear.getFeedback_batVoltage());
display.print(" V"); display.print(F("cmdred min=")); display.print(minimum_constant_cmd_reduce);
display.println(); display.print(F(" p=")); display.print(brake_cmdreduce_proportional);
display.println();
//display.print(F("Temp:")); display.print(escFront.getFeedback_boardTemp());
//display.print(F("/")); display.print(escRear.getFeedback_boardTemp());
display.print(F("T:")); display.print(temp_ESCFront,0);
display.print(F("/")); display.print(temp_ESCRear,0);
display.print(F("/")); display.print(temp_Air,0);
display.print(" C");
display.println();
display.print(F("Trip:")); display.print(F("brkI="));
dtostrf(_displaytrip,1,0,buf); dtostrf(startbrakecurrent,1,1,buf);
display.print((String)buf); display.print((String)buf);
display.print("m "); display.print("A");
//dtostrf(_displaycurrent,1,2,buf); display.print(F(" free="));
dtostrf(_displaywatthours,1,2,buf); dtostrf(freewheel_break_factor,1,1,buf);
display.print((String)buf); display.print((String)buf);
//display.print(" Ah"); display.println();
display.print("Wh");
display.println();
display.print(F("")); display.print(F("thrmax=")); display.print(throttle_max);
//dtostrf( _displaytrip/1000/_displaycurrent ,1,2,buf); display.print(F(" rev="));
dtostrf( _displaywatthours/_displaytrip*100,1,2,buf); dtostrf(reverse_speed,1,2,buf);
display.print((String)buf); display.print((String)buf);
//display.print(" km/Ah"); display.println();
display.print(" kWh/100km");
}else{
if (_displayOverall) { //alternate between this trip and overall trip
_displaytrip=overallTrip;
//_displaycurrent=overallCurrentConsumed;
_displaywatthours=overallWatthoursConsumed;
}
display.print(F("Vbat:")); display.print(escFront.getFeedback_batVoltage());
display.print(F("/")); display.print(escRear.getFeedback_batVoltage());
display.print(" V");
display.println();
//display.print(F("Temp:")); display.print(escFront.getFeedback_boardTemp());
//display.print(F("/")); display.print(escRear.getFeedback_boardTemp());
display.print(F("T:")); display.print(temp_ESCFront,0);
display.print(F("/")); display.print(temp_ESCRear,0);
display.print(F("/")); display.print(temp_Air,0);
display.print(" C");
display.println();
display.print(F("Trip:"));
dtostrf(_displaytrip,1,0,buf);
display.print((String)buf);
display.print("m ");
//dtostrf(_displaycurrent,1,2,buf);
dtostrf(_displaywatthours,1,2,buf);
display.print((String)buf);
//display.print(" Ah");
display.print("Wh");
display.println();
display.print(F(""));
//dtostrf( _displaytrip/1000/_displaycurrent ,1,2,buf);
dtostrf( _displaywatthours/_displaytrip*100,1,2,buf);
display.print((String)buf);
//display.print(" km/Ah");
display.print(" kWh/100km");
if (_displayOverall){
display.print(" sum");
}
display.println();
if (_displayOverall){
display.print(" sum");
} }
display.println();
} }
@ -279,6 +340,18 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc
display.print("brake="); display.print("brake=");
dtostrf(ads_brake_raw,1,0,buf); dtostrf(ads_brake_raw,1,0,buf);
display.print((String)buf); display.print((String)buf);
/*display.print(" c=");
dtostrf(ads_control_raw,1,0,buf);
display.print((String)buf); */
if (control_buttonA){
display.print(" A");
}
if (control_buttonB){
display.print(" B");
}
} }

60
controller_teensy/src/main.cpp
View File

@ -58,6 +58,7 @@ void checkLog();
void leds(); void leds();
void readButtons(); void readButtons();
void readADSButtons();
uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending); uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending);
@ -245,6 +246,7 @@ void loop() {
if (loopmillis - last_buttonread > BUTTONREADPERIOD) { //read digital input states if (loopmillis - last_buttonread > BUTTONREADPERIOD) { //read digital input states
last_buttonread=loopmillis; last_buttonread=loopmillis;
readButtons(); readButtons();
readADSButtons();
} }
@ -396,7 +398,7 @@ void readADS() { //sequentially read ads and write to variable
case 2: //Brake case 2: //Brake
ads_brake_raw=ads_val; ads_brake_raw=ads_val;
break; break;
case 3: //Buttons TODO case 3: //Buttons
ads_control_raw=ads_val; ads_control_raw=ads_val;
break; break;
} }
@ -455,6 +457,9 @@ void readADC() {
throttle_pos=constrain(throttlebreak_pos,0,1000); throttle_pos=constrain(throttlebreak_pos,0,1000);
brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos
//throttlemax=750 - cmd ist aber 543
//throttlemax=250 - cmd ist aber 117
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", "); //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println(); //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
@ -571,6 +576,8 @@ void failChecks() {
void calculateSetSpeed(unsigned long timediff){ void calculateSetSpeed(unsigned long timediff){
int16_t adjusted_throttle_pos=constrain(throttle_pos*(throttle_max/1000.0),0,throttle_max);
int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000); int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);
float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000) float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
@ -578,9 +585,7 @@ void calculateSetSpeed(unsigned long timediff){
int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*timediff/1000)); //reduce cmd value every cycle int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*timediff/1000)); //reduce cmd value every cycle
float freewheel_current=startbrakecurrent_offset-brake_pos_expo*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling float freewheel_current=startbrakecurrent_offset-brake_pos_expo*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average)
float filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR()); float filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR());
float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_curR()); float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_curR());
@ -588,7 +593,7 @@ void calculateSetSpeed(unsigned long timediff){
filtered_currentAll=filtered_currentFront+filtered_currentRear; //positive value is current Drawn from battery. negative value is braking current filtered_currentAll=filtered_currentFront+filtered_currentRear; //positive value is current Drawn from battery. negative value is braking current
if(throttle_pos<last_cmd_send){ //freewheeling or braking if(adjusted_throttle_pos<last_cmd_send){ //freewheeling or braking
if (filtered_currentAll>freewheel_current) { //drive current too high if (filtered_currentAll>freewheel_current) { //drive current too high
cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(timediff/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(timediff/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
} }
@ -597,17 +602,17 @@ void calculateSetSpeed(unsigned long timediff){
} }
//acceleration //acceleration
cmd_send += constrain(throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly cmd_send += constrain(adjusted_throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly
cmd_send=constrain(cmd_send,0,1000); cmd_send=constrain(cmd_send,0,throttle_max);
last_cmd_send=cmd_send; last_cmd_send=cmd_send;
int16_t cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking" int16_t cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,throttle_max); //brake "ducking"
if (reverse_enabled) { if (reverse_enabled) {
cmd_send_toMotor-=brake_pos*REVERSE_SPEED; cmd_send_toMotor-=brake_pos*reverse_speed;
} }
if (!controllers_connected || !armed) { //controllers not connected or not armed if (!controllers_connected || !armed) { //controllers not connected or not armed
@ -777,6 +782,17 @@ void readButtons() {
if (escFront.getControllerConnected() && escRear.getControllerConnected()) { if (escFront.getControllerConnected() && escRear.getControllerConnected()) {
armed=true; //arm if button pressed long enough armed=true; //arm if button pressed long enough
writeLogComment(loopmillis, "Armed by button"); writeLogComment(loopmillis, "Armed by button");
if (control_buttonA) { //button A is held down during start button press
throttle_max=1000;
reverse_speed=0.25;
}else if (control_buttonB) { //button B is held down during start button press
throttle_max=750;
reverse_speed=0.25;
}else { //no control button pressed during start
throttle_max=250;
reverse_speed=0.15;
}
}else{ }else{
writeLogComment(loopmillis, "Unable to arm"); writeLogComment(loopmillis, "Unable to arm");
} }
@ -785,6 +801,32 @@ void readButtons() {
} }
void readADSButtons() {
bool last_control_buttonA=control_buttonA;
bool last_control_buttonB=control_buttonB;
if ( (ads_control_raw > (calib_control_buttonA-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonA+calib_control_treshold) ) ) {
control_buttonA=true;
control_buttonB=false;
}else if ( (ads_control_raw > (calib_control_buttonB-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonB+calib_control_treshold) ) ) {
control_buttonA=false;
control_buttonB=true;
}else if ( (ads_control_raw > (calib_control_buttonAB-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonAB+calib_control_treshold) ) ) {
control_buttonA=true;
control_buttonB=true;
}else if ( ads_control_raw > calib_control_max){
control_buttonA=false;
control_buttonB=false;
}
if (control_buttonA && !last_control_buttonA) { //button A was just pressed
writeLogComment(loopmillis, "Button A Pressed");
}
if (control_buttonB && !last_control_buttonB) { //button B was just pressed
writeLogComment(loopmillis, "Button B Pressed");
}
}
uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending) { uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending) {
//input is raw adc value from hall sensor //input is raw adc value from hall sensor