add oled display

controller_teensy/include/comms.h

@@ -0,0 +1,162 @@
+#ifndef _COMMS_H_
+#define _COMMS_H_
+
+
+#include "definitions.h"
+#include "structs.h"
+
+void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
+bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
+
+void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis);
+
+void writeLogInfo(HardwareSerial &SerialRef);
+void writeLogHeader(HardwareSerial &SerialRef);
+void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake);
+void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg);
+
+
+void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
+{
+  // Create command
+  scom.start    = (uint16_t)START_FRAME;
+  scom.speedLeft    = (int16_t)uSpeedLeft;
+  scom.speedRight    = (int16_t)uSpeedRight;
+  scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
+  
+  SerialRef.write((uint8_t *) &scom, sizeof(scom));
+  
+}
+
+bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
+{
+  bool _result=false; //return true if new full data frame received
+  // Check for new data availability in the Serial buffer
+  if ( SerialRef.available() ) {
+    sread.incomingByte    = SerialRef.read();                                 // Read the incoming byte
+    sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev;  // Construct the start frame    
+  }
+  else {
+    return false; //nothing new
+  }
+
+  // If DEBUG_RX is defined print all incoming bytes
+  #ifdef DEBUG_RX
+    Serial.print(sread.incomingByte);
+  #endif      
+  
+  // Copy received data
+  if (sread.bufStartFrame == START_FRAME) {                     // Initialize if new data is detected
+    sread.p     = (byte *)&NewFeedback;
+    *sread.p++  = sread.incomingBytePrev;
+    *sread.p++  = sread.incomingByte;   
+    sread.idx   = 2;    
+  } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) {  // Save the new received data
+    *sread.p++  = sread.incomingByte; 
+    sread.idx++;
+  } 
+  
+  // Check if we reached the end of the package
+  if (sread.idx == sizeof(SerialFeedback)) {    
+    uint16_t checksum;
+  
+    checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
+          ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
+  
+    // Check validity of the new data
+    if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
+      // Copy the new data
+      memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
+      sread.lastValidDataSerial_time = millis();
+      _result=true;
+    } else {
+      _result=false;
+    }
+    sread.idx = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
+  }
+  /*
+    // Print data to built-in Serial
+    Serial.print("1: ");   Serial.print(Feedback.cmd1);
+    Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
+    Serial.print(" 3: ");  Serial.print(Feedback.speedR);
+    Serial.print(" 4: ");  Serial.print(Feedback.speedL);
+    Serial.print(" 5: ");  Serial.print(Feedback.speedR_meas);
+    Serial.print(" 6: ");  Serial.print(Feedback.speedL_meas);
+    Serial.print(" 7: ");  Serial.print(Feedback.batVoltage);
+    Serial.print(" 8: ");  Serial.println(Feedback.boardTemp);
+  } else {
+    Serial.println("Non-valid data skipped");
+  }*/
+
+  // Update previous states
+  sread.incomingBytePrev = sread.incomingByte;
+
+  return _result; //new data was available
+}
+
+
+
+void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis) {
+  mp.cur_pos++;
+  mp.cur_pos%=CURRENT_FILTER_SIZE;
+  mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
+  mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
+  mp.millis=_loopmillis;
+  log_update=true;
+}
+
+
+
+void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
+  SerialRef.print("#TIMESTAMP:");
+  SerialRef.println(now()); 
+}
+
+void writeLogHeader(HardwareSerial &SerialRef) {
+  SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
+  SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
+  SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
+  SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
+  SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
+}
+
+void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
+{
+  SerialRef.print(time/1000.0,3); SerialRef.print(","); //time in seconds
+  SerialRef.print(mpfront.cmdL); SerialRef.print(",");
+  SerialRef.print(mpfront.cmdR); SerialRef.print(",");
+  SerialRef.print(mprear.cmdL); SerialRef.print(",");
+  SerialRef.print(mprear.cmdR); SerialRef.print(",");
+
+  SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(",");
+  SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(",");
+  SerialRef.print(mprear.filtered_curL,3); SerialRef.print(",");
+  SerialRef.print(mprear.filtered_curR,3); SerialRef.print(",");
+
+  SerialRef.print(fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
+  SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
+  SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
+  SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");
+
+  SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC
+  SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC
+  SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
+  SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
+
+  SerialRef.print(currentAll,3); SerialRef.print(",");
+  SerialRef.print(throttle); SerialRef.print(",");
+  SerialRef.print(brake); SerialRef.print(",");
+  SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
+  SerialRef.print(trip); SerialRef.print(","); //in m
+  SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours)
+  
+}
+
+void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) 
+{
+  SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
+}
+
+
+
+#endif

