316 lines
11 KiB
C++
316 lines
11 KiB
C++
// *******************************************************************
|
|
// 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
|
|
// *******************************************************************
|
|
|
|
//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware
|
|
//Board: Generic STM32F103C series
|
|
//Upload method: serial
|
|
//20k RAM 64k Flash
|
|
//may need 3v3 from usb ttl converter (hold down flash button while connecting). Holding down the power button is not needed in this case.
|
|
//Sometimes reconnecting the usb ttl converter to the pc helps just before pressing the upload button
|
|
|
|
// RX(green) is A10 , TX (blue) ist A9 (3v3 level)
|
|
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
|
|
//set boot0 back to 0 to run program on powerup
|
|
|
|
// ########################## 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 DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
|
|
|
|
#define MAXADCVALUE 4095
|
|
|
|
#define PIN_POWERLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
|
|
#define PIN_POWERBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
|
|
#define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON)
|
|
|
|
#define SENDPERIOD 200 //ms. delay for sending speed and steer data to motor controller via serial
|
|
#define PIN_THROTTLE PA0
|
|
|
|
#define PIN_ENABLE PB9
|
|
|
|
#define PIN_MODESWITCH PB5 // LOW if pressed in ("down")
|
|
#define MODESWITCH_DOWN !digitalRead(PIN_MODESWITCH)
|
|
#define PIN_MODELED_GREEN PA12
|
|
#define PIN_MODELED_RED PA11
|
|
|
|
#define PIN_RELAISFRONT PB14 //connected to relais which presses the powerbutton of the hoverboard for the front wheels
|
|
#define PIN_RELAISREAR PB15 //connected to relais which presses the powerbutton of the hoverboard for the rear wheels
|
|
|
|
int testcounter=0;
|
|
|
|
long last_send = 0;
|
|
|
|
|
|
// 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 speedLeft;
|
|
int16_t speedRight;
|
|
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(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
|
|
|
Serial1.begin(19200); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2)
|
|
Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3)
|
|
|
|
|
|
Serial1.begin(SERIAL_CONTROL_BAUD);
|
|
pinMode(PIN_POWERLED, OUTPUT);
|
|
pinMode(PIN_ENABLE, OUTPUT);
|
|
digitalWrite(PIN_ENABLE, HIGH); //keep power on
|
|
pinMode(PIN_POWERBUTTON, INPUT_PULLUP);
|
|
pinMode(PIN_MODESWITCH, INPUT_PULLUP);
|
|
pinMode(PIN_MODELED_GREEN, OUTPUT);
|
|
pinMode(PIN_MODELED_RED, OUTPUT);
|
|
pinMode(PIN_RELAISFRONT, OUTPUT);
|
|
pinMode(PIN_RELAISREAR, OUTPUT);
|
|
|
|
Serial.println("Initialized");
|
|
}
|
|
|
|
// ########################## SEND ##########################
|
|
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
|
|
{
|
|
// Create command
|
|
Command.start = (uint16_t)START_FRAME;
|
|
Command.speedLeft = (int16_t)uSpeedLeft;
|
|
Command.speedRight = (int16_t)uSpeedRight;
|
|
Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight);
|
|
|
|
// Write to Serial
|
|
Serial1.write((uint8_t *) &Command, sizeof(Command));
|
|
}
|
|
|
|
// ########################## RECEIVE ##########################
|
|
void ReceiveSerial1()
|
|
{
|
|
// Check for new data availability in the Serial buffer
|
|
if (Serial1.available()) {
|
|
incomingByte = Serial1.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 ##########################
|
|
|
|
|
|
|
|
void loop() {
|
|
selfTest(); //start selftest, does not return
|
|
|
|
ReceiveSerial1(); // Check for new received data
|
|
|
|
if (millis()>2000 && POWERBUTTON_DOWN) {
|
|
poweronBoards();
|
|
}
|
|
|
|
if (millis() - last_send > SENDPERIOD) {
|
|
Serial.print("powerbutton="); Serial.print(POWERBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
|
|
|
|
|
int16_t speedvalue=constrain(analogRead(PIN_THROTTLE)*1.0/MAXADCVALUE*1000, 0, 1000);
|
|
|
|
SendSerial1(speedvalue,0);
|
|
//Serial.print("millis="); Serial.print(millis()); Serial.print(", steer=0"); Serial.print(", speed="); Serial.println(speedvalue);
|
|
|
|
last_send = millis();
|
|
|
|
digitalWrite(PIN_POWERLED, !digitalRead(PIN_POWERLED));
|
|
if (testcounter%3==0) {
|
|
digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN));
|
|
}
|
|
if (testcounter%5==0) {
|
|
digitalWrite(PIN_MODELED_RED, !digitalRead(PIN_MODELED_RED));
|
|
}
|
|
|
|
testcounter++;
|
|
}
|
|
|
|
if (millis()>60000) {
|
|
poweroff();
|
|
}
|
|
|
|
}
|
|
|
|
// ########################## END ##########################
|
|
|
|
|
|
void poweroff() {
|
|
|
|
//TODO: trigger Relais for Board 1
|
|
// Wait for board to shut down
|
|
//TODO: trigger Relais for Board 2
|
|
// Wait for board to shut down
|
|
|
|
//Timeout error handling
|
|
|
|
digitalWrite(PIN_ENABLE, LOW); //poweroff own latch
|
|
|
|
delay(1000);
|
|
Serial.println("Still powered");
|
|
//still powered on: set error status "power latch error"
|
|
}
|
|
|
|
void poweronBoards() {
|
|
digitalWrite(PIN_RELAISFRONT,HIGH);
|
|
delay(200);digitalWrite(PIN_RELAISFRONT,LOW);
|
|
delay(50);
|
|
digitalWrite(PIN_RELAISREAR,HIGH);
|
|
delay(200);digitalWrite(PIN_RELAISREAR,LOW);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void selfTest() {
|
|
digitalWrite(PIN_ENABLE,HIGH); //make shure latch is on
|
|
Serial.println("Entering selftest");
|
|
#define TESTDELAY 1000 //delay between test
|
|
#define TESTTIME 500 //time to keep tested pin on
|
|
|
|
delay(TESTDELAY); Serial.println("PIN_POWERLED");
|
|
digitalWrite(PIN_POWERLED,HIGH); delay(TESTTIME); digitalWrite(PIN_POWERLED,LOW);
|
|
|
|
delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN");
|
|
digitalWrite(PIN_MODELED_GREEN,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_GREEN,HIGH);
|
|
|
|
delay(TESTDELAY); Serial.println("PIN_MODELED_RED");
|
|
digitalWrite(PIN_MODELED_RED,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_RED,HIGH);
|
|
|
|
delay(TESTDELAY); Serial.println("PIN_RELAISFRONT");
|
|
digitalWrite(PIN_RELAISFRONT,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISFRONT,LOW);
|
|
|
|
delay(TESTDELAY); Serial.println("PIN_RELAISREAR");
|
|
digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISREAR,LOW);
|
|
|
|
delay(TESTDELAY); Serial.println("ALL ON");
|
|
digitalWrite(PIN_POWERLED,HIGH);
|
|
digitalWrite(PIN_MODELED_GREEN,LOW);
|
|
digitalWrite(PIN_MODELED_RED,LOW);
|
|
digitalWrite(PIN_RELAISFRONT,HIGH);
|
|
digitalWrite(PIN_RELAISREAR,HIGH);
|
|
delay(TESTTIME*5);
|
|
digitalWrite(PIN_POWERLED,LOW);
|
|
digitalWrite(PIN_MODELED_GREEN,HIGH);
|
|
digitalWrite(PIN_MODELED_RED,HIGH);
|
|
digitalWrite(PIN_RELAISFRONT,LOW);
|
|
digitalWrite(PIN_RELAISREAR,LOW);
|
|
delay(TESTDELAY);
|
|
|
|
Serial.println("Powers off latch at millis>=60000");
|
|
Serial.println("Inputs:");
|
|
while(true) { //Keep printing input values forever
|
|
delay(100);
|
|
Serial.print("millis="); Serial.print(millis()); Serial.print(", throttle ADC="); Serial.println(analogRead(PIN_THROTTLE));
|
|
Serial.print("powerbutton down="); Serial.print(POWERBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
|
|
|
while (millis()>=60000) {
|
|
digitalWrite(PIN_ENABLE, LOW); //poweroff own latch
|
|
Serial.println(millis());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|