// ******************************************************************* // 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 // ******************************************************************* //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()); } } }