controller_teensy/include/definitions.h

@@ -0,0 +1,126 @@
+#ifndef _DEFINITIONS_H
+#define _DEFINITIONS_H
+
+
+// ########################## DEFINES ##########################
+#define SERIAL_CONTROL_BAUD   115200       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
+#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
+#define START_FRAME         0xABCD       // [-] Start frme definition for reliable serial communication
+
+#define SERIAL_LOG_BAUD 115200 // baud rate for logging output
+bool log_update=true;
+unsigned long last_log_send=0;
+
+#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
+#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
+#define LOGMAXINTERVAL 10000  //maximum time (ms) after which data is send
+
+#define WRITE_HEADER_TIME 400  //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments
+bool log_header_written = false;
+
+
+#define FEEDBACKRECEIVETIMEOUT 500
+
+bool controllerFront_connected=false;
+bool controllerRear_connected=false;
+bool controllers_connected=false;
+
+#define PIN_THROTTLE A7
+//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 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
+const uint16_t calib_brake_min = 2000;//better a bit too high than too low
+const uint16_t calib_brake_max = 11000;
+const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered
+const uint16_t failsafe_brake_max = 13000; //if adc value goes above this failsafe is triggered
+
+uint16_t ads_throttle_A_raw=0;
+uint16_t ads_throttle_B_raw=0;
+uint16_t ads_brake_raw=failsafe_brake_min;
+uint16_t ads_control_raw=0;
+
+int16_t throttle_pos=0;
+int16_t brake_pos=0;
+
+
+#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query
+#define ADCREADPERIOD 10
+#define BUTTONREADPERIOD 20
+unsigned long last_adsread=0; //needed for failcheck
+uint16_t throttle_raw=failsafe_throttle_min;  //start at min so that failsafe is not triggered
+#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response
+uint16_t brake_raw=failsafe_brake_min;  //start at min so that failsafe is not triggered
+#define ADC_OUTOFRANGE_TIME 100
+unsigned long throttle_ok_time=0;
+unsigned long brake_ok_time=0;
+bool error_throttle_outofrange=false;
+bool error_brake_outofrange=false;
+bool error_ads_max_read_interval=false;
+
+#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward
+#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1
+
+#define NORMAL_MAX_ACCELERATION_RATE 10000
+#define SLOW_MAX_ACCELERATION_RATE 500
+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)
+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
+
+bool reverse_enabled=false;
+unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
+
+#define PIN_START A9
+#define PIN_LED_START 2 //Enginge start led
+
+#define PIN_LATCH_ENABLE A6
+
+#define PIN_MODE_SWITCH 3
+#define PIN_MODE_LEDG 4
+#define PIN_MODE_LEDR 5
+
+
+
+unsigned long last_send = 0;
+unsigned long last_receive = 0;
+
+float filtered_currentAll=0;
+
+int16_t cmd_send=0;
+int16_t last_cmd_send=0;
+
+uint8_t speedmode=0;
+#define SPEEDMODE_SLOW 1
+#define SPEEDMODE_NORMAL 0
+
+
+unsigned long button_start_lastchange=0;
+bool button_start_state=false;
+#define LONG_PRESS_ARMING_TIME 2000
+#define DEBOUNCE_TIME 50
+
+bool armed = false; //cmd output values forced to 0 if false
+
+
+
+#define CURRENT_FILTER_SIZE 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
+#define CURRENT_MEANVALUECOUNT 20 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used
+
+#define DISPLAYUPDATEPERIOD 100
+
+#endif

