add reverse

This commit is contained in:
interfisch 2021-05-30 00:07:43 +02:00
parent f27026488a
commit 58b8a80f58
1 changed files with 21 additions and 4 deletions

View File

@ -75,7 +75,8 @@ int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount propor
float startbrakecurrent=3; //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=3; //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.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
bool reverse_enabled=false;
unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
#define PIN_START A9 #define PIN_START A9
#define PIN_LED_START 2 //Enginge start led #define PIN_LED_START 2 //Enginge start led
@ -310,6 +311,8 @@ void loop() {
readADC(); readADC();
readButtons(); readButtons();
} }
@ -422,6 +425,15 @@ void readADC() {
brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
//brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000); //brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000);
if (throttle_pos>0 || meanSpeedms>0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
last_notidle=loopmillis;
reverse_enabled=false;
}
#define REVERSE_ENABLE_TIME 1000
if (loopmillis-last_notidle > REVERSE_ENABLE_TIME) {
reverse_enabled=true;
}
int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
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
@ -542,9 +554,7 @@ void sendCMD() {
} }
if (!controllers_connected || !armed) { //controllers not connected or not armed
cmd_send=0; //safety off
}
cmd_send=constrain(cmd_send,0,1000); cmd_send=constrain(cmd_send,0,1000);
@ -552,7 +562,14 @@ void sendCMD() {
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,1000); //brake "ducking"
if (reverse_enabled) {
cmd_send_toMotor-=brake_pos*0.1;
}
if (!controllers_connected || !armed) { //controllers not connected or not armed
cmd_send=0;
cmd_send_toMotor=0; //safety off
}
//apply throttle command to all motors //apply throttle command to all motors
motorparamsFront.cmdL=cmd_send_toMotor; motorparamsFront.cmdL=cmd_send_toMotor;