From 649daaa25ef90cd7e4854ce6d178d979981dfd02 Mon Sep 17 00:00:00 2001 From: Fisch Date: Tue, 2 Mar 2021 23:48:20 +0100 Subject: [PATCH] fix serial esc test control --- controller_teensy/platformio.ini | 6 ++ controller_teensy/src/main.cpp | 116 +++++++++++++++--------- hoverboard-firmware-hack-foc-serial-esc | 2 +- 3 files changed, 79 insertions(+), 45 deletions(-) diff --git a/controller_teensy/platformio.ini b/controller_teensy/platformio.ini index 88ccea1..5b25997 100644 --- a/controller_teensy/platformio.ini +++ b/controller_teensy/platformio.ini @@ -13,3 +13,9 @@ platform = teensy board = teensy31 framework = arduino + + +monitor_speed = 115200 + +build_flags = + -D USB_SERIAL_HID \ No newline at end of file diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index e7dd92d..fe4da04 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -1,21 +1,25 @@ #include // ########################## DEFINES ########################## -#define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) +#define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) -#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication +#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication #define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial +#define RECEIVEPERIOD 50 //ms + +#define PIN_THROTTLE A0 +const uint16_t calib_throttle_min = 350; +const uint16_t calib_throttle_max = 810; int16_t out_speedFL=0; int16_t out_speedFR=0; int16_t out_speedRL=0; int16_t out_speedRR=0; unsigned long last_send = 0; +unsigned long last_receive = 0; -void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight); -void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback); // Global variables for serial communication typedef struct{ @@ -25,8 +29,6 @@ typedef struct{ byte incomingByte; byte incomingBytePrev; long lastValidDataSerial_time; - long boardlastPoweron=0; //mainly for failcheck - long boardlastPoweroff=0; } SerialRead; SerialRead SerialcomFront; @@ -55,31 +57,37 @@ typedef struct{ SerialFeedback FeedbackFront; SerialFeedback NewFeedbackFront; -void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight) + +void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef); +bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef); + +void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) { // Create command scom.start = (uint16_t)START_FRAME; scom.speedLeft = (int16_t)uSpeedLeft; scom.speedRight = (int16_t)uSpeedRight; scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight); - Serial1.write((uint8_t *) &scom, sizeof(scom)); //TODO: use correct serial object + + SerialRef.write((uint8_t *) &scom, sizeof(scom)); + } -void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback) +bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef) { + bool _result=1; // Check for new data availability in the Serial buffer - if ( Serial1.available() ) { - sread.incomingByte = Serial1.read(); // Read the incoming byte - sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame + if ( SerialRef.available() ) { + sread.incomingByte = SerialRef.read(); // Read the incoming byte + sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame } else { - return; + return 0; } // If DEBUG_RX is defined print all incoming bytes #ifdef DEBUG_RX Serial.print(sread.incomingByte); - return; #endif // Copy received data @@ -105,26 +113,31 @@ void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N // Copy the new data memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); sread.lastValidDataSerial_time = millis(); + } else { + _result=0; } sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) - /* - // Print data to built-in Serial - Serial.print("1: "); Serial.print(Feedback.cmd1); - Serial.print(" 2: "); Serial.print(Feedback.cmd2); - Serial.print(" 3: "); Serial.print(Feedback.speedR); - Serial.print(" 4: "); Serial.print(Feedback.speedL); - Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); - Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); - Serial.print(" 7: "); Serial.print(Feedback.batVoltage); - Serial.print(" 8: "); Serial.println(Feedback.boardTemp); - } else { - Serial.println("Non-valid data skipped"); - }*/ - - // Update previous states - sread.incomingBytePrev = sread.incomingByte; - } + /* + // Print data to built-in Serial + Serial.print("1: "); Serial.print(Feedback.cmd1); + Serial.print(" 2: "); Serial.print(Feedback.cmd2); + Serial.print(" 3: "); Serial.print(Feedback.speedR); + Serial.print(" 4: "); Serial.print(Feedback.speedL); + Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); + Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); + Serial.print(" 7: "); Serial.print(Feedback.batVoltage); + Serial.print(" 8: "); Serial.println(Feedback.boardTemp); + } else { + Serial.println("Non-valid data skipped"); + }*/ + + // Update previous states + sread.incomingBytePrev = sread.incomingByte; + + return _result; //new data was available + +} // ########################## SETUP ########################## void setup() @@ -132,33 +145,48 @@ void setup() Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) - Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear) - Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front) + Serial2.begin(SERIAL_CONTROL_BAUD); //control + Serial3.begin(SERIAL_CONTROL_BAUD); //control + pinMode(PIN_THROTTLE, INPUT_PULLUP); } + +unsigned long loopmillis; // ########################## LOOP ########################## void loop() { loopmillis=millis(); //read millis for this cycle - ReceiveSerial(); + bool newData=false; + newData=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2); + + + //Serial.print("fo="); Serial.println(count); + //count++; + if (newData) { + float _current = (FeedbackFront.curL_DC+FeedbackFront.curR_DC)/2.0 / 50; + + Serial.print(FeedbackFront.curL_DC); Serial.print(", "); + Serial.print(FeedbackFront.curR_DC); Serial.print(", "); + + Serial.print(", mean="); Serial.print(_current); + Serial.print(", "); + Serial.print(FeedbackFront.cmd1); Serial.print(", "); + Serial.print(FeedbackFront.cmd2); Serial.print(", "); + Serial.println(); + } if (loopmillis - last_send > SENDPERIOD) { last_send=loopmillis; - - SendSerial(out_speedRL,out_speedRR); - - float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; + uint16_t throttle_raw = analogRead(PIN_THROTTLE); + Serial.print("Analog: "); Serial.print(throttle_raw); + out_speedFL=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); - Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear - Serial.print(Feedback1.curR_DC); Serial.print(", "); - Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front - Serial.print(Feedback2.curR_DC); - Serial.print(", mean="); Serial.print(_current); - Serial.println(); + Serial.print(", Send: "); Serial.println(out_speedFL); + SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2); } } \ No newline at end of file diff --git a/hoverboard-firmware-hack-foc-serial-esc b/hoverboard-firmware-hack-foc-serial-esc index 2bab3aa..898898a 160000 --- a/hoverboard-firmware-hack-foc-serial-esc +++ b/hoverboard-firmware-hack-foc-serial-esc @@ -1 +1 @@ -Subproject commit 2bab3aa1a46d1754ecc81692524120122b5b07f7 +Subproject commit 898898a63c6b494e0525df8a8ed2da98fcd5cdb1