controller_teensy/include/display.h

@@ -0,0 +1,38 @@
+#ifndef _DISPLAY_H_
+#define _DISPLAY_H_
+
+#include <Wire.h>
+#include <Adafruit_GFX.h>
+#include <Adafruit_SSD1306.h>
+
+#define SCREEN_WIDTH 128 // OLED display width, in pixels
+#define SCREEN_HEIGHT 32 // OLED display height, in pixels
+
+#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
+#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
+Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
+
+void display_init();
+void display_test();
+
+void display_init(){
+  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
+    Serial.println(F("SSD1306 allocation failed"));
+  }
+  display.clearDisplay();
+  display.display();
+}
+
+void display_test(){
+  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.println(F("Hello Welt"));
+  display.println(millis());
+  display.println(now());
+  display.display();
+}
+
+
+#endif

controller_teensy/include/helpfunctions.h

@@ -0,0 +1,34 @@
+#ifndef _HELPFUNCTIONS_H_
+#define _HELPFUNCTIONS_H_
+
+#include "definitions.h"
+
+int sort_desc(const void *cmp1, const void *cmp2);
+float filterMedian(int16_t* values);
+
+int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
+{
+  float a = *((float *)cmp1);
+  float b = *((float *)cmp2);
+  return a > b ? -1 : (a < b ? 1 : 0);
+}
+
+
+float filterMedian(int16_t* values) {
+  float copied_values[CURRENT_FILTER_SIZE];
+  for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
+    copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
+  }
+  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
+  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
+
+  float mean=copied_values[CURRENT_FILTER_SIZE/2];
+  for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
+    mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
+  }
+  mean/=(1+CURRENT_MEANVALUECOUNT*2);
+
+  return mean;
+}
+
+#endif

controller_teensy/include/structs.h

@@ -0,0 +1,60 @@
+#ifndef _STRUCTS_H_
+#define _STRUCTS_H_
+
+// Global variables for serial communication
+typedef struct{
+    uint8_t idx = 0;                        // Index for new data pointer
+    uint16_t bufStartFrame;                 // Buffer Start Frame
+    byte *p;                                // Pointer declaration for the new received data
+    byte incomingByte;
+    byte incomingBytePrev;
+    long lastValidDataSerial_time;
+} SerialRead;
+SerialRead SerialcomFront;
+SerialRead SerialcomRear;
+
+
+typedef struct{
+   uint16_t start;
+   int16_t  speedLeft;
+   int16_t  speedRight;
+   uint16_t checksum;
+} SerialCommand;
+SerialCommand CommandFront;
+SerialCommand CommandRear;
+
+
+typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
+   uint16_t start;
+   int16_t  cmd1;
+   int16_t  cmd2;
+   int16_t  speedL_meas; //left speed is positive when driving forward
+   int16_t  speedR_meas; //right speed is negatie when driving forward
+   int16_t  batVoltage;
+   int16_t  boardTemp;
+   int16_t  curL_DC; //negative values are current consumed. positive values mean generated current
+   int16_t  curR_DC;
+   uint16_t cmdLed;
+   uint16_t  checksum;
+} SerialFeedback;
+SerialFeedback FeedbackFront;
+SerialFeedback NewFeedbackFront;
+SerialFeedback FeedbackRear;
+SerialFeedback NewFeedbackRear;
+
+
+typedef struct{
+  int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
+  int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
+  uint8_t cur_pos=0;
+  int16_t cmdL=0;
+  int16_t cmdR=0;
+  float filtered_curL=0;
+  float filtered_curR=0;
+  unsigned long millis=0; //time when last message received
+} MotorParameter;
+MotorParameter motorparamsFront;
+MotorParameter motorparamsRear;
+
+
+#endif

controller_teensy/platformio.ini

@@ -22,3 +22,4 @@ build_flags =
 
 lib_deps =
     robtillaart/ADS1X15@^0.3.9
