fix speed display and improve display stats

This commit is contained in:
interfisch 2023-06-02 19:18:21 +02:00
parent 73c70f8b95
commit 62c4b15b24
5 changed files with 134 additions and 17 deletions

View File

@ -19,6 +19,7 @@ unsigned long last_log_send=0;
bool log_header_written = false; bool log_header_written = false;
//#define FEEDBACKRECEIVETIMEOUT 500 //#define FEEDBACKRECEIVETIMEOUT 500
//bool controllerFront_connected=false; //bool controllerFront_connected=false;
@ -36,7 +37,7 @@ const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9
const uint16_t calib_brake_min = 2000;//better a bit too high than too low 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 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_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 const uint16_t failsafe_brake_max = 13200; //if adc value goes above this failsafe is triggered
uint16_t ads_throttle_A_raw=0; uint16_t ads_throttle_A_raw=0;
uint16_t ads_throttle_B_raw=0; uint16_t ads_throttle_B_raw=0;
@ -46,6 +47,8 @@ uint16_t ads_control_raw=0;
int16_t throttle_pos=0; int16_t throttle_pos=0;
int16_t brake_pos=0; int16_t brake_pos=0;
unsigned long loopmillis;
#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query #define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query
#define ADCREADPERIOD 10 #define ADCREADPERIOD 10

View File

@ -8,7 +8,6 @@
#include <Fonts/FreeSansBold9pt7b.h> #include <Fonts/FreeSansBold9pt7b.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels #define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels #define SCREEN_HEIGHT 32 // OLED display height, in pixels
@ -17,7 +16,10 @@
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
bool display_init(); bool display_init();
void display_update(); void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_standingOffDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
bool display_init(){ bool display_init(){
@ -47,12 +49,29 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){
display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage()); display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage());
*/ */
if (escFront.getControllerConnected() && escRear.getControllerConnected()) {
if (loopmillis-last_notidle>1000) {
display_standingDisplay(escFront,escRear);
}else{
display_drivingDisplay(escFront,escRear);
}
}else{
display_standingOffDisplay(escFront,escRear);
}
display.display();
}
void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
//## Km/h Display
display.setFont(&FreeMonoBold18pt7b); display.setFont(&FreeMonoBold18pt7b);
display.setTextSize(1); // Normal 1:1 pixel scale display.setTextSize(1); // Normal 1:1 pixel scale
display.setTextColor(SSD1306_WHITE); // Draw white text display.setTextColor(SSD1306_WHITE); // Draw white text
display.setCursor(0,SCREEN_HEIGHT-(SCREEN_HEIGHT-18)/2); // Start at top-left corner display.setCursor(0,SCREEN_HEIGHT-(SCREEN_HEIGHT-18)/2 - 3); // Start at top-left corner
float _speeddisplay=(-escFront.getMeanSpeed()-escRear.getMeanSpeed())/2.0*3.6; float _speeddisplay=(escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0*3.6;
//_speeddisplay=(millis()/1000)%21; //debugging //_speeddisplay=(millis()/1000)%21; //debugging
char buf[8]; char buf[8];
dtostrf(_speeddisplay,1,1,buf); dtostrf(_speeddisplay,1,1,buf);
@ -63,12 +82,98 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){
} }
display.print(strbuf); display.print(strbuf);
display.setCursor(SCREEN_WIDTH-25,SCREEN_HEIGHT-1);
display.setFont(); display.setFont();
display.setCursor(SCREEN_WIDTH-25,SCREEN_HEIGHT-16);
display.print("km/h"); display.print("km/h");
display.display();
//## Trip / Current Consumed Display
display.setCursor(1,SCREEN_HEIGHT-7);
if (((millis()/2500)%2)==0) {
//## Trip
dtostrf(escFront.getTrip(),1,0,buf);
display.print((String)buf);
display.print("m + ");
dtostrf(escRear.getTrip(),1,0,buf);
display.print((String)buf);
display.print("m");
}else{
//## Current Consumed
dtostrf(escFront.getCurrentConsumed(),1,1,buf);
display.print((String)buf);
display.print("Ah + ");
dtostrf(escRear.getCurrentConsumed(),1,1,buf);
display.print((String)buf);
display.print("Ah");
}
}
void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
char buf[8];
display.setFont();
display.setCursor(0,0);
display.print(F("Vbat: ")); display.print(escFront.getFeedback_batVoltage());
display.print(F("/")); display.print(escFront.getFeedback_batVoltage());
display.print(" V");
display.println();
display.print(F("Trip: "));
dtostrf(escFront.getTrip(),1,0,buf);
display.print((String)buf);
display.print("/");
dtostrf(escRear.getTrip(),1,0,buf);
display.print((String)buf);
display.print(" m");
display.println();
display.print(F("Cons. "));
dtostrf(escFront.getCurrentConsumed(),1,2,buf);
display.print((String)buf);
display.print("/");
dtostrf(escRear.getCurrentConsumed(),1,2,buf);
display.print((String)buf);
display.print(" Ah");
display.println();
} }
void display_standingOffDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
//Displayed stuff here when escs are powered off / disconnected
char buf[8];
display.setFont();
display.setCursor(0,0);
display.print(getLogFilename());
display.print(F(" ")); display.print(loopmillis/1000);
display.print(F("s"));
display.println();
display.print(F("ESC F="));
display.print(escFront.getControllerConnected());
display.print(F(" R="));
display.print(escRear.getControllerConnected());
display.println();
display.print("throttle=");
dtostrf(ads_throttle_A_raw,1,0,buf);
display.print((String)buf);
display.print("/");
dtostrf(ads_throttle_B_raw,1,0,buf);
display.print((String)buf);
display.println();
display.print("brake=");
dtostrf(ads_brake_raw,1,0,buf);
display.print((String)buf);
}
#endif #endif

