add driving parameter change via buttons and button display screen toggle
This commit is contained in:
parent
87a65ccde5
commit
ecbb3bb491
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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_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
|
||||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue