modify code to use hoverboard-esc-serial-comm and add sd card via SPI. untested

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "hoverboard-firmware-hack-foc-serial-esc"]
 	path = hoverboard-firmware-hack-foc-serial-esc
 	url = https://repos.ctdo.de/interfisch/hoverboard-firmware-hack-foc-serial-esc
+[submodule "controller_teensy/lib/hoverboard-esc-serial-comm"]
+	path = controller_teensy/lib/hoverboard-esc-serial-comm
+	url = git@git.ctdo.de:interfisch/hoverboard-esc-serial-comm.git

controller_teensy/include/comms.h

@@ -106,7 +106,7 @@ void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _lo
 }
 
 
-
+/*
 void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
   SerialRef.print("#TIMESTAMP:");
   SerialRef.println(now()); 
@@ -156,7 +156,7 @@ void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
 {
   SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
 }
-
+*/
 
 
 #endif

controller_teensy/include/definitions.h

@@ -11,7 +11,7 @@
 bool log_update=true;
 unsigned long last_log_send=0;
 
-#define SENDPERIOD 20 //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
 #define LOGMAXINTERVAL 10000  //maximum time (ms) after which data is send
 
@@ -19,10 +19,10 @@ unsigned long last_log_send=0;
 bool log_header_written = false;
 
 
-#define FEEDBACKRECEIVETIMEOUT 500
+//#define FEEDBACKRECEIVETIMEOUT 500
 
-bool controllerFront_connected=false;
+//bool controllerFront_connected=false;
-bool controllerRear_connected=false;
+//bool controllerRear_connected=false;
 bool controllers_connected=false;
 
 #define PIN_THROTTLE A7
@@ -90,14 +90,13 @@ unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
 
 #define PIN_LATCH_ENABLE A6
 
-#define PIN_MODE_SWITCH 3
 #define PIN_MODE_LEDG 4
 #define PIN_MODE_LEDR 5
 
 
 
-unsigned long last_send = 0;
+//unsigned long last_send = 0;
-unsigned long last_receive = 0;
+//unsigned long last_receive = 0;
 
 float filtered_currentAll=0;
 
@@ -123,4 +122,7 @@ bool armed = false; //cmd output values forced to 0 if false
 
 #define DISPLAYUPDATEPERIOD 100
 
+
+
+
 #endif

controller_teensy/include/display.h

@@ -13,24 +13,30 @@
 Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
 
 void display_init();
-void display_test();
+void display_update();
 
 void display_init(){
   if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
     Serial.println(F("SSD1306 allocation failed"));
   }
   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();
 }
 
-void display_test(){
+void display_update(){
   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.println(F("Hello Welt"));
+  display.print(F("Speed   : ")); display.println(meanSpeedms);
-  display.println(millis());
+  display.print(F("Throttle: ")); display.println(throttle_pos);
-  display.println(now());
+  display.print(F("Brake   : ")); display.println(brake_pos);
+  display.print(F("Current : ")); display.println(filtered_currentAll);
+  
+  
   display.display();
 }
 

controller_teensy/include/helpfunctions.h

@@ -1,7 +1,7 @@
 #ifndef _HELPFUNCTIONS_H_
 #define _HELPFUNCTIONS_H_
 
-#include "definitions.h"
+//#include "definitions.h"
 
 int sort_desc(const void *cmp1, const void *cmp2);
 float filterMedian(int16_t* values);

controller_teensy/include/logging.h

@@ -0,0 +1,115 @@
+#ifndef _LOGGING_H_
+#define _LOGGING_H_
+
+// SD Card Logging
+
+#include <SPI.h> //SCK=13, MISO=12, MOSI=11
+#include <SD.h> //Format sd cart with FAT or FAT16
+
+#define SDCHIPSELECT 15
+boolean datalogging=true;
+String datalogging_filename="UNKNOWN.txt";
+
+void initLogging();
+void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);
+void writeLogComment(unsigned long time, String msg);
+
+void initLogging() {
+    Serial.print("Initializing SD card...");
+    // see if the card is present and can be initialized:
+    if (!SD.begin(SDCHIPSELECT)) {
+        Serial.println("Card failed, or not present");
+        display.print(F("Fail!")); display.display();
+        datalogging=false; //disable logging
+        delay(1000);
+    }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) && 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.display();
+        }
+    }
+}
+
+
+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, 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.println("currentAll,throttle,brake,speed,trip,currentConsumed,motorenabled,disarmedByDelay");
+          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(meanSpeedms);  dataFile.print(";");
+        dataFile.print(escFront.getTrip());  dataFile.print(";");
+        dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(";");
+        dataFile.println("");
+        dataFile.close();
+      }
+    }
+  }
+}
+
+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, 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();
+        }
+    }
+}
+
+#endif

controller_teensy/lib/hoverboard-esc-serial-comm

@@ -0,0 +1 @@
+Subproject commit 00b432942f437fdbb285f04a1847d3bebc390044

controller_teensy/platformio.ini

@@ -23,3 +23,4 @@ build_flags =
 lib_deps =
     robtillaart/ADS1X15@^0.3.9
     adafruit/Adafruit SSD1306@^2.5.7
+    arduino-libraries/SD@^1.2.4

controller_teensy/src/main.cpp

@@ -1,14 +1,22 @@
 #include <Arduino.h>
 
 #include "definitions.h"
-#include "structs.h"
+//#include "structs.h"
 #include "helpfunctions.h"
 #include <TimeLib.h> //for teensy rtc 
-#include "comms.h"
+#include "hoverboard-esc-serial-comm.h"
+//#include "comms.h"
 #include "display.h"
-
+#include "logging.h"
 #include "ADS1X15.h"
 
+
+ESCSerialComm escFront(Serial2);
+ESCSerialComm escRear(Serial3);
+//Serial1 = TX1=1, RX1=0
+//Serial2 = TX2=10, RX2=9
+//Serial3 = TX3=8, RX3=7
+
 ADS1115 ADS(0x48);
 
 /*
@@ -25,7 +33,8 @@ Tennsy Pin, Pin Name, Connected to
 void readADS();
 void readADC();
 void failChecks();
-void sendCMD();
+//void sendCMD();
+void calculateSetSpeed();
 void checkLog();
 
 void leds();
@@ -46,8 +55,8 @@ void setup()
 
   Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
 
-  Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
+  //Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
-  Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
+  //Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
 
   pinMode(PIN_THROTTLE, INPUT);
   pinMode(PIN_BRAKE, INPUT);
@@ -55,6 +64,8 @@ void setup()
 
   pinMode(PIN_LED_START, OUTPUT); //Active High
   
+
+  //TODO: remove mode button things
   pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low
   digitalWrite(PIN_MODE_LEDG,LOW);
   pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low
@@ -62,18 +73,24 @@ void setup()
 
   pinMode(PIN_LATCH_ENABLE, OUTPUT);
   digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
-  pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
   
+  delay(2000);
+  Serial.println("Init Functions");
   
   display_init();
 
+  initLogging();
+
+  escFront.init();
+  escRear.init();
+
   delay(2000);
   Serial.println("Wait finished. Booting..");
 
   //init ADS1115
   if (!ADS.begin()) {
     Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!");
-    writeLogComment(Serial1,(unsigned long)millis(), "Error ADS1115 Init");
+    writeLogComment((unsigned long)millis(), "Error ADS1115 Init");
   }
   ADS.setGain(0);
   ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms,  4-> 9ms, 0-> 124ms
@@ -89,9 +106,12 @@ void setup()
   }
 
 
+  writeLogComment(millis(), "Setup Finished");
+  
 }
 
 unsigned long loopmillis;
+
 // ########################## LOOP ##########################
 void loop() {
   //Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms
@@ -99,6 +119,7 @@ void loop() {
   loopmillis=millis(); //read millis for this cycle
 
 
+  /*
   // ____ Debugging pending serial byted for feedback
   static int s2availmax=0;
   int _a2=Serial2.available();
@@ -133,6 +154,8 @@ void loop() {
   if (newData3) {
     updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
   }
+  */
+
 
 
 
