bobbycar/controller_teensy/include/logging.h

#ifndef _LOGGING_H_
#define _LOGGING_H_

// SD Card Logging

#include <SPI.h>
#include <SD.h> //Format sd cart with FAT or FAT16. FAT32 for >1GB Cards on Teensy4.1

//#define SDCHIPSELECT 14
boolean datalogging=true;
String datalogging_filename="UNKNOWN.txt";

bool initLogging();
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);
void writeLogComment(unsigned long time, String msg);


bool initLogging() {
    Serial.print("Initializing SD card...");
    // see if the card is present and can be initialized:
    if (!SD.begin(BUILTIN_SDCARD)) {
        Serial.println("Card failed, or not present");
        display.println(F("SD Init Fail!")); display.display();
        datalogging=false; //disable logging
        delay(1000);
        return false;
    }else{
        Serial.println("Card initialized.");
        display.print(F("LOG=")); display.display();
        if (datalogging){
          int filenumber=0;
          char buffer[6];
          sprintf(buffer, "%04d", filenumber);
          datalogging_filename="LOG_"+String(buffer)+".TXT";
          while(SD.exists(datalogging_filename.c_str()) && filenumber<10000) {
              Serial.print(datalogging_filename); Serial.println(" exists");
              filenumber++;
              sprintf(buffer, "%04d", filenumber);
              datalogging_filename="LOG_"+String(buffer)+".TXT";

          }      
          Serial.print(datalogging_filename); Serial.println(" is free");
          display.print(datalogging_filename); display.println(); display.display();
        }
    }
    return true;
}


void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {

  static unsigned long last_datalogging_write=0;
  static boolean logging_headerWritten=false;
  if (datalogging) {
    #define LOGGINGINTERVAL 100
    if (loopmillis-last_datalogging_write>LOGGINGINTERVAL)
    {
      last_datalogging_write=loopmillis;
      
      File dataFile = SD.open(datalogging_filename.c_str(), FILE_WRITE);


      if (dataFile) { // if the file is available, write to it
        if (!logging_headerWritten) {
          dataFile.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
          dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
          dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
          dataFile.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
          dataFile.print("currentAll,throttle,brake,speed,trip_Front,trip_Rear,trip,currentConsumed_Front,currentConsumed_Rear,currentConsumed,watthoursConsumed,");
          dataFile.println("temp_ESCFront,temp_ESCRear,temp_Air,looptime_duration");
          dataFile.print("#TIMESTAMP:"); dataFile.println(now());
          logging_headerWritten=true;
        }
        dataFile.print(loopmillis/1000.0,3); dataFile.print(",");
        dataFile.print(escFront.getCmdL());  dataFile.print(",");
        dataFile.print(escFront.getCmdR());  dataFile.print(",");
        dataFile.print(escRear.getCmdL());  dataFile.print(",");
        dataFile.print(escRear.getCmdR());  dataFile.print(",");
        dataFile.print(escFront.getFiltered_curL(),3);  dataFile.print(",");
        dataFile.print(escFront.getFiltered_curR(),3);  dataFile.print(",");
        dataFile.print(escRear.getFiltered_curL(),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_speedR_meas());  dataFile.print(","); //-
        dataFile.print(escRear.getFeedback_speedL_meas());  dataFile.print(","); //+
        dataFile.print(escRear.getFeedback_speedR_meas());  dataFile.print(","); //-
        dataFile.print(escFront.getFeedback_boardTemp());  dataFile.print(",");
        dataFile.print(escRear.getFeedback_boardTemp());  dataFile.print(",");
        dataFile.print(escFront.getFeedback_batVoltage());  dataFile.print(",");
        dataFile.print(escRear.getFeedback_batVoltage());  dataFile.print(",");
        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.getTrip());  dataFile.print(",");
        dataFile.print(escRear.getTrip());  dataFile.print(",");
        dataFile.print(trip);  dataFile.print(",");
        dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(",");
        dataFile.print(escRear.getCurrentConsumed(),3); dataFile.print(",");
        dataFile.print(currentConsumed,3); dataFile.print(",");
        dataFile.print(watthoursConsumed,3); dataFile.print(",");
        dataFile.print(temp_ESCFront,2); dataFile.print(",");
        dataFile.print(temp_ESCRear,2); dataFile.print(",");
        dataFile.print(temp_Air,2); dataFile.print(",");
        dataFile.print(looptime_duration); //dataFile.print(",");
        looptime_duration=0; //reset
        
        dataFile.println("");
        dataFile.close();
      }else{ //dataFile not available
        error_sdfile_unavailable=true;
      }
    }
  }
}

