interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
bobbycar/controller/controller.ino

427 lines
15 KiB
C++
Raw Blame History

// *******************************************************************
// 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 ADC_CALIB_THROTTLE_MIN 2000
#define ADC_CALIB_THROTTLE_MAX 3120
#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 50 //ms. delay for sending speed and steer data to motor controller via serial
#define PIN_THROTTLE PA4
#define PIN_BRAKE PA5
#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 idx1 = 0; // Index for new data pointer
uint16_t bufStartFrame1; // Buffer Start Frame
byte *p1; // Pointer declaration for the new received data
byte incomingByte1;
byte incomingBytePrev1;
//Same for Serial2
uint8_t idx2 = 0; // Index for new data pointer
uint16_t bufStartFrame2; // Buffer Start Frame
byte *p2; // Pointer declaration for the new received data
byte incomingByte2;
byte incomingBytePrev2;
typedef struct{
uint16_t start;
int16_t speedLeft;
int16_t speedRight;
uint16_t checksum;
} SerialCommand;
SerialCommand Command1;
SerialCommand Command2;
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 Feedback1;
SerialFeedback NewFeedback1;
SerialFeedback Feedback2;
SerialFeedback NewFeedback2;
// ########################## SETUP ##########################
void setup()
{
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial1.begin(38400); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector
Serial2.begin(38400); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector
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);
pinMode(PIN_THROTTLE, INPUT);
pinMode(PIN_BRAKE, INPUT);
Serial.println("Initialized");
}
// ########################## SEND ##########################
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
{
// Create command
Command1.start = (uint16_t)START_FRAME;
Command1.speedLeft = (int16_t)uSpeedLeft;
Command1.speedRight = (int16_t)uSpeedRight;
Command1.checksum = (uint16_t)(Command1.start ^ Command1.speedLeft ^ Command1.speedRight);
Serial1.write((uint8_t *) &Command1, sizeof(Command1));
}
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
{
// Create command
Command2.start = (uint16_t)START_FRAME;
Command2.speedLeft = (int16_t)uSpeedLeft;
Command2.speedRight = (int16_t)uSpeedRight;
Command2.checksum = (uint16_t)(Command2.start ^ Command2.speedLeft ^ Command2.speedRight);
Serial2.write((uint8_t *) &Command2, sizeof(Command2));
}
// ########################## RECEIVE ##########################
void ReceiveSerial1()
{
// Check for new data availability in the Serial buffer
if ( Serial1.available() ) {
incomingByte1 = Serial1.read(); // Read the incoming byte
bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) + incomingByte1; // Construct the start frame
}
else {
return;
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(incomingByte1);
return;
#endif
// Copy received data
if (bufStartFrame1 == START_FRAME) { // Initialize if new data is detected
p1 = (byte *)&NewFeedback1;
*p1++ = incomingBytePrev1;
*p1++ = incomingByte1;
idx1 = 2;
} else if (idx1 >= 2 && idx1 < sizeof(SerialFeedback)) { // Save the new received data
*p1++ = incomingByte1;
idx1++;
}
// Check if we reached the end of the package
if (idx1 == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL
^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp);
// Check validity of the new data
if (NewFeedback1.start == START_FRAME && checksum == NewFeedback1.checksum) {
// Copy the new data
memcpy(&Feedback1, &NewFeedback1, sizeof(SerialFeedback));
}
idx1 = 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
incomingBytePrev1 = incomingByte1;
}
void ReceiveSerial2()
{
// Check for new data availability in the Serial buffer
if ( Serial2.available() ) {
incomingByte2 = Serial2.read(); // Read the incoming byte
bufStartFrame2 = ((uint16_t)(incomingBytePrev2) << 8) + incomingByte2; // Construct the start frame
}
else {
return;
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(incomingByte2);
return;
#endif
// Copy received data
if (bufStartFrame2 == START_FRAME) { // Initialize if new data is detected
p2 = (byte *)&NewFeedback2;
*p2++ = incomingBytePrev2;
*p2++ = incomingByte2;
idx2 = 2;
} else if (idx2 >= 2 && idx2 < sizeof(SerialFeedback)) { // Save the new received data
*p2++ = incomingByte2;
idx2++;
}
// Check if we reached the end of the package
if (idx2 == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = (uint16_t)(NewFeedback2.start ^ NewFeedback2.cmd1 ^ NewFeedback2.cmd2 ^ NewFeedback2.speedR ^ NewFeedback2.speedL
^ NewFeedback2.speedR_meas ^ NewFeedback2.speedL_meas ^ NewFeedback2.batVoltage ^ NewFeedback2.boardTemp);
// Check validity of the new data
if (NewFeedback2.start == START_FRAME && checksum == NewFeedback2.checksum) {
// Copy the new data
memcpy(&Feedback2, &NewFeedback2, sizeof(SerialFeedback));
}
idx2 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
}
// Update previous states
incomingBytePrev2 = incomingByte2;
}
// ########################## 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);
int _read=analogRead(PIN_THROTTLE);
int16_t speedvalue=constrain( map(_read, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000);
if (MODESWITCH_DOWN) {
SendSerial1(speedvalue,0);
SendSerial2(speedvalue,0);
Serial.print("L_");
}else{
SendSerial1(0,speedvalue);
SendSerial2(0,speedvalue);
Serial.print("R_");
}
Serial.print("millis="); Serial.print(millis()); Serial.print(", adcthrottle="); Serial.print(_read);
Serial.print(", 1.L="); Serial.print(Command1.speedLeft); Serial.print(", 1.R="); Serial.print(Command1.speedRight);
Serial.print(", 2.L="); Serial.print(Command2.speedLeft); Serial.print(", 2.R="); Serial.println(Command2.speedRight);
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++;
//Print Motor values
Serial.print("cmd1");
Serial.print(", "); Serial.print("cmd2");
Serial.print(","); Serial.print("speedR");
Serial.print(","); Serial.print("speedL");
Serial.print(", "); Serial.print("speedR_meas");
Serial.print(","); Serial.print("speedL_meas");
Serial.print(", "); Serial.print("batVoltage");
Serial.print(", "); Serial.println("boardTemp");
Serial.println();
Serial.print("1: "); Serial.print(Feedback1.cmd1);
Serial.print(", "); Serial.print(Feedback1.cmd2);
Serial.print(","); Serial.print(Feedback1.speedR);
Serial.print(","); Serial.print(Feedback1.speedL);
Serial.print(", "); Serial.print(Feedback1.speedR_meas);
Serial.print(","); Serial.print(Feedback1.speedL_meas);
Serial.print(", "); Serial.print(Feedback1.batVoltage);
Serial.print(", "); Serial.println(Feedback1.boardTemp);
Serial.println();
Serial.print("2: "); Serial.print(Feedback2.cmd1);
Serial.print(", "); Serial.print(Feedback2.cmd2);
Serial.print(","); Serial.print(Feedback2.speedR);
Serial.print(","); Serial.print(Feedback2.speedL);
Serial.print(", "); Serial.print(Feedback2.speedR_meas);
Serial.print(","); Serial.print(Feedback2.speedL_meas);
Serial.print(", "); Serial.print(Feedback2.batVoltage);
Serial.print(", "); Serial.println(Feedback2.boardTemp);
}
if (millis()>30000 && POWERBUTTON_DOWN) {
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());
}
}
}
Reference in New Issue View Git Blame Copy Permalink