@@ -163,9 +186,19 @@ void loop() {
 
   failChecks();  
 
+  static unsigned long last_calculateSetSpeed=0;
+  if (loopmillis - last_calculateSetSpeed > SENDPERIOD) {
+    calculateSetSpeed();
+  }
+
+  escFront.update(loopmillis);
+  escRear.update(loopmillis);
+
+  /*  TODO: remove this if, because everything contained in esc.update()
   if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers    
     last_send=loopmillis;
-    sendCMD();    
+    //sendCMD();    
+    
 
     //Update speed and trip
     float _meanRPM=(FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas)/4.0;
@@ -175,16 +208,18 @@ void loop() {
     //mah consumed
     currentConsumed += (motorparamsFront.filtered_curL+motorparamsFront.filtered_curR+motorparamsRear.filtered_curL+motorparamsRear.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours
   }
+  */
 
   //If needed write log to serial port
-  checkLog();
+  //checkLog(); //TODO remove
+  loggingLoop(loopmillis,escFront,escRear);
 
   leds();
 
   static unsigned long last_display_update=0;
   if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
     last_display_update=loopmillis;
-    display_test();
+    display_update();
   }
 }
 
@@ -263,13 +298,13 @@ void readADC() {
   //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
   //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
 
-  
+  /*
   if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
     if (speedmode!=SPEEDMODE_SLOW) {
       speedmode=SPEEDMODE_SLOW;
       max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
       if (loopmillis>WRITE_HEADER_TIME) {
-        writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_SLOW");
+        //writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_SLOW");
       }
     }
   }else{ //button not pushed in
@@ -277,20 +312,24 @@ void readADC() {
       speedmode=SPEEDMODE_NORMAL;
       max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE;
       if (loopmillis>WRITE_HEADER_TIME) {
-        writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_NORMAL");
+        //writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_NORMAL");
       }
     }
   } 
+  */
 
 
+  /*
   if (speedmode==SPEEDMODE_SLOW) {
     throttle_pos/=2;
   }
+  */
 
 
 }
 
 void failChecks() {
+  /*
   if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT  ) { //controller disconnected
     if (controllerFront_connected) { //just got disconnected
       controllerFront_connected=false;
@@ -311,9 +350,9 @@ void failChecks() {
   }else if(!controllerRear_connected  && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
     controllerRear_connected=true;
     writeLogComment(Serial1,loopmillis, "Controller Rear connected");
-  }
+  }*/
 
-  controllers_connected=controllerFront_connected & controllerRear_connected;
+  controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();
 
   //ADC Range Check
   if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
@@ -322,7 +361,7 @@ void failChecks() {
   if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
     if (!error_throttle_outofrange) {
       error_throttle_outofrange=true;
-      writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
+      writeLogComment(loopmillis, "Error Throttle ADC Out of Range");
       
     }
     //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
@@ -333,8 +372,7 @@ void failChecks() {
   if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
     if(!error_brake_outofrange) {
       error_brake_outofrange=true;
-      writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
+      writeLogComment(loopmillis, "Error Brake ADC Out of Range");
-      
     }
     //Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw);
   }
@@ -342,7 +380,7 @@ void failChecks() {
   if (loopmillis-last_adsread > ADS_MAX_READ_INTERVAL) {
     if (!error_ads_max_read_interval) {
       error_ads_max_read_interval=true;
-      writeLogComment(Serial1,loopmillis, "Error ADS Max read interval");
+      writeLogComment(loopmillis, "Error ADS Max read interval");
     }
     //Serial.print("Error ADS Max read interval=");Serial.println(loopmillis-last_adsread);
   }
@@ -353,13 +391,64 @@ void failChecks() {
   }
 }
 
-void sendCMD() {
+void calculateSetSpeed(){
-  /* ## FOR REFERENCE:
+
-  int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
+  int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);
-  int16_t brake_cmdreduce_proportional=100; //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)
+
-  float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
+  float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
-  float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
+
-  */
+  int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*SENDPERIOD/1000)); //reduce cmd value every cycle
+
+  float freewheel_current=startbrakecurrent_offset-brake_pos_expo*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=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average)
+  
+  
+  float filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR());
+  float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_curR());
+
+  filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current
+
+  if (throttle_pos>=last_cmd_send) { //accelerating
+    cmd_send += constrain(throttle_pos-cmd_send,0,max_acceleration_rate*SENDPERIOD/1000); //if throttle higher than last applied value, apply throttle directly
+  }else{ //freewheeling or braking
+    if (filtered_currentAll>freewheel_current) { //drive current too high
+      cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
+    }
+    cmd_send-=max(minimum_constant_cmd_reduce,cmdreduce_constant); //reduce slowly anyways
+    
+  }
+
+  cmd_send=constrain(cmd_send,0,1000);
+
+  last_cmd_send=cmd_send;
+
+  int16_t cmd_send_toMotor=constrain(cmd_send*  (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking"
+
+  
+  if (reverse_enabled) {
+    cmd_send_toMotor-=brake_pos*REVERSE_SPEED;
+  }
+
+  if (!controllers_connected || !armed) { //controllers not connected or not armed
+    cmd_send=0;
+    cmd_send_toMotor=0; //safety off
+  }
+
+  escFront.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
+  escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
+
+  log_update=true;
+
+}
+
+/*
+void sendCMD() { //TODO: remove complete function because replaced by calculateSetSpeed()
+  // ## FOR REFERENCE:
+  //int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
+  //int16_t brake_cmdreduce_proportional=100; //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)
+  //float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
+  //float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
+  
   int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);
 
   float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
@@ -407,27 +496,32 @@ void sendCMD() {
   }
 
   //apply throttle command to all motors
+  
   motorparamsFront.cmdL=cmd_send_toMotor;
   motorparamsFront.cmdR=cmd_send_toMotor;
   motorparamsRear.cmdL=cmd_send_toMotor;
   motorparamsRear.cmdR=cmd_send_toMotor;
   
+
   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()
     
-  }/*else if(loopmillis>last_log_send+LOGMININTERVAL){
+  }//else if(loopmillis>last_log_send+LOGMININTERVAL){
-    //Serial.print(throttle_raw);  Serial.println();
+  //  //Serial.print(throttle_raw);  Serial.println();
-    Serial.print(linearizeThrottle(throttle_raw));  Serial.println();
+  //  Serial.print(linearizeThrottle(throttle_raw));  Serial.println();
-    last_log_send=loopmillis;
+  //  last_log_send=loopmillis;
-  }*/
+  //}
-}
+}*/
 
 
-void checkLog() {
+/*
+void checkLog() { //TODO: remove
   if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
     writeLogInfo(Serial1);
     writeLogHeader(Serial1);
@@ -440,6 +534,7 @@ void checkLog() {
     writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
   }
 }
+*/
 
 
 
@@ -477,11 +572,11 @@ void readButtons() {
     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
         armed=true; //arm if button pressed long enough
-        writeLogComment(Serial1,loopmillis, "Armed by button");
+        writeLogComment(loopmillis, "Armed by button");
       }      
     }else if (armed){ //not pressed long enough and is armed
       armed=false; //disarm
-      writeLogComment(Serial1,loopmillis, "Disarmed by button");
+      writeLogComment(loopmillis, "Disarmed by button");
     }
   }