parent
1af5be81cd
commit
6d46028aee
@ -1,174 +0,0 @@
|
|||||||
// *******************************************************************
|
|
||||||
// Arduino Nano 3.3V example code
|
|
||||||
// for https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC
|
|
||||||
//
|
|
||||||
// Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
|
|
||||||
//
|
|
||||||
// *******************************************************************
|
|
||||||
// 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.h>
|
|
||||||
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 ##########################
|
|
||||||
Loading…
Reference in new issue