diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index 3c9afce..b76adb5 100644 --- a/controller_teensy/include/definitions.h +++ b/controller_teensy/include/definitions.h @@ -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 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_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; 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 b3db537..3cc8ded 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -21,6 +21,8 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); void display_standingOffDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); +void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); + bool display_init(){ 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()); */ + if (escFront.getControllerConnected() && escRear.getControllerConnected()) { - if (loopmillis-last_notidle>1000) { + if (loopmillis-last_notidle>5000) { display_standingDisplay(escFront,escRear); }else{ display_drivingDisplay(escFront,escRear); + //display_debugDisplay(escFront,escRear); } }else{ display_standingOffDisplay(escFront,escRear); } + + + display.display(); } void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { + //## Km/h Display display.setFont(&FreeMonoBold18pt7b); display.setTextSize(1); // Normal 1:1 pixel scale @@ -89,6 +97,7 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { //## Trip / Current Consumed Display display.setCursor(1,SCREEN_HEIGHT-7); + if (((millis()/2500)%2)==0) { //## Trip dtostrf(escFront.getTrip(),1,0,buf); @@ -108,6 +117,22 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { display.print((String)buf); 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) { @@ -116,7 +141,7 @@ void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { display.setCursor(0,0); 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.println(); @@ -138,8 +163,6 @@ void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { display.print((String)buf); display.print(" Ah"); display.println(); - - } diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 7c7ff79..fc54504 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -44,7 +44,7 @@ void readADS(); void readADC(); void failChecks(); //void sendCMD(); -void calculateSetSpeed(); +void calculateSetSpeed(unsigned long timediff); void checkLog(); void leds(); @@ -222,7 +222,9 @@ void loop() { static unsigned long last_calculateSetSpeed=0; if (loopmillis - last_calculateSetSpeed > SENDPERIOD) { - calculateSetSpeed(); + unsigned long _timediff=loopmillis-last_calculateSetSpeed; + last_calculateSetSpeed=loopmillis; + calculateSetSpeed(_timediff); } 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); 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_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()); 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 - 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 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