remove unused meanSpeedms, trip and currentConsumed in main

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)

controller_teensy/include/display.h

@@ -12,29 +12,34 @@
 #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();
+bool display_init();
 void display_update();
 
-void display_init(){
+bool display_init(){
   if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
     Serial.println(F("SSD1306 allocation failed"));
+    return false;
   }
   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.display();
+  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();

controller_teensy/include/led.h

@@ -17,27 +17,111 @@ Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRBW + NEO_KHZ800);
 unsigned long last_ledupdate=0;
 #define LEDUPDATEINTERVAL 100
 
+uint8_t led_errorcount=0; //count led progress errors. used for delay at end if any errors occured
+
+void led_dotcircle(unsigned long loopmillis);
+void led_voltage(unsigned long loopmillis,float vbat,float vbat_min,float vbat_max);
+
 void init_led() {
   strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
   strip.show();            // Turn OFF all pixels ASAP
   strip.setBrightness(BRIGHTNESS);
 }
 
-void led_update(unsigned long loopmillis) {
+void led_testLEDSBlocking(){
+    strip.clear();
+    strip.show();
+    delay(10);
+    uint32_t color=strip.Color(0,   0,   0, 0);
+
+    for(int i=0; i<strip.numPixels()*4; i++) { // For each pixel in strip...
+        switch(i/strip.numPixels()) {
+            case 0:
+                color=strip.Color(255,   0,   0, 0);
+            break;
+            case 1:
+                color=strip.Color(0,   255,   0, 0);
+            break;
+            case 2:
+                color=strip.Color(0,   0,   255, 0);
+            break;
+            case 3:
+                color=strip.Color(0,   0,   0, 255);
+            break;
+        }
+        strip.setPixelColor(i%strip.numPixels(), color);
+        strip.show();
+        delay(25);
+    }
+    strip.clear();
+    strip.show();
+}
+
+void led_simpeProgress(uint8_t progressID,uint8_t result){
+    if (result!=1) {
+        led_errorcount++;
+    }
+    uint32_t color=strip.Color(0,   0,   0, 0);
+    switch(result) {
+        case 0: //fail
+            color=strip.Color(255,   0,   0, 0);
+        break;
+        case 1: //success
+            color=strip.Color(0,   255,   0, 0);
+        break;
+        case 2: //warning
+            color=strip.Color(127,   127,   0, 0);
+        break;
+    }
+    strip.setPixelColor(progressID, color);
+    strip.show();
+}
+
+void led_simpleProgressWait() {
+    if (led_errorcount>0) {
+        delay(5000);
+    }else{
+        delay(100);
+    }
+}
+
+void led_update(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
     if (loopmillis>last_ledupdate+LEDUPDATEINTERVAL) {
         last_ledupdate=loopmillis;
-        uint32_t color=strip.Color(0,   0,   0, 255);
-        uint32_t colorOff=strip.Color(30,  0,   0, 0);
-        uint8_t position=(loopmillis/100)%strip.numPixels();
-        for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
-            if (i==position) {
-                strip.setPixelColor(i, color);         //  Set pixel's color (in RAM)
-            }else{
-                strip.setPixelColor(i, colorOff);         //  Set pixel's color (in RAM)
-            }
-        }
+
+        float vbat=min(escRear.getFeedback_batVoltage(),escFront.getFeedback_batVoltage());
+
+        //led_dotcircle(loopmillis);
+        led_voltage(loopmillis,vbat,3*12,4.2*12);
         strip.show();                          //  Update strip to match
     }
 }
 
+void led_voltage(unsigned long loopmillis,float vbat,float vbat_min,float vbat_max) {
+    uint32_t colorBG=strip.Color(0,   255,   0, 0);
+    uint32_t colorEmpty=strip.Color(255,  0,   0, 0);
+    uint8_t position=map( max(min(vbat,vbat_max),vbat_min)  ,vbat_min,vbat_max, 0,strip.numPixels());
+    for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
+        uint8_t pp=i%strip.numPixels()+8; //Offset
+        if (i<position) {
+            strip.setPixelColor(pp, colorBG);         //  Set pixel's color (in RAM)
+        }else{
+            strip.setPixelColor(pp, colorEmpty);         //  Set pixel's color (in RAM)
+        }
+    }
+}
+
+void led_dotcircle(unsigned long loopmillis) {
+    uint32_t color=strip.Color(0,   0,   0, 255);
+    uint32_t colorOff=strip.Color(30,  0,   0, 0);
+    uint8_t position=(loopmillis/100)%strip.numPixels();
+    for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
+        if (i==position) {
+            strip.setPixelColor(i, color);         //  Set pixel's color (in RAM)
+        }else{
+            strip.setPixelColor(i, colorOff);         //  Set pixel's color (in RAM)
+        }
+    }
+}
+
 #endif

controller_teensy/include/logging.h

@@ -10,11 +10,11 @@
 boolean datalogging=true;
 String datalogging_filename="UNKNOWN.txt";
 
-void initLogging();
+bool initLogging();
 void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);
 void writeLogComment(unsigned long time, String msg);
 
