diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h
index 52db129..48e9904 100644
--- a/controller_teensy/include/definitions.h
+++ b/controller_teensy/include/definitions.h
@@ -69,13 +69,6 @@ bool error_ads_max_read_interval=false;
 int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second
 
 
-float meanSpeedms=0;
-float trip=0; //trip distance in meters
-float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi
-
-float currentConsumed=0; //Ah
-
-
 //Driving parameters
 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)
diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h
index 6ea10e6..d2f9a7f 100644
--- a/controller_teensy/include/display.h
+++ b/controller_teensy/include/display.h
@@ -28,15 +28,18 @@ bool display_init(){
   return true;
 }
 
-void display_update(){
+void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){
   display.clearDisplay();
   display.setTextSize(1);             // Normal 1:1 pixel scale
   display.setTextColor(SSD1306_WHITE);        // Draw white text
   display.setCursor(0,0);             // Start at top-left corner
-  display.print(F("Speed   : ")); display.println(meanSpeedms);
-  display.print(F("Throttle: ")); display.println(throttle_pos);
-  display.print(F("Brake   : ")); display.println(brake_pos);
+  display.print(F("Speed   : ")); display.println((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0);
+  display.print(F("Throttle: ")); display.print(throttle_pos);
+  display.print(F(" Brake : ")); display.println(brake_pos);
   display.print(F("Current : ")); display.println(filtered_currentAll);
+  display.print(F("Vbat : ")); display.print(escFront.getFeedback_batVoltage());
+  display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage());
+  
   
   
   display.display();
diff --git a/controller_teensy/include/logging.h b/controller_teensy/include/logging.h
index 1e6c761..73f5a3d 100644
--- a/controller_teensy/include/logging.h
+++ b/controller_teensy/include/logging.h
@@ -88,7 +88,7 @@ void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm
         dataFile.print(filtered_currentAll,3);  dataFile.print(";");
         dataFile.print(throttle_pos);  dataFile.print(";");
         dataFile.print(brake_pos);  dataFile.print(";");
-        dataFile.print(meanSpeedms);  dataFile.print(";");
+        dataFile.print((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0);  dataFile.print(";");
         dataFile.print(escFront.getTrip());  dataFile.print(";");
         dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(";");
         dataFile.println("");
diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 7d407e4..731f6a7 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -252,7 +252,7 @@ void loop() {
   static unsigned long last_display_update=0;
   if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
     last_display_update=loopmillis;
-    display_update();
+    display_update(escFront,escRear);
   }
 }
 
@@ -315,7 +315,7 @@ void readADC() {
 
 
 
-  if (throttle_pos>0 || meanSpeedms>0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
+  if (throttle_pos>0 || ((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0) >0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
     last_notidle=loopmillis;
     reverse_enabled=false;
   }