+    adafruit/Adafruit SSD1306@^2.5.7

controller_teensy/src/main.cpp

@@ -1,5 +1,11 @@
 #include <Arduino.h>
 
+#include "definitions.h"
+#include "structs.h"
+#include "helpfunctions.h"
+#include <TimeLib.h> //for teensy rtc 
+#include "comms.h"
+#include "display.h"
 
 #include "ADS1X15.h"
 
@@ -14,280 +20,23 @@ Tennsy Pin, Pin Name, Connected to
 7, Rx3, Hoverboard TX(Blue)
 */
 
-// ########################## DEFINES ##########################
-#define SERIAL_CONTROL_BAUD   115200       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
-#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
-#define START_FRAME         0xABCD       // [-] Start frme definition for reliable serial communication
 
-#define SERIAL_LOG_BAUD 115200 // baud rate for logging output
-bool log_update=true;
-unsigned long last_log_send=0;
-
-#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
-#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
-#define LOGMAXINTERVAL 10000  //maximum time (ms) after which data is send
-
-#define WRITE_HEADER_TIME 400  //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments
-bool log_header_written = false;
-
-
-#define FEEDBACKRECEIVETIMEOUT 500
-
-bool controllerFront_connected=false;
-bool controllerRear_connected=false;
-bool controllers_connected=false;
-
-#define PIN_THROTTLE A7
-//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 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
-const uint16_t calib_brake_min = 2000;//better a bit too high than too low
-const uint16_t calib_brake_max = 11000;
-const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered
-const uint16_t failsafe_brake_max = 13000; //if adc value goes above this failsafe is triggered
-
-uint16_t ads_throttle_A_raw=0;
-uint16_t ads_throttle_B_raw=0;
-uint16_t ads_brake_raw=failsafe_brake_min;
-uint16_t ads_control_raw=0;
-
-int16_t throttle_pos=0;
-int16_t brake_pos=0;
-
-
-#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query
-#define ADCREADPERIOD 10
-#define BUTTONREADPERIOD 20
-unsigned long last_adsread=0; //needed for failcheck
-uint16_t throttle_raw=failsafe_throttle_min;  //start at min so that failsafe is not triggered
-#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response
-uint16_t brake_raw=failsafe_brake_min;  //start at min so that failsafe is not triggered
-#define ADC_OUTOFRANGE_TIME 100
-unsigned long throttle_ok_time=0;
-unsigned long brake_ok_time=0;
-bool error_throttle_outofrange=false;
-bool error_brake_outofrange=false;
-bool error_ads_max_read_interval=false;
-
-#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward
-#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1
-
-#define NORMAL_MAX_ACCELERATION_RATE 10000
-#define SLOW_MAX_ACCELERATION_RATE 500
-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)
-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
-
-bool reverse_enabled=false;
-unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
-
-#define PIN_START A9
-#define PIN_LED_START 2 //Enginge start led
-
-#define PIN_LATCH_ENABLE A6
-
-#define PIN_MODE_SWITCH 3
-#define PIN_MODE_LEDG 4
-#define PIN_MODE_LEDR 5
-
-
-
-unsigned long last_send = 0;
-unsigned long last_receive = 0;
-
-float filtered_currentAll=0;
-
-int16_t cmd_send=0;
-int16_t last_cmd_send=0;
-
-uint8_t speedmode=0;
-#define SPEEDMODE_SLOW 1
-#define SPEEDMODE_NORMAL 0
-
-
-unsigned long button_start_lastchange=0;
-bool button_start_state=false;
-#define LONG_PRESS_ARMING_TIME 2000
-#define DEBOUNCE_TIME 50
-
-bool armed = false; //cmd output values forced to 0 if false
-
-
-// Global variables for serial communication
-typedef struct{
-    uint8_t idx = 0;                        // Index for new data pointer
-    uint16_t bufStartFrame;                 // Buffer Start Frame
-    byte *p;                                // Pointer declaration for the new received data
-    byte incomingByte;
-    byte incomingBytePrev;
-    long lastValidDataSerial_time;
-} SerialRead;
-SerialRead SerialcomFront;
-SerialRead SerialcomRear;
-
-
-typedef struct{
-   uint16_t start;
-   int16_t  speedLeft;
-   int16_t  speedRight;
-   uint16_t checksum;
-} SerialCommand;
-SerialCommand CommandFront;
-SerialCommand CommandRear;
-
-
-typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
-   uint16_t start;
-   int16_t  cmd1;
-   int16_t  cmd2;
-   int16_t  speedL_meas; //left speed is positive when driving forward
-   int16_t  speedR_meas; //right speed is negatie when driving forward
-   int16_t  batVoltage;
-   int16_t  boardTemp;
-   int16_t  curL_DC; //negative values are current consumed. positive values mean generated current
-   int16_t  curR_DC;
-   uint16_t cmdLed;
-   uint16_t  checksum;
-} SerialFeedback;
-SerialFeedback FeedbackFront;
-SerialFeedback NewFeedbackFront;
-SerialFeedback FeedbackRear;
-SerialFeedback NewFeedbackRear;
-
-#define CURRENT_FILTER_SIZE 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
-#define CURRENT_MEANVALUECOUNT 20 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used
-typedef struct{
-  int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
-  int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
-  uint8_t cur_pos=0;
-  int16_t cmdL=0;
-  int16_t cmdR=0;
-  float filtered_curL=0;
-  float filtered_curR=0;
-  unsigned long millis=0; //time when last message received
-} MotorParameter;
-MotorParameter motorparamsFront;
-MotorParameter motorparamsRear;
-
-
-void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
-bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
-
-int sort_desc(const void *cmp1, const void *cmp2);
-float filterMedian(int16_t* values);
-
-void writeLogHeader(HardwareSerial &SerialRef);
-void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake);
-void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg);
 
 void readADS();
 void readADC();
 void failChecks();
 void sendCMD();
 void checkLog();
