fix speedms wrong direction

controller_teensy/include/definitions.h

@@ -29,7 +29,7 @@ bool controllers_connected=false;
 //const uint16_t calib_throttle_min = 420; //better a bit too high than too low
 //const uint16_t calib_throttle_max = 790;
 const uint16_t failsafe_throttle_min = 4900; //if adc value falls below this failsafe is triggered. old 20
-const uint16_t failsafe_throttle_max = 14000; //if adc value goes above this failsafe is triggered. old 1000
+const uint16_t failsafe_throttle_max = 14500; //if adc value goes above this failsafe is triggered. old 1000
 //const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel
 const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel
 #define PIN_BRAKE A8

controller_teensy/include/display.h

@@ -4,6 +4,10 @@
 #include <Wire.h>
 #include <Adafruit_GFX.h>
 #include <Adafruit_SSD1306.h>
+#include <Fonts/FreeMonoBold18pt7b.h>
+#include <Fonts/FreeSansBold9pt7b.h>
+
+
 
 #define SCREEN_WIDTH 128 // OLED display width, in pixels
 #define SCREEN_HEIGHT 32 // OLED display height, in pixels
@@ -15,6 +19,7 @@ Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
 bool display_init();
 void display_update();
 
+
 bool display_init(){
   if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
     Serial.println(F("SSD1306 allocation failed"));
@@ -30,17 +35,37 @@ bool display_init(){
 
 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((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0);
-  display.print(F("Throttle: ")); display.print(throttle_pos);
+  display.print(F("Thr: ")); display.print(throttle_pos);
-  display.print(F(" Brake : ")); display.println(brake_pos);
+  display.print(F(" Brk : ")); 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.setFont(&FreeMonoBold18pt7b);
+  display.setTextSize(1);             // Normal 1:1 pixel scale
+  display.setTextColor(SSD1306_WHITE);        // Draw white text
+  display.setCursor(0,SCREEN_HEIGHT-(SCREEN_HEIGHT-18)/2);             // Start at top-left corner
+
+  float _speeddisplay=(-escFront.getMeanSpeed()-escRear.getMeanSpeed())/2.0*3.6;
+  //_speeddisplay=(millis()/1000)%21; //debugging
+  char buf[8];
+  dtostrf(_speeddisplay,1,1,buf);
+  String strbuf=buf;
   
+  if (strbuf.length()<4) { //pad spaces on the left
+    strbuf=" "+strbuf;
+  }
+  display.print(strbuf);
   
+  display.setCursor(SCREEN_WIDTH-25,SCREEN_HEIGHT-1);
+  display.setFont();
+  display.print("km/h");
   
   display.display();
 }

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((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0);  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("");

controller_teensy/src/main.cpp

@@ -300,7 +300,8 @@ void readADS() { //sequentially read ads and write to variable
 
 
 void readADC() {
-  /*Serial.print(ads_throttle_A_raw); Serial.print('\t'); 
+  /*
+  Serial.print(ads_throttle_A_raw); Serial.print('\t'); 
   Serial.print(ads_throttle_B_raw); Serial.print('\t'); 
   Serial.print(ads_brake_raw); Serial.print('\t'); 
   Serial.print(ads_control_raw); Serial.println();*/
@@ -315,7 +316,7 @@ void readADC() {
 
 
 
-  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)
+  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;
   }