void writeLogComment(unsigned long time, String msg) {
    //SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
    if (datalogging) {
        File dataFile = SD.open(datalogging_filename.c_str(), FILE_WRITE);

        if (dataFile) { // if the file is available, write to it
            dataFile.print("#");
            dataFile.print(time/1000.0,3); 
            dataFile.print(","); 
            dataFile.print(msg); 
            dataFile.println();
            dataFile.close();
        }
    }
}

String getLogFilename() {
  return datalogging_filename;
}

bool loadTripSD()
{
  //stats.txt containt lines with driven distance or consume current.
  //the lowest line is the latest data
  //data is usually written rather slowly

  File myFile = SD.open("stats.txt", FILE_WRITE);
  if (myFile) {
    myFile.print("ini=");
    myFile.println(now());
    myFile.flush();
    delay(100);
    myFile.close();
    Serial.println("TripSD: time written");
  }else{
    return false;
  }


  myFile = SD.open("stats.txt");
  if (myFile) {
    while (myFile.available()) {
      String line=myFile.readStringUntil('\n');      
      Serial.print("TripSD Read Line:");
      Serial.println(line);
      if (line.substring(0,4)=="trp="){
        overallTrip=(line.substring(4)).toFloat();
      }else if (line.substring(0,4)=="occ="){
        overallCurrentConsumed=(line.substring(4)).toFloat();
      }else if (line.substring(0,4)=="owh="){
        overallWatthoursConsumed=(line.substring(4)).toFloat();
      }else if (line.substring(0,4)=="vlt="){
        lastTripVoltage=(line.substring(4)).toFloat();
      }
    }
    Serial.print("TripSD trip:");
    Serial.println(overallTrip);
    display.print(F("TripSD=")); display.println(overallTrip);
    Serial.print("TripSD current:");
    Serial.println(overallCurrentConsumed);
    Serial.print("TripSD watthours:");
    Serial.println(overallWatthoursConsumed);
    display.print(F("Cur. SD=")); display.println(overallCurrentConsumed); display.display();
    Serial.print("TripSD voltage:");
    Serial.println(lastTripVoltage);
    display.print(F("Volt. SD=")); display.println(lastTripVoltage); display.display();

    myFile.flush();
    delay(10);
    myFile.close();
  }else{
    return false;
  }
  return true;
}

void writeTrip(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
  if (datalogging) {
    File myFile = SD.open("stats.txt", FILE_WRITE);
    if (myFile) {
      myFile.print("trp=");
      myFile.println(overallTrip);
      myFile.print("occ=");
      myFile.println(overallCurrentConsumed);
      myFile.print("owh=");
      myFile.println(overallWatthoursConsumed);
      myFile.print("vlt=");
      float _voltage=(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0;
      myFile.println(_voltage,2);
      myFile.close();
    }
  }
}

void resetTrip() {
  if (datalogging) {
    File myFile = SD.open("stats.txt", FILE_WRITE);
    if (myFile) {
      myFile.print("trp=");
      myFile.println(0);
      myFile.print("occ=");
      myFile.println(0);
      myFile.print("owh=");
      myFile.println(0);
      myFile.print("vlt=");
      myFile.println(0);
      myFile.flush();
      delay(10);
      myFile.close();
    }
  }
}

#endif