hoverboard-firmware-hack-fo.../Arduino/controller_stm_PotiTest_NewFoc/controller_stm_PotiTest_New...

207 lines
7.5 KiB
Arduino
Raw Normal View 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
// RX ist A10, TX 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 A2BIT_CONV 50
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
#define MAXADCVALUE 4095
#define PIN_LED PC13
#define SENDPERIOD 100 //ms. delay for sending speed and steer data to motor controller via serial
#define PIN_POTI PA0
long last_send = 0;
2019-12-20 21:21:21 +00:00
long last_validReceive=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 speedL;
2020-02-02 15:56:09 +00:00
int16_t speedR;
int16_t speedL_meas;
2020-02-02 15:56:09 +00:00
int16_t speedR_meas;
int16_t batVoltage;
int16_t boardTemp;
2019-12-20 21:21:21 +00:00
int16_t curL_DC;
int16_t curR_DC;
uint16_t checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;
// ########################## SETUP ##########################
void setup()
{
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3)
Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2)
pinMode(LED_BUILTIN, 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()) {
2019-12-03 21:53:56 +00:00
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.speedL ^ NewFeedback.speedR
^ NewFeedback.speedL_meas^ NewFeedback.speedR_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC);
// Check validity of the new data
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
// Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
2019-12-20 21:21:21 +00:00
Serial.print(millis()-last_validReceive); Serial.print("ms ");
// 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.speedL);
Serial.print(" 4: "); Serial.print(Feedback.speedR);
Serial.print(" 5: "); Serial.print(Feedback.speedL_meas);
Serial.print(" 6: "); Serial.print(Feedback.speedR_meas);
Serial.print(" 7: "); Serial.print(Feedback.batVoltage);
2019-12-20 21:21:21 +00:00
Serial.print(" 8: "); Serial.print(Feedback.boardTemp);
2019-12-28 13:03:35 +00:00
Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in mA (100mA resolution), in hoverbrett negative sign for forward
Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in mA (100mA resolution), in hoverbrett negative sign for forward
*/
Serial.print(" LS: "); Serial.print(Feedback.speedL_meas);
Serial.print(" RS: "); Serial.print(Feedback.speedR_meas);
Serial.print(" LC: "); Serial.print(Feedback.curL_DC*1.0/A2BIT_CONV); //in A, in hoverbrett negative sign for forward
Serial.print(" RC: "); Serial.println(Feedback.curR_DC*1.0/A2BIT_CONV); //in A, in hoverbrett negative sign for forward
2019-12-20 21:21:21 +00:00
last_validReceive=millis();
} 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() {
ReceiveSerial1(); // Check for new received data
if (millis() - last_send > SENDPERIOD) {
2019-12-20 21:21:21 +00:00
int16_t speedvalue=constrain(analogRead(PIN_POTI)*1.0/MAXADCVALUE*1000, 0, 1000);
2019-12-13 20:54:21 +00:00
SendSerial1(speedvalue,speedvalue);
//Serial.print("Send "); Serial.print(millis()); Serial.print("ms, steer=0"); Serial.print(", speed="); Serial.println(speedvalue);
last_send = millis();
}
}
// ########################## END ##########################