View File

@ -14,12 +14,13 @@ bool initLogging();
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear); void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);
void writeLogComment(unsigned long time, String msg); void writeLogComment(unsigned long time, String msg);
bool initLogging() { bool initLogging() {
Serial.print("Initializing SD card..."); Serial.print("Initializing SD card...");
// see if the card is present and can be initialized: // see if the card is present and can be initialized:
if (!SD.begin(SDCHIPSELECT)) { if (!SD.begin(SDCHIPSELECT)) {
Serial.println("Card failed, or not present"); Serial.println("Card failed, or not present");
display.print(F("Fail!")); display.display(); display.print(F("SD Init Fail!")); display.display();
datalogging=false; //disable logging datalogging=false; //disable logging
delay(1000); delay(1000);
return false; return false;
@ -64,7 +65,7 @@ void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm
dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,"); dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,"); dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
dataFile.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,"); dataFile.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
dataFile.println("currentAll,throttle,brake,speed,trip,currentConsumed,motorenabled,disarmedByDelay"); dataFile.println("currentAll,throttle,brake,speed,trip_Front,trip_Rear,currentConsumed_Front,currentConsumed_Rear");
dataFile.print("#TIMESTAMP:"); dataFile.println(now()); dataFile.print("#TIMESTAMP:"); dataFile.println(now());
logging_headerWritten=true; logging_headerWritten=true;
} }
@ -77,10 +78,10 @@ void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm
dataFile.print(escFront.getFiltered_curR(),3); dataFile.print(";"); dataFile.print(escFront.getFiltered_curR(),3); dataFile.print(";");
dataFile.print(escRear.getFiltered_curL(),3); dataFile.print(";"); dataFile.print(escRear.getFiltered_curL(),3); dataFile.print(";");
dataFile.print(escRear.getFiltered_curR(),3); dataFile.print(";"); dataFile.print(escRear.getFiltered_curR(),3); dataFile.print(";");
dataFile.print(escFront.getFeedback_speedL_meas()); dataFile.print(";"); //Todo: check if speed for R wheels needs to be negated dataFile.print(escFront.getFeedback_speedL_meas()); dataFile.print(";"); //+ //Todo: check if speed for R wheels needs to be negated
dataFile.print(escFront.getFeedback_speedR_meas()); dataFile.print(";"); dataFile.print(escFront.getFeedback_speedR_meas()); dataFile.print(";"); //-
dataFile.print(escRear.getFeedback_speedL_meas()); dataFile.print(";"); dataFile.print(escRear.getFeedback_speedL_meas()); dataFile.print(";"); //+
dataFile.print(escRear.getFeedback_speedR_meas()); dataFile.print(";"); dataFile.print(escRear.getFeedback_speedR_meas()); dataFile.print(";"); //-
dataFile.print(escFront.getFeedback_boardTemp()); dataFile.print(";"); dataFile.print(escFront.getFeedback_boardTemp()); dataFile.print(";");
dataFile.print(escRear.getFeedback_boardTemp()); dataFile.print(";"); dataFile.print(escRear.getFeedback_boardTemp()); dataFile.print(";");
dataFile.print(escFront.getFeedback_batVoltage()); dataFile.print(";"); dataFile.print(escFront.getFeedback_batVoltage()); dataFile.print(";");
@ -88,9 +89,11 @@ void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm
dataFile.print(filtered_currentAll,3); dataFile.print(";"); dataFile.print(filtered_currentAll,3); dataFile.print(";");
dataFile.print(throttle_pos); dataFile.print(";"); dataFile.print(throttle_pos); dataFile.print(";");
dataFile.print(brake_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.getTrip()); dataFile.print(";");
dataFile.print(escRear.getTrip()); dataFile.print(";");
dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(";"); dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(";");
dataFile.print(escRear.getCurrentConsumed(),3); dataFile.print(";");
dataFile.println(""); dataFile.println("");
dataFile.close(); dataFile.close();
} }
@ -114,4 +117,8 @@ void writeLogComment(unsigned long time, String msg) {
} }
} }
String getLogFilename() {
return datalogging_filename;
}
#endif #endif

@ -1 +1 @@
Subproject commit 00b432942f437fdbb285f04a1847d3bebc390044 Subproject commit 8d180debf7633d3a8ff86a588c00b199c0876225

View File

@ -11,8 +11,10 @@
//#include "comms.h" //#include "comms.h"
String getLogFilename();
#include "display.h" #include "display.h"
#include "logging.h" #include "logging.h"
#include "ADS1X15.h" #include "ADS1X15.h"
@ -142,7 +144,7 @@ void setup()
led_simpleProgressWait(); //wait longer if any errors were displayed with led_simpeProgress() led_simpleProgressWait(); //wait longer if any errors were displayed with led_simpeProgress()
} }
unsigned long loopmillis;
// ########################## LOOP ########################## // ########################## LOOP ##########################
void loop() { void loop() {
@ -316,7 +318,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; last_notidle=loopmillis;
reverse_enabled=false; reverse_enabled=false;
} }