-void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
+
 void leds();
 
 void readButtons();
 
 uint16_t linearizeThrottle(uint16_t v);
 
-#include <TimeLib.h> //for teensy rtc 
+
 time_t getTeensy3Time();
 
-void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
-{
-  // Create command
-  scom.start    = (uint16_t)START_FRAME;
-  scom.speedLeft    = (int16_t)uSpeedLeft;
-  scom.speedRight    = (int16_t)uSpeedRight;
-  scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
-  
-  SerialRef.write((uint8_t *) &scom, sizeof(scom));
-  
-}
-
-bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
-{
-  bool _result=false; //return true if new full data frame received
-  // Check for new data availability in the Serial buffer
-  if ( SerialRef.available() ) {
-    sread.incomingByte    = SerialRef.read();                                 // Read the incoming byte
-    sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev;  // Construct the start frame    
-  }
-  else {
-    return false; //nothing new
-  }
-
-  // If DEBUG_RX is defined print all incoming bytes
-  #ifdef DEBUG_RX
-    Serial.print(sread.incomingByte);
-  #endif      
-  
-  // Copy received data
-  if (sread.bufStartFrame == START_FRAME) {                     // Initialize if new data is detected
-    sread.p     = (byte *)&NewFeedback;
-    *sread.p++  = sread.incomingBytePrev;
-    *sread.p++  = sread.incomingByte;   
-    sread.idx   = 2;    
-  } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) {  // Save the new received data
-    *sread.p++  = sread.incomingByte; 
-    sread.idx++;
-  } 
-  
-  // Check if we reached the end of the package
-  if (sread.idx == sizeof(SerialFeedback)) {    
-    uint16_t checksum;
-  
-    checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
-          ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
-  
-    // Check validity of the new data
-    if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
-      // Copy the new data
-      memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
-      sread.lastValidDataSerial_time = millis();
-      _result=true;
-    } else {
-      _result=false;
-    }
-    sread.idx = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
-  }
-  /*
-    // Print data to built-in Serial
-    Serial.print("1: ");   Serial.print(Feedback.cmd1);
-    Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
-    Serial.print(" 3: ");  Serial.print(Feedback.speedR);
-    Serial.print(" 4: ");  Serial.print(Feedback.speedL);
-    Serial.print(" 5: ");  Serial.print(Feedback.speedR_meas);
-    Serial.print(" 6: ");  Serial.print(Feedback.speedL_meas);
-    Serial.print(" 7: ");  Serial.print(Feedback.batVoltage);
-    Serial.print(" 8: ");  Serial.println(Feedback.boardTemp);
-  } else {
-    Serial.println("Non-valid data skipped");
-  }*/
-
-  // Update previous states
-  sread.incomingBytePrev = sread.incomingByte;
-
-  return _result; //new data was available
-}
 
   // ########################## SETUP ##########################
 void setup() 
