diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 01b50a5..ac6fa4e 100644 --- a/controller_teensy/src/main.cpp +++ b/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 avg_currentR=0; +float filtered_currentAll=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 - Serial3.begin(SERIAL_CONTROL_BAUD); //control + 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 pinMode(PIN_THROTTLE, INPUT); pinMode(PIN_BRAKE, INPUT); @@ -242,14 +253,24 @@ 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_LEDR,LOW); + digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on + digitalWrite(PIN_MODE_LEDR,HIGH); }else{ //button not pushed in - digitalWrite(PIN_MODE_LEDG,LOW); - digitalWrite(PIN_MODE_LEDR,HIGH); //Red + digitalWrite(PIN_MODE_LEDG,HIGH); + 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) { @@ -257,12 +278,16 @@ void loop() { 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 filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps - float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps - float filtered_curRL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps - float filtered_curRR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps + 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) + motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps + motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps + motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_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_currentRear=min(filtered_curRL,filtered_curRR); + float filtered_currentFront=min(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR); + 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 @@ -333,4 +366,42 @@ float filterMedian(int16_t* values) { mean/=(1+CURRENT_MEANVALUECOUNT*2); 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(); } \ No newline at end of file