From c7264eba6cadf6a3d7d073dfbf0101a850c941fc Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 8 Dec 2019 18:14:28 +0100 Subject: [PATCH] implement connected buttons and testfunction --- controller/controller.ino | 315 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 315 insertions(+) create mode 100644 controller/controller.ino diff --git a/controller/controller.ino b/controller/controller.ino new file mode 100644 index 0000000..4b399a2 --- /dev/null +++ b/controller/controller.ino @@ -0,0 +1,315 @@ +// ******************************************************************* +// 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 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()); + } + } + + + +}