interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

implement connected buttons and testfunction

master
interfisch 3 years ago
parent
9a66480f9c
commit
c7264eba6c
1 changed files with 315 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 315
      controller/controller.ino

315
controller/controller.ino
Unescape View File

@ -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 <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());
}
}
}
Write Preview
Loading…
Cancel
Save