From ecbb3bb491f576a6e321fe2fe82ffef207e7dc06 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sat, 2 Mar 2024 21:55:40 +0100 Subject: [PATCH] add driving parameter change via buttons and button display screen toggle --- controller_teensy/include/definitions.h | 16 ++- controller_teensy/include/display.h | 157 +++++++++++++++++------- controller_teensy/src/main.cpp | 60 +++++++-- 3 files changed, 181 insertions(+), 52 deletions(-) diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index 556d8d7..1f3089a 100644 --- a/controller_teensy/include/definitions.h +++ b/controller_teensy/include/definitions.h @@ -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_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_B_raw=0; 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 +bool control_buttonA=false; +bool control_buttonB=false; + unsigned long loopmillis; unsigned long looptime_duration; @@ -80,7 +91,7 @@ bool error_ads_max_read_interval=false; bool error_sdfile_unavailable=false; #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 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) 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 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; unsigned long last_notidle=0; //not rolling to fast, no pedal pressed diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index bb3222b..ec2e834 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -85,8 +85,11 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){ if (loopmillis-last_notidle>5000) { display_standingDisplay(escFront,escRear); }else{ - display_drivingDisplay(escFront,escRear); - //display_debugDisplay(escFront,escRear); + if (!control_buttonA) { + display_drivingDisplay(escFront,escRear); + }else{ + display_debugDisplay(escFront,escRear); + } } }else{ @@ -147,7 +150,8 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { //## Trip / Current Consumed Display display.setCursor(1,SCREEN_HEIGHT-7); - if (((millis()/2500)%2)==0) { + //if (((millis()/2500)%2)==0) { + if (control_buttonB) { //## Speed statistic display.print("max: "); 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) { //Debug - display.setTextSize(1); // Normal 1:1 pixel scale + display.setTextSize(2); // Normal 1:1 pixel scale display.setFont(); display.setTextColor(SSD1306_WHITE); // Draw white text char buf[8]; @@ -190,6 +194,32 @@ void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { dtostrf(filtered_currentAll,1,3,buf); display.print((String)buf); 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 _displaycurrent=currentConsumed; double _displaywatthours=watthoursConsumed; - bool _displayOverall= ((millis()/3000)%2==0); - if (_displayOverall) { //alternate between this trip and overall trip - _displaytrip=overallTrip; - //_displaycurrent=overallCurrentConsumed; - _displaywatthours=overallWatthoursConsumed; - } + //bool _displayOverall= ((millis()/3000)%2==0); //switch based on time + bool _displayOverall=control_buttonB; //switch with button + bool _displayParameters=control_buttonA; char buf[8]; + display.setFont(); display.setCursor(0,0); - display.print(F("Vbat:")); display.print(escFront.getFeedback_batVoltage()); - display.print(F("/")); display.print(escRear.getFeedback_batVoltage()); - display.print(" V"); - display.println(); + if (_displayParameters) { + + display.print(F("cmdred min=")); display.print(minimum_constant_cmd_reduce); + 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:")); - 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("brkI=")); + dtostrf(startbrakecurrent,1,1,buf); + display.print((String)buf); + display.print("A"); + display.print(F(" free=")); + dtostrf(freewheel_break_factor,1,1,buf); + display.print((String)buf); + 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"); + display.print(F("thrmax=")); display.print(throttle_max); + display.print(F(" rev=")); + dtostrf(reverse_speed,1,2,buf); + display.print((String)buf); + display.println(); + + }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="); dtostrf(ads_brake_raw,1,0,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"); + } + } diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index f5aee5b..23ba8d4 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -58,6 +58,7 @@ void checkLog(); void leds(); void readButtons(); +void readADSButtons(); 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 last_buttonread=loopmillis; readButtons(); + readADSButtons(); } @@ -396,7 +398,7 @@ void readADS() { //sequentially read ads and write to variable case 2: //Brake ads_brake_raw=ads_val; break; - case 3: //Buttons TODO + case 3: //Buttons ads_control_raw=ads_val; break; } @@ -455,6 +457,9 @@ void readADC() { throttle_pos=constrain(throttlebreak_pos,0,1000); 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_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println(); @@ -571,6 +576,8 @@ void failChecks() { 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); 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 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_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 - if(throttle_posfreewheel_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 } @@ -597,17 +602,17 @@ void calculateSetSpeed(unsigned long timediff){ } //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; - 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) { - cmd_send_toMotor-=brake_pos*REVERSE_SPEED; + cmd_send_toMotor-=brake_pos*reverse_speed; } if (!controllers_connected || !armed) { //controllers not connected or not armed @@ -777,6 +782,17 @@ void readButtons() { if (escFront.getControllerConnected() && escRear.getControllerConnected()) { armed=true; //arm if button pressed long enough 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{ 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) { //input is raw adc value from hall sensor