From 378832ecb6f54904b140066e63a26dda40763817 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sat, 20 Mar 2021 23:03:13 +0100
Subject: [PATCH] fix logging

---
 controller_teensy/src/main.cpp | 37 ++++++++++++++++++++--------------
 1 file changed, 22 insertions(+), 15 deletions(-)

diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 1e4f9f6..5d3d851 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -19,7 +19,8 @@ Tennsy Pin, Pin Name, Connected to
 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 SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
+#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
 
 bool controllers_connected=false;
 
@@ -34,7 +35,7 @@ int16_t throttle_pos=0;
 int16_t brake_pos=0;
 
 unsigned long last_adcread=0;
-#define ADCREADPERIOD 25
+#define ADCREADPERIOD 10
 
 #define PIN_START A9
 #define PIN_LED_START 2 //Enginge start led
@@ -244,9 +245,9 @@ void loop() {
     uint16_t brake_raw = analogRead(PIN_BRAKE);
     brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
 
-    int16_t throttlebreak_pos = throttle_pos-brake_pos*2;
-    throttle_pos=constrain(throttlebreak_pos,0,1000); //reduce throttle_when applying brake
-    brake_pos=constrain(-throttlebreak_pos/2,0,1000);
+    int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
+    throttle_pos=constrain(throttlebreak_pos,0,1000);
+    brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos
 
     //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
     //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
@@ -295,7 +296,7 @@ void loop() {
     last_send=loopmillis;
 
     int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
-    #define MAXBREAKCURRENT 20
+    #define MAXBREAKCURRENT 5
     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
@@ -319,7 +320,11 @@ void loop() {
       cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
       cmd_send=constrain(cmd_send,0,1000);
     }
-    
+
+    if (!controllers_connected) { //controllers not connected
+      cmd_send=0; //safety off
+    }
+
     last_cmd_send=cmd_send;
 
     //apply throttle command to all motors
@@ -328,15 +333,17 @@ void loop() {
     motorparamsRear.cmdL=cmd_send;
     motorparamsRear.cmdR=cmd_send;
 
-    SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
-    SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
+    if (controllers_connected) {
+      SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
+      SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
 
-    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();
+      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;
@@ -380,7 +387,7 @@ 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)
 {
-  SerialRef.print(time/1000.0); SerialRef.print(","); //time in seconds
+  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(",");
@@ -398,8 +405,8 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
 
   SerialRef.print(fbfront.boardTemp/10.0); SerialRef.print(","); //in degC
   SerialRef.print(fbrear.boardTemp/10.0); SerialRef.print(","); //in degC
-  SerialRef.print(fbfront.batVoltage); SerialRef.print(",");
-  SerialRef.print(fbrear.batVoltage); SerialRef.print(",");
+  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(",");