From cbc6b07656fe441886cbfdf1f6905519a5d2d27b Mon Sep 17 00:00:00 2001 From: JbLb Date: Tue, 31 Dec 2019 23:07:45 +0100 Subject: [PATCH] first commit --- hoverserial.ino | 174 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 174 insertions(+) create mode 100644 hoverserial.ino diff --git a/hoverserial.ino b/hoverserial.ino new file mode 100644 index 0000000..d6908c3 --- /dev/null +++ b/hoverserial.ino @@ -0,0 +1,174 @@ +// ******************************************************************* +// Arduino Nano 3.3V example code +// for https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC +// +// Copyright (C) 2019-2020 Emanuel FERU +// +// ******************************************************************* +// INFO: +// • This sketch uses the the Serial Software interface to communicate and send commands to the hoverboard +// • The built-in (HW) Serial interface is used for debugging and visualization. In case the debugging is not needed, +// it is recommended to use the built-in Serial interface for full speed perfomace. +// • The data packaging includes a Start Frame, checksum, and re-syncronization capability for reliable communication +// +// CONFIGURATION on the hoverboard side in config.h: +// • Option 1: Serial on Left Sensor cable (long wired cable) +// #define CONTROL_SERIAL_USART2 +// #define FEEDBACK_SERIAL_USART2 +// // #define DEBUG_SERIAL_USART2 +// • Option 2: Serial on Right Sensor cable (short wired cable) - recommended, so the ADCs on the other cable are still available +// #define CONTROL_SERIAL_USART3 +// #define FEEDBACK_SERIAL_USART3 +// // #define DEBUG_SERIAL_USART3 +// ******************************************************************* + +// ########################## DEFINES ########################## +#define HOVER_SERIAL_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 TIME_SEND 100 // [ms] Sending time interval +#define SPEED_MAX_TEST 300 // [-] Maximum speed for testing +//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) + +#include +SoftwareSerial HoverSerial(2,3); // RX, TX + +// Global variables +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; + +typedef struct{ + uint16_t start; + int16_t steer; + int16_t speed; + uint16_t checksum; +} SerialCommand; +SerialCommand Command; + +typedef struct{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedR; + int16_t speedL; + int16_t speedR_meas; + int16_t speedL_meas; + int16_t batVoltage; + int16_t boardTemp; + int16_t checksum; +} SerialFeedback; +SerialFeedback Feedback; +SerialFeedback NewFeedback; + +// ########################## SETUP ########################## +void setup() +{ + Serial.begin(SERIAL_BAUD); + Serial.println("Hoverboard Serial v1.0"); + + HoverSerial.begin(HOVER_SERIAL_BAUD); + pinMode(LED_BUILTIN, OUTPUT); +} + +// ########################## SEND ########################## +void Send(int16_t uSteer, int16_t uSpeed) +{ + // Create command + Command.start = (uint16_t)START_FRAME; + Command.steer = (int16_t)uSteer; + Command.speed = (int16_t)uSpeed; + Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed); + + // Write to Serial + HoverSerial.write((uint8_t *) &Command, sizeof(Command)); +} + +// ########################## RECEIVE ########################## +void Receive() +{ + // Check for new data availability in the Serial buffer + if (HoverSerial.available()) { + incomingByte = HoverSerial.read(); // Read the incoming byte + bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame + } + else { + return; + } + + // If DEBUG_RX is defined print all incoming bytes + #ifdef DEBUG_RX + Serial.print(incomingByte); + return; + #endif + + // Copy received data + if (bufStartFrame == START_FRAME) { // Initialize if new data is detected + p = (byte *)&NewFeedback; + *p++ = incomingBytePrev; + *p++ = incomingByte; + idx = 2; + } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data + *p++ = incomingByte; + idx++; + } + + // Check if we reached the end of the package + if (idx == sizeof(SerialFeedback)) { + uint16_t checksum; + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL + ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp); + + // Check validity of the new data + if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { + // Copy the new data + memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); + + // 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"); + } + idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) + } + + // Update previous states + incomingBytePrev = incomingByte; +} + +// ########################## LOOP ########################## + +unsigned long iTimeSend = 0; +int iTestMax = SPEED_MAX_TEST; +int iTest = 0; + +void loop(void) +{ + unsigned long timeNow = millis(); + + // Check for new received data + Receive(); + + // Send commands + if (iTimeSend > timeNow) return; + iTimeSend = timeNow + TIME_SEND; + Send(0, abs(iTest)); + + // Calculate test command signal + iTest += 10; + if (iTest > iTestMax) iTest = -iTestMax; + + // Blink the LED + digitalWrite(LED_BUILTIN, (timeNow%2000)<1000); +} + +// ########################## END ##########################