fix breaking, setSpeedLoop wrong timing

This commit is contained in:
interfisch 2023-06-02 21:10:45 +02:00
parent 62c4b15b24
commit 14ecb8aa8f
3 changed files with 37 additions and 11 deletions

View File

@ -76,7 +76,7 @@ int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send i
int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
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=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.07; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
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

View File

@ -21,6 +21,8 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_standingOffDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); void display_standingOffDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
bool display_init(){ bool display_init(){
if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
@ -49,22 +51,28 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){
display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage()); display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage());
*/ */
if (escFront.getControllerConnected() && escRear.getControllerConnected()) { if (escFront.getControllerConnected() && escRear.getControllerConnected()) {
if (loopmillis-last_notidle>1000) { if (loopmillis-last_notidle>5000) {
display_standingDisplay(escFront,escRear); display_standingDisplay(escFront,escRear);
}else{ }else{
display_drivingDisplay(escFront,escRear); display_drivingDisplay(escFront,escRear);
//display_debugDisplay(escFront,escRear);
} }
}else{ }else{
display_standingOffDisplay(escFront,escRear); display_standingOffDisplay(escFront,escRear);
} }
display.display(); display.display();
} }
void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
//## Km/h Display //## Km/h Display
display.setFont(&FreeMonoBold18pt7b); display.setFont(&FreeMonoBold18pt7b);
display.setTextSize(1); // Normal 1:1 pixel scale display.setTextSize(1); // Normal 1:1 pixel scale
@ -89,6 +97,7 @@ 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) {
//## Trip //## Trip
dtostrf(escFront.getTrip(),1,0,buf); dtostrf(escFront.getTrip(),1,0,buf);
@ -108,6 +117,22 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
display.print((String)buf); display.print((String)buf);
display.print("Ah"); display.print("Ah");
} }
}
void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
//Debug
display.setTextSize(1); // Normal 1:1 pixel scale
display.setFont();
display.setTextColor(SSD1306_WHITE); // Draw white text
char buf[8];
display.setCursor(1,0);
dtostrf(filtered_currentAll,1,3,buf);
display.print((String)buf);
display.print(" A > ");
} }
void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
@ -116,7 +141,7 @@ void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
display.setCursor(0,0); display.setCursor(0,0);
display.print(F("Vbat: ")); display.print(escFront.getFeedback_batVoltage()); display.print(F("Vbat: ")); display.print(escFront.getFeedback_batVoltage());
display.print(F("/")); display.print(escFront.getFeedback_batVoltage()); display.print(F("/")); display.print(escRear.getFeedback_batVoltage());
display.print(" V"); display.print(" V");
display.println(); display.println();
@ -138,8 +163,6 @@ void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
display.print((String)buf); display.print((String)buf);
display.print(" Ah"); display.print(" Ah");
display.println(); display.println();
} }

View File

@ -44,7 +44,7 @@ void readADS();
void readADC(); void readADC();
void failChecks(); void failChecks();
//void sendCMD(); //void sendCMD();
void calculateSetSpeed(); void calculateSetSpeed(unsigned long timediff);
void checkLog(); void checkLog();
void leds(); void leds();
@ -222,7 +222,9 @@ void loop() {
static unsigned long last_calculateSetSpeed=0; static unsigned long last_calculateSetSpeed=0;
if (loopmillis - last_calculateSetSpeed > SENDPERIOD) { if (loopmillis - last_calculateSetSpeed > SENDPERIOD) {
calculateSetSpeed(); unsigned long _timediff=loopmillis-last_calculateSetSpeed;
last_calculateSetSpeed=loopmillis;
calculateSetSpeed(_timediff);
} }
escFront.update(loopmillis); escFront.update(loopmillis);
@ -427,28 +429,29 @@ void failChecks() {
} }
} }
void calculateSetSpeed(){ void calculateSetSpeed(unsigned long timediff){
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)
int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*SENDPERIOD/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 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());
filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current
if (throttle_pos>=last_cmd_send) { //accelerating if (throttle_pos>=last_cmd_send) { //accelerating
cmd_send += constrain(throttle_pos-cmd_send,0,max_acceleration_rate*SENDPERIOD/1000); //if throttle higher than last applied value, apply throttle directly 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
}else{ //freewheeling or braking }else{ //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*(SENDPERIOD/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
} }
cmd_send-=max(minimum_constant_cmd_reduce,cmdreduce_constant); //reduce slowly anyways cmd_send-=max(minimum_constant_cmd_reduce,cmdreduce_constant); //reduce slowly anyways