-void initLogging() {
+bool initLogging() {
     Serial.print("Initializing SD card...");
     // see if the card is present and can be initialized:
     if (!SD.begin(SDCHIPSELECT)) {
@@ -22,6 +22,7 @@ void initLogging() {
         display.print(F("Fail!")); display.display();
         datalogging=false; //disable logging
         delay(1000);
+        return false;
     }else{
         Serial.println("Card initialized.");
         display.print(F("LOG=")); display.display();
@@ -41,6 +42,7 @@ void initLogging() {
         display.print(datalogging_filename); display.display();
         }
     }
+    return true;
 }
 
 
@@ -86,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("");

controller_teensy/src/main.cpp

@@ -1,12 +1,15 @@
 #include <Arduino.h>
 
-#include "led.h"
 
 #include "definitions.h"
 //#include "structs.h"
-#include "helpfunctions.h"
+
 #include <TimeLib.h> //for teensy rtc 
+#include "helpfunctions.h"
 #include "hoverboard-esc-serial-comm.h"
+#include "led.h"
+
+
 //#include "comms.h"
 #include "display.h"
 #include "logging.h"
@@ -81,52 +84,62 @@ void setup()
 
 
   init_led();
-  led_update(millis());
+  led_testLEDSBlocking();
+  
   
   delay(2000);
   Serial.println("Init Functions");
-  led_update(millis());
+  led_simpeProgress(0,1);
+
+  bool initResult=false;
+  
+  initResult=display_init();
+  led_simpeProgress(1,initResult);
+  
 
   
-  display_init();
-  led_update(millis());
-
-  initLogging();
-  led_update(millis());
+  initResult=initLogging();
+  led_simpeProgress(2,initResult);
 
   escFront.init();
-  led_update(millis());
+  led_simpeProgress(3,true);
   escRear.init();
-  led_update(millis());
+  led_simpeProgress(4,true);
 
   delay(2000);
   Serial.println("Wait finished. Booting..");
-  led_update(millis());
+  led_simpeProgress(5,true);
 
   //init ADS1115
   if (!ADS.begin()) {
     Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!");
+    led_simpeProgress(6,false);
     writeLogComment((unsigned long)millis(), "Error ADS1115 Init");
+  }else{
+    ADS.setGain(0);
+    ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms,  4-> 9ms, 0-> 124ms
+      // also set ADSREADPERIOD to at least the read interval
+    ADS.requestADC(0); //Start requesting a channel
+    led_simpeProgress(6,true);
   }
-  ADS.setGain(0);
-  ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms,  4-> 9ms, 0-> 124ms
-    // also set ADSREADPERIOD to at least the read interval
-  ADS.requestADC(0); //Start requesting a channel
   delay(10);
-  led_update(millis());
+  
 
   setSyncProvider(getTeensy3Time); //See https://www.pjrc.com/teensy/td_libs_Time.html#teensy3
   if (timeStatus()!= timeSet) {
     Serial.println("Unable to sync with the RTC");
+    led_simpeProgress(7,false);
   } else {
     Serial.println("RTC has set the system time");
+    led_simpeProgress(7,true);
   }
-  led_update(millis());
+  
 
 
   writeLogComment(millis(), "Setup Finished");
-  led_update(millis());
+  led_simpeProgress(15,true);
   
+  led_simpleProgressWait(); //wait longer if any errors were displayed with led_simpeProgress()
 }
 
 unsigned long loopmillis;
@@ -234,12 +247,12 @@ void loop() {
   loggingLoop(loopmillis,escFront,escRear);
 
   leds();
-  led_update(loopmillis);
+  led_update(loopmillis,escFront,escRear); //ws2812 led ring
 
   static unsigned long last_display_update=0;
   if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
     last_display_update=loopmillis;
-    display_update();
+    display_update(escFront,escRear);
   }
 }
 
@@ -302,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;
   }
@@ -577,16 +590,44 @@ void leds() {
 }
 
 void readButtons() {
+  bool button_start_longpress_flag=false;
+  bool button_start_shortpress_flag=false;
+
+  static bool button_start_wait_release_flag=false;
+  bool last_button_start_state=button_start_state;
+
   if (loopmillis > button_start_lastchange+DEBOUNCE_TIME) { //wait some time after last change
     if (digitalRead(PIN_START) && !button_start_state) { //start engine button pressed and was not pressed before
       button_start_state=true; //pressed
       button_start_lastchange=loopmillis; //save time for debouncing
     }else if (!digitalRead(PIN_START) && button_start_state) { //released an was pressed before
       button_start_state=false; // not pressed
+      button_start_wait_release_flag=false;
       button_start_lastchange=loopmillis; //save time for debouncing
     }
   }
 
+  if (!button_start_wait_release_flag) { //action not prohibited currently
+    if (button_start_state) { //button is pressed
+      if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
+        button_start_longpress_flag=true;
+        button_start_wait_release_flag=true; //do not trigger again until button released
+      }
+    }else if(!button_start_state && last_button_start_state) { //just released
+      button_start_shortpress_flag=true;
+    }
+  }
+
+  if (button_start_shortpress_flag) {
+    armed=false; //disarm
+    writeLogComment(loopmillis, "Disarmed by button");
+  }
+  if (button_start_longpress_flag) {
+    armed=true; //arm if button pressed long enough
+    writeLogComment(loopmillis, "Armed by button");
+  }
+
+  /* TODO: if works, remove this
   if (button_start_state) { //pressed
     if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
       if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) {  //brake or thottle not pressed, controllers connected
@@ -598,6 +639,7 @@ void readButtons() {
       writeLogComment(loopmillis, "Disarmed by button");
     }
   }
+  */
 
 }