From 7516b76d2a366e0d5b1044bf5efee317bfc8c40c Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 16 Apr 2023 00:13:02 +0200 Subject: [PATCH] modify code to use hoverboard-esc-serial-comm and add sd card via SPI. untested --- controller_teensy/include/comms.h | 4 +- controller_teensy/include/definitions.h | 16 +- controller_teensy/include/display.h | 16 +- controller_teensy/include/helpfunctions.h | 2 +- controller_teensy/include/logging.h | 115 ++++++++++++ .../lib/hoverboard-esc-serial-comm | 2 +- controller_teensy/platformio.ini | 3 +- controller_teensy/src/main.cpp | 167 ++++++++++++++---- 8 files changed, 272 insertions(+), 53 deletions(-) create mode 100644 controller_teensy/include/logging.h diff --git a/controller_teensy/include/comms.h b/controller_teensy/include/comms.h index ebb890e..c88af5d 100644 --- a/controller_teensy/include/comms.h +++ b/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 \ No newline at end of file diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index 5657a3e..52db129 100644 --- a/controller_teensy/include/definitions.h +++ b/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 controllerRear_connected=false; +//bool controllerFront_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_receive = 0; +//unsigned long last_send = 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 \ No newline at end of file diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index 57c13b6..6fbb740 100644 --- a/controller_teensy/include/display.h +++ b/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.println(millis()); - display.println(now()); + display.print(F("Speed : ")); display.println(meanSpeedms); + display.print(F("Throttle: ")); display.println(throttle_pos); + display.print(F("Brake : ")); display.println(brake_pos); + display.print(F("Current : ")); display.println(filtered_currentAll); + + display.display(); } diff --git a/controller_teensy/include/helpfunctions.h b/controller_teensy/include/helpfunctions.h index 337b90f..6624edc 100644 --- a/controller_teensy/include/helpfunctions.h +++ b/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); diff --git a/controller_teensy/include/logging.h b/controller_teensy/include/logging.h new file mode 100644 index 0000000..2f8fad6 --- /dev/null +++ b/controller_teensy/include/logging.h @@ -0,0 +1,115 @@ +#ifndef _LOGGING_H_ +#define _LOGGING_H_ + +// SD Card Logging + +#include //SCK=13, MISO=12, MOSI=11 +#include //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 \ No newline at end of file diff --git a/controller_teensy/lib/hoverboard-esc-serial-comm b/controller_teensy/lib/hoverboard-esc-serial-comm index 91bfeff..00b4329 160000 --- a/controller_teensy/lib/hoverboard-esc-serial-comm +++ b/controller_teensy/lib/hoverboard-esc-serial-comm @@ -1 +1 @@ -Subproject commit 91bfeffcb33448f45b17a753ef8b798886e63af1 +Subproject commit 00b432942f437fdbb285f04a1847d3bebc390044 diff --git a/controller_teensy/platformio.ini b/controller_teensy/platformio.ini index 8c45f07..36bccdf 100644 --- a/controller_teensy/platformio.ini +++ b/controller_teensy/platformio.ini @@ -22,4 +22,5 @@ build_flags = lib_deps = robtillaart/ADS1X15@^0.3.9 - adafruit/Adafruit SSD1306@^2.5.7 \ No newline at end of file + adafruit/Adafruit SSD1306@^2.5.7 + arduino-libraries/SD@^1.2.4 \ No newline at end of file diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index b1b5e2b..ad45773 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -1,14 +1,22 @@ #include #include "definitions.h" -#include "structs.h" +//#include "structs.h" #include "helpfunctions.h" #include //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 - Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7 + //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); @@ -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 @@ -88,10 +105,13 @@ void setup() Serial.println("RTC has set the system time"); } + + 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() { - /* ## 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 - */ +void calculateSetSpeed(){ + + 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) + + 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){ - //Serial.print(throttle_raw); Serial.println(); - Serial.print(linearizeThrottle(throttle_raw)); Serial.println(); - last_log_send=loopmillis; - }*/ -} + }//else if(loopmillis>last_log_send+LOGMININTERVAL){ + // //Serial.print(throttle_raw); Serial.println(); + // Serial.print(linearizeThrottle(throttle_raw)); Serial.println(); + // 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"); } }