fix breaking, setSpeedLoop wrong timing

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

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();
-
-  
 }
 
 

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