add logging

controller_teensy/src/main.cpp

@@ -15,9 +15,13 @@ Tennsy Pin, Pin Name, Connected to
 #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 50 //ms. delay for sending speed and steer data to motor controller via serial
-#define RECEIVEPERIOD 50 //ms
+
+bool controllers_connected=false;
 
 #define PIN_THROTTLE A7
 const uint16_t calib_throttle_min = 400; //better a bit too high than too low
@@ -46,8 +50,7 @@ unsigned long last_adcread=0;
 unsigned long last_send = 0;
 unsigned long last_receive = 0;
 
-float avg_currentL=0;
+float filtered_currentAll=0;
-float avg_currentR=0;
 
 int16_t cmd_send=0;
 int16_t last_cmd_send=0;
@@ -102,6 +105,9 @@ typedef struct{
   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;
@@ -113,6 +119,9 @@ bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
 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 SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
 {
   // Create command
@@ -194,10 +203,12 @@ bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
 void setup() 
 {
 
-  Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)
+  Serial.begin(SERIAL_BAUD); //Debug and Program
 
-  Serial2.begin(SERIAL_CONTROL_BAUD); //control
+  Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
-  Serial3.begin(SERIAL_CONTROL_BAUD); //control
+
+  Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
+  Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
 
   pinMode(PIN_THROTTLE, INPUT);
   pinMode(PIN_BRAKE, INPUT);
@@ -242,27 +253,41 @@ void loop() {
 
     if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
       throttle_pos/=2;
-      digitalWrite(PIN_MODE_LEDG,HIGH); //Green
+      digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
-      digitalWrite(PIN_MODE_LEDR,LOW);
+      digitalWrite(PIN_MODE_LEDR,HIGH);
     }else{ //button not pushed in
-      digitalWrite(PIN_MODE_LEDG,LOW);
+      digitalWrite(PIN_MODE_LEDG,HIGH);
-      digitalWrite(PIN_MODE_LEDR,HIGH); //Red
+      digitalWrite(PIN_MODE_LEDR,LOW); //Red
     }
     
   }
 
+  #define FEEDBACKRECEIVETIMEOUT 500
+  if ( (loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT) | (loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT) ) { //timeout of at least one controller
+    throttle_pos=0;
+    brake_pos=0;
+    controllers_connected=false;
+  }else if(!controllers_connected) { //not timeouted but was before
+    controllers_connected=true;
+    writeLogHeader(Serial1); //connection recovered, write log header
+  }
+  
 
   if (newData2) {
     motorparamsFront.cur_pos++;
     motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
     motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
     motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
+    motorparamsFront.millis=loopmillis;
+    log_update=true;
   }
   if (newData3) {
     motorparamsRear.cur_pos++;
     motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
     motorparamsRear.curL_DC[motorparamsRear.cur_pos] = FeedbackRear.curL_DC;
     motorparamsRear.curR_DC[motorparamsRear.cur_pos] = FeedbackRear.curR_DC;
+    motorparamsRear.millis=loopmillis;
+    log_update=true;
   }
 
   if (loopmillis - last_send > SENDPERIOD) {
@@ -274,16 +299,16 @@ void loop() {
     float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
 
     float freewheel_current=0.1+brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
-    float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
+    float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
-    float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
+    motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
-    float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
+    motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
-    float filtered_curRL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
+    motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
-    float filtered_curRR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
+    motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
 
-    float filtered_currentFront=min(filtered_curFL,filtered_curFR);
+    float filtered_currentFront=min(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
-    float filtered_currentRear=min(filtered_curRL,filtered_curRR);
+    float filtered_currentRear=min(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
 
-    float filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
+    filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
 
     if (throttle_pos>=last_cmd_send) { //accelerating
       cmd_send = throttle_pos; //if throttle higher than apply throttle directly
@@ -306,9 +331,17 @@ void loop() {
     SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
     SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
 
-    Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
+    log_update=true;
+    //Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
     
   }
+
+  #define LOGMININTERVAL 20 //minimum interval (ms) to send logs
+  if (log_update && loopmillis>last_log_send+LOGMININTERVAL) {
+    last_log_send=loopmillis;
+    log_update=false;
+    writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
+  }
 }
 
 int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
@@ -334,3 +367,41 @@ float filterMedian(int16_t* values) {
 
   return mean;
 }
+
+
+
+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("temp_Front, temp_Rear, vbat_Front, vbat_Rear, ");
+  SerialRef.print("speed_FrontL, speed_FrontR, speed_RearL, speed_RearR, ");
+  SerialRef.print("currentAll, throttle, brake"); Serial.println();
+}
+
+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); 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(", ");
+  SerialRef.print(fbfront.speedR_meas); SerialRef.print(", ");
+  SerialRef.print(fbrear.speedL_meas); SerialRef.print(", ");
+  SerialRef.print(fbrear.speedR_meas); SerialRef.print(", ");
+
+  SerialRef.print(fbfront.boardTemp); SerialRef.print(", ");
+  SerialRef.print(fbrear.boardTemp); SerialRef.print(", ");
+  SerialRef.print(fbfront.batVoltage); SerialRef.print(", ");
+  SerialRef.print(fbrear.batVoltage); SerialRef.print(", ");
+
+  SerialRef.print(currentAll,3); SerialRef.print(", ");
+  SerialRef.print(throttle); SerialRef.print(", ");
+  SerialRef.print(brake); SerialRef.println();
+}