@@ -315,6 +64,9 @@ void setup()
   digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
   pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
   
+  
+  display_init();
+
   delay(2000);
   Serial.println("Wait finished. Booting..");
 
@@ -376,10 +128,10 @@ void loop() {
   //Max (40) or 22 available/pending bytes
 
   if (newData2) {
-    updateMotorparams(motorparamsFront,FeedbackFront);
+    updateMotorparams(motorparamsFront,FeedbackFront,loopmillis);
   }
   if (newData3) {
-    updateMotorparams(motorparamsRear,FeedbackRear);
+    updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
   }
 
 
@@ -429,92 +181,21 @@ void loop() {
 
   leds();
 
-
+  static unsigned long last_display_update=0;
+  if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
+    last_display_update=loopmillis;
+    display_test();
+  }
 }
 
 
-// ##### HELPFUNCTIONS
-
 time_t getTeensy3Time()
 {
   return Teensy3Clock.get();
 }
 
 
-int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
-{
-  float a = *((float *)cmp1);
-  float b = *((float *)cmp2);
-  return a > b ? -1 : (a < b ? 1 : 0);
-}
 
-float filterMedian(int16_t* values) {
-  float copied_values[CURRENT_FILTER_SIZE];
-  for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
-    copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
-  }
-  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
-  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
-
-  float mean=copied_values[CURRENT_FILTER_SIZE/2];
-  for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
-    mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
-  }
-  mean/=(1+CURRENT_MEANVALUECOUNT*2);
-
-  return mean;
-}
-
-
-void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
-  SerialRef.print("#TIMESTAMP:");
-  SerialRef.println(now()); 
-}
-
-void writeLogHeader(HardwareSerial &SerialRef) {
-  SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
-  SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
-  SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
-  SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
-  SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
-}
-
-void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
-{
-  SerialRef.print(time/1000.0,3); SerialRef.print(","); //time in seconds
-  SerialRef.print(mpfront.cmdL); SerialRef.print(",");
-  SerialRef.print(mpfront.cmdR); SerialRef.print(",");
-  SerialRef.print(mprear.cmdL); SerialRef.print(",");
-  SerialRef.print(mprear.cmdR); SerialRef.print(",");
-
-  SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(",");
-  SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(",");
-  SerialRef.print(mprear.filtered_curL,3); SerialRef.print(",");
-  SerialRef.print(mprear.filtered_curR,3); SerialRef.print(",");
-
-  SerialRef.print(fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
-  SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
-  SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
-  SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");
-
-  SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC
-  SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC
-  SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
-  SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
-
-  SerialRef.print(currentAll,3); SerialRef.print(",");
-  SerialRef.print(throttle); SerialRef.print(",");
-  SerialRef.print(brake); SerialRef.print(",");
-  SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
-  SerialRef.print(trip); SerialRef.print(","); //in m
-  SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours)
-  
-}
-
-void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) 
-{
-  SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
-}
 
 
 void readADS() { //sequentially read ads and write to variable
@@ -760,14 +441,7 @@ void checkLog() {
   }
 }
 
-void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
+
-  mp.cur_pos++;
-  mp.cur_pos%=CURRENT_FILTER_SIZE;
-  mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
-  mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
-  mp.millis=loopmillis;
-  log_update=true;
-}
 
 void leds() {
   //Start LED