From e62286cb8b6884e060074939568570866124bea6 Mon Sep 17 00:00:00 2001 From: Fisch Date: Fri, 26 Feb 2021 19:37:12 +0100 Subject: [PATCH] copy send and receive functions to new project for controller with teensy32 --- controller_teensy/lib/README | 46 +++++++++ controller_teensy/platformio.ini | 15 +++ controller_teensy/src/main.cpp | 164 +++++++++++++++++++++++++++++++ 3 files changed, 225 insertions(+) create mode 100644 controller_teensy/lib/README create mode 100644 controller_teensy/platformio.ini create mode 100644 controller_teensy/src/main.cpp diff --git a/controller_teensy/lib/README b/controller_teensy/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/controller_teensy/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/controller_teensy/platformio.ini b/controller_teensy/platformio.ini new file mode 100644 index 0000000..88ccea1 --- /dev/null +++ b/controller_teensy/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:teensy31] +platform = teensy +board = teensy31 +framework = arduino + diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp new file mode 100644 index 0000000..e7dd92d --- /dev/null +++ b/controller_teensy/src/main.cpp @@ -0,0 +1,164 @@ +#include + +// ########################## DEFINES ########################## +#define SERIAL_CONTROL_BAUD 38400 // [-] 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 SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial + +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; + +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{ + uint8_t idx = 0; // Index for new data pointer + uint16_t bufStartFrame; // Buffer Start Frame + byte *p; // Pointer declaration for the new received data + byte incomingByte; + byte incomingBytePrev; + long lastValidDataSerial_time; + long boardlastPoweron=0; //mainly for failcheck + long boardlastPoweroff=0; +} SerialRead; +SerialRead SerialcomFront; + + +typedef struct{ + uint16_t start; + int16_t speedLeft; + int16_t speedRight; + uint16_t checksum; +} SerialCommand; +SerialCommand CommandFront; + +typedef struct{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedL_meas; + int16_t speedR_meas; + int16_t batVoltage; + int16_t boardTemp; + int16_t curL_DC; + int16_t curR_DC; + uint16_t cmdLed; + uint16_t checksum; +} SerialFeedback; +SerialFeedback FeedbackFront; +SerialFeedback NewFeedbackFront; + +void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight) +{ + // 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 +} + +void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback) +{ + // 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 + } + else { + return; + } + + // If DEBUG_RX is defined print all incoming bytes + #ifdef DEBUG_RX + Serial.print(sread.incomingByte); + return; + #endif + + // Copy received data + if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected + sread.p = (byte *)&NewFeedback; + *sread.p++ = sread.incomingBytePrev; + *sread.p++ = sread.incomingByte; + sread.idx = 2; + } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data + *sread.p++ = sread.incomingByte; + sread.idx++; + } + + // Check if we reached the end of the package + if (sread.idx == sizeof(SerialFeedback)) { + uint16_t checksum; + + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 + ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed); + + // Check validity of the new data + if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { + // Copy the new data + memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); + sread.lastValidDataSerial_time = millis(); + } + 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; + + } + + // ########################## SETUP ########################## +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) + + +} + +// ########################## LOOP ########################## +void loop() { + loopmillis=millis(); //read millis for this cycle + + ReceiveSerial(); + + + 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; + + 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(); + + } +} \ No newline at end of file