add state machine for modes
This commit is contained in:
parent
53ee2f4d22
commit
05c42ccbc5
|
|
@ -1,27 +1,3 @@
|
||||||
// *******************************************************************
|
|
||||||
// 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
|
//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware
|
||||||
//Board: Generic STM32F103C series
|
//Board: Generic STM32F103C series
|
||||||
//Upload method: serial
|
//Upload method: serial
|
||||||
|
|
@ -41,12 +17,13 @@
|
||||||
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
|
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
|
||||||
|
|
||||||
//#define MAXADCVALUE 4095
|
//#define MAXADCVALUE 4095
|
||||||
#define ADC_CALIB_THROTTLE_MIN 2000
|
#define ADC_CALIB_THROTTLE_LOWEST 1900 //a bit above maximum adc value if throttle it not touched
|
||||||
#define ADC_CALIB_THROTTLE_MAX 3120
|
#define ADC_CALIB_THROTTLE_MIN 2000 //minimum adc value that should correspond to 0 speed
|
||||||
|
#define ADC_CALIB_THROTTLE_MAX 3120 //maximum adc value that should correspond to full speed
|
||||||
|
|
||||||
#define PIN_POWERLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
|
#define PIN_STARTLED 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 PIN_STARTBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
|
||||||
#define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON)
|
#define STARTBUTTON_DOWN digitalRead(PIN_STARTBUTTON)
|
||||||
|
|
||||||
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
|
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
|
||||||
#define PIN_THROTTLE PA4
|
#define PIN_THROTTLE PA4
|
||||||
|
|
@ -62,7 +39,36 @@
|
||||||
#define PIN_RELAISFRONT PB14 //connected to relais which presses the powerbutton of the hoverboard for the front wheels
|
#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
|
#define PIN_RELAISREAR PB15 //connected to relais which presses the powerbutton of the hoverboard for the rear wheels
|
||||||
|
|
||||||
int testcounter=0;
|
#define DEBOUNCETIME 20 //time to not check for inputs after key press
|
||||||
|
#define BUTTONTIMEHOLD 750 //time for button hold
|
||||||
|
long millis_lastinput=0; //for button debounce
|
||||||
|
long timebuttonpressed_start;
|
||||||
|
boolean button_start=false;
|
||||||
|
boolean button_hold_start=false;
|
||||||
|
|
||||||
|
|
||||||
|
#define TIME_AUTOPOWEROFF 600000 //600000 = 10 minutes
|
||||||
|
|
||||||
|
long loopmillis=0; //only use one millis reading each loop
|
||||||
|
long millis_lastchange=0; //for poweroff after some time with no movement
|
||||||
|
|
||||||
|
String errormessage=""; //store some error message to print
|
||||||
|
|
||||||
|
//Mode change variables
|
||||||
|
uint8_t state_modechange=0;
|
||||||
|
long state_modechange_time=0;
|
||||||
|
|
||||||
|
long millis_lastadc=0;
|
||||||
|
#define ADC_READTIME 10 //time interval to read adc (for filtering)
|
||||||
|
#define ADC_THROTTLE_FILTER 0.05 //low value = slower change
|
||||||
|
int adc_throttle_raw=0; //raw throttle value from adc
|
||||||
|
float adc_throttle=0; //filtered value
|
||||||
|
|
||||||
|
uint16_t out_speedFL=0;
|
||||||
|
uint16_t out_speedFR=0;
|
||||||
|
uint16_t out_speedRL=0;
|
||||||
|
uint16_t out_speedRR=0;
|
||||||
|
|
||||||
|
|
||||||
long last_send = 0;
|
long last_send = 0;
|
||||||
|
|
||||||
|
|
@ -73,6 +79,7 @@ uint16_t bufStartFrame1; // Buffer Start Frame
|
||||||
byte *p1; // Pointer declaration for the new received data
|
byte *p1; // Pointer declaration for the new received data
|
||||||
byte incomingByte1;
|
byte incomingByte1;
|
||||||
byte incomingBytePrev1;
|
byte incomingBytePrev1;
|
||||||
|
long lastValidDataSerial1_time;
|
||||||
|
|
||||||
//Same for Serial2
|
//Same for Serial2
|
||||||
uint8_t idx2 = 0; // Index for new data pointer
|
uint8_t idx2 = 0; // Index for new data pointer
|
||||||
|
|
@ -80,6 +87,7 @@ uint16_t bufStartFrame2; // Buffer Start Frame
|
||||||
byte *p2; // Pointer declaration for the new received data
|
byte *p2; // Pointer declaration for the new received data
|
||||||
byte incomingByte2;
|
byte incomingByte2;
|
||||||
byte incomingBytePrev2;
|
byte incomingBytePrev2;
|
||||||
|
long lastValidDataSerial2_time;
|
||||||
|
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
|
|
@ -108,21 +116,31 @@ SerialFeedback NewFeedback1;
|
||||||
SerialFeedback Feedback2;
|
SerialFeedback Feedback2;
|
||||||
SerialFeedback NewFeedback2;
|
SerialFeedback NewFeedback2;
|
||||||
|
|
||||||
|
|
||||||
|
enum mode{idle, on, error, off};
|
||||||
|
/*
|
||||||
|
* idle: controller is on, hoverboards are off
|
||||||
|
* on: hoverbaords are on and happy
|
||||||
|
* error: some error occured, stop everything and show errors
|
||||||
|
* off: shutdown triggered. will power down latch soon
|
||||||
|
*/
|
||||||
|
mode currentmode; //current active mode
|
||||||
|
mode requestmode; //change this variable to initiate a mode change
|
||||||
|
|
||||||
// ########################## SETUP ##########################
|
// ########################## SETUP ##########################
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
|
|
||||||
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
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
|
Serial1.begin(SERIAL_CONTROL_BAUD); //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
|
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector
|
||||||
|
|
||||||
|
// Pin Setup
|
||||||
Serial1.begin(SERIAL_CONTROL_BAUD);
|
pinMode(PIN_STARTLED, OUTPUT);
|
||||||
pinMode(PIN_POWERLED, OUTPUT);
|
|
||||||
pinMode(PIN_ENABLE, OUTPUT);
|
pinMode(PIN_ENABLE, OUTPUT);
|
||||||
digitalWrite(PIN_ENABLE, HIGH); //keep power on
|
digitalWrite(PIN_ENABLE, HIGH); //keep power on
|
||||||
pinMode(PIN_POWERBUTTON, INPUT_PULLUP);
|
pinMode(PIN_STARTBUTTON, INPUT_PULLUP);
|
||||||
pinMode(PIN_MODESWITCH, INPUT_PULLUP);
|
pinMode(PIN_MODESWITCH, INPUT_PULLUP);
|
||||||
pinMode(PIN_MODELED_GREEN, OUTPUT);
|
pinMode(PIN_MODELED_GREEN, OUTPUT);
|
||||||
pinMode(PIN_MODELED_RED, OUTPUT);
|
pinMode(PIN_MODELED_RED, OUTPUT);
|
||||||
|
|
@ -132,150 +150,275 @@ void setup()
|
||||||
pinMode(PIN_BRAKE, INPUT);
|
pinMode(PIN_BRAKE, INPUT);
|
||||||
|
|
||||||
Serial.println("Initialized");
|
Serial.println("Initialized");
|
||||||
|
|
||||||
|
currentmode = idle; //start in idle mode
|
||||||
|
requestmode = currentmode;
|
||||||
|
|
||||||
|
millis_lastchange=millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
// ########################## 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 ##########################
|
// ########################## LOOP ##########################
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
|
loopmillis=millis(); //read millis for this cycle
|
||||||
|
|
||||||
|
ReceiveSerial1(); // Check for new received data
|
||||||
|
ReceiveSerial2(); // Check for new received data
|
||||||
|
|
||||||
|
handleInputs();
|
||||||
|
handleModeChange(); //mode changes
|
||||||
|
|
||||||
|
modeloops();
|
||||||
|
|
||||||
|
if (loopmillis - last_send > SENDPERIOD) {
|
||||||
|
last_send=loopmillis;
|
||||||
|
|
||||||
|
if (currentmode!=off || currentmode!=idle) { //if boards should be powered on
|
||||||
|
SendSerial2(out_speedFL,out_speedFR); //Front
|
||||||
|
SendSerial1(out_speedRL,out_speedRR); //Rear
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleInputs()
|
||||||
|
{
|
||||||
|
//Short press (true when button short pressed, on release)
|
||||||
|
button_start=false;
|
||||||
|
|
||||||
|
//long press (true when button is held down for BUTTONTIMEHOLD, on time elapsed)
|
||||||
|
button_hold_start=false;
|
||||||
|
|
||||||
|
if (loopmillis-millis_lastinput>DEBOUNCETIME) //Button debouncing
|
||||||
|
{
|
||||||
|
//Trigger
|
||||||
|
if (timebuttonpressed_start == 0 && STARTBUTTON_DOWN){ //first time pressed down. (low when pressed)
|
||||||
|
timebuttonpressed_start=loopmillis; //set time of button press
|
||||||
|
}else if(timebuttonpressed_start != 0 && !STARTBUTTON_DOWN){ //button released (was pressed)
|
||||||
|
if (loopmillis-timebuttonpressed_start < BUTTONTIMEHOLD){ //short press
|
||||||
|
button_start=true;
|
||||||
|
}
|
||||||
|
timebuttonpressed_start=0; //re-enable after short press and release from hold
|
||||||
|
}else if(loopmillis-timebuttonpressed_start >= BUTTONTIMEHOLD && timebuttonpressed_start>0){ //held down long enough and not already hold triggered
|
||||||
|
button_hold_start=true;
|
||||||
|
timebuttonpressed_start=-1; //-1 as flag for hold triggered
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if ( button_start || button_hold_start) {
|
||||||
|
millis_lastchange=loopmillis; //for auto poweroff
|
||||||
|
millis_lastinput=loopmillis; //for debouncing
|
||||||
|
}
|
||||||
|
|
||||||
|
if (loopmillis-millis_lastadc>ADC_READTIME) {
|
||||||
|
adc_throttle_raw = analogRead(PIN_THROTTLE);
|
||||||
|
adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER;
|
||||||
|
if (adc_throttle_raw >= ADC_CALIB_THROTTLE_MIN) { //throttle pressed
|
||||||
|
millis_lastchange=loopmillis;
|
||||||
|
}
|
||||||
|
millis_lastadc=loopmillis;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (loopmillis-millis_lastchange>TIME_AUTOPOWEROFF){
|
||||||
|
requestmode = off;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleModeChange() {
|
||||||
|
if (button_start){ //short press start button
|
||||||
|
requestmode=off; //short press in any mode turns off everything
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentmode==requestmode) { //## Not currently changing modes ##
|
||||||
|
switch (currentmode) { //mode dependant
|
||||||
|
case idle:
|
||||||
|
if (button_hold_start){ //long press
|
||||||
|
requestmode=on; //long press switches betweeen idle and on
|
||||||
|
state_modechange=0; //start at state 0
|
||||||
|
}
|
||||||
|
if (button_start) { //short press
|
||||||
|
requestmode=off;
|
||||||
|
state_modechange=0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case on:
|
||||||
|
if (button_hold_start){ //long press
|
||||||
|
requestmode=idle; //long press switches betweeen idle and on
|
||||||
|
state_modechange=0; //start at state 0
|
||||||
|
}
|
||||||
|
if (button_start) { //short press
|
||||||
|
requestmode=off;
|
||||||
|
state_modechange=0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case error:
|
||||||
|
if (button_start) { //short press
|
||||||
|
requestmode=off;
|
||||||
|
state_modechange=0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case off:
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
currentmode=error; //something else? -> error
|
||||||
|
}
|
||||||
|
}else{ // ## Change requested ##
|
||||||
|
switch (requestmode) { //mode changes
|
||||||
|
case idle: case on: case off: //similar for on, idle and off
|
||||||
|
switch(state_modechange) {
|
||||||
|
case 0:
|
||||||
|
if (requestmode==on && adc_throttle > ADC_CALIB_THROTTLE_LOWEST) { //requested to turn on but throttle is pressed
|
||||||
|
state_modechange=0;
|
||||||
|
requestmode=currentmode; //abort modechange
|
||||||
|
//TODO: led show aborted modechange
|
||||||
|
}else{ //everythings fine, turn on/off
|
||||||
|
digitalWrite(PIN_RELAISFRONT,HIGH); //simulate hoverboard power button press
|
||||||
|
state_modechange++;
|
||||||
|
state_modechange_time=loopmillis; //set to current time
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
if (loopmillis - state_modechange_time > 200) { //wait some time
|
||||||
|
digitalWrite(PIN_RELAISFRONT,LOW); //release simulated button
|
||||||
|
state_modechange++;
|
||||||
|
state_modechange_time=loopmillis; //set to current time
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
if (loopmillis - state_modechange_time > 200) { //wait some time
|
||||||
|
digitalWrite(PIN_RELAISREAR,HIGH); //simulate hoverboard power button press
|
||||||
|
state_modechange++;
|
||||||
|
state_modechange_time=loopmillis; //set to current time
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
if (loopmillis - state_modechange_time > 200) { //wait some time
|
||||||
|
digitalWrite(PIN_RELAISREAR,LOW); //release simulated button
|
||||||
|
state_modechange++;
|
||||||
|
state_modechange_time=loopmillis; //set to current time
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
// ### Request On ###
|
||||||
|
if (requestmode==on) {//wait for both boards to send feedback
|
||||||
|
if ( serial1Active() && serial2Active() ) { //got recent feedback from both boards
|
||||||
|
state_modechange++;
|
||||||
|
}
|
||||||
|
if (loopmillis - state_modechange_time > 5000) { //timeout
|
||||||
|
currentmode=error; //error
|
||||||
|
requestmode=currentmode;
|
||||||
|
errormessage="No feedback from board(s) on startup";
|
||||||
|
state_modechange=0;
|
||||||
|
}
|
||||||
|
// ### Request Idle or Off (both power boards off) ###
|
||||||
|
}else if(requestmode==idle || requestmode==off) { //wait for no response
|
||||||
|
if ( !serial1Active() && !serial2Active() ) { //no new data since some time
|
||||||
|
state_modechange++;
|
||||||
|
}
|
||||||
|
if (loopmillis - state_modechange_time > 5000) { //timeout
|
||||||
|
currentmode=error; //error
|
||||||
|
requestmode=currentmode;
|
||||||
|
errormessage="Boards did not turn off";
|
||||||
|
state_modechange=0;
|
||||||
|
}
|
||||||
|
}else{ //if changed off from error mode
|
||||||
|
state_modechange++;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default: //finished modechange
|
||||||
|
currentmode=requestmode;
|
||||||
|
state_modechange=0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
|
||||||
|
case error:
|
||||||
|
currentmode=error; //stay in this mode
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
currentmode=error;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
boolean serial1Active() {
|
||||||
|
return loopmillis-lastValidDataSerial1_time < 2000;
|
||||||
|
}
|
||||||
|
boolean serial2Active() {
|
||||||
|
return loopmillis-lastValidDataSerial2_time < 2000;
|
||||||
|
}
|
||||||
|
|
||||||
|
void modeloops() {
|
||||||
|
switch (requestmode) { //mode changes
|
||||||
|
case idle:
|
||||||
|
loop_idle();
|
||||||
|
break;
|
||||||
|
case on:
|
||||||
|
loop_on();
|
||||||
|
break;
|
||||||
|
case error:
|
||||||
|
loop_error();
|
||||||
|
break;
|
||||||
|
case off:
|
||||||
|
loop_off();
|
||||||
|
break;
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop_idle() {
|
||||||
|
out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop_on() {
|
||||||
|
int16_t speedvalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000);
|
||||||
|
|
||||||
|
out_speedFL=speedvalue;
|
||||||
|
out_speedFR=speedvalue;
|
||||||
|
out_speedRL=speedvalue;
|
||||||
|
out_speedRR=speedvalue;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop_error() {
|
||||||
|
out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors
|
||||||
|
|
||||||
|
//TODO: blink error led
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop_off() {
|
||||||
|
//loop enters when boards are sucessfully turned off
|
||||||
|
//TODO: led show
|
||||||
|
digitalWrite(PIN_ENABLE, LOW); //cut own power
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
// Old loop
|
||||||
|
void loopold() {
|
||||||
//selfTest(); //start selftest, does not return
|
//selfTest(); //start selftest, does not return
|
||||||
|
|
||||||
ReceiveSerial1(); // Check for new received data
|
ReceiveSerial1(); // Check for new received data
|
||||||
|
|
||||||
if (millis()>2000 && POWERBUTTON_DOWN) {
|
if (millis()>2000 && STARTBUTTON_DOWN) {
|
||||||
poweronBoards();
|
poweronBoards();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (millis() - last_send > SENDPERIOD) {
|
if (millis() - last_send > SENDPERIOD) {
|
||||||
//Serial.print("powerbutton="); Serial.print(POWERBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
//Serial.print("powerbutton="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
||||||
|
|
||||||
int _read=analogRead(PIN_THROTTLE);
|
int _read=analogRead(PIN_THROTTLE);
|
||||||
|
|
||||||
|
|
@ -296,7 +439,7 @@ void loop() {
|
||||||
|
|
||||||
last_send = millis();
|
last_send = millis();
|
||||||
|
|
||||||
digitalWrite(PIN_POWERLED, !digitalRead(PIN_POWERLED));
|
digitalWrite(PIN_STARTLED, !digitalRead(PIN_STARTLED));
|
||||||
if (testcounter%3==0) {
|
if (testcounter%3==0) {
|
||||||
digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN));
|
digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN));
|
||||||
}
|
}
|
||||||
|
|
@ -336,7 +479,7 @@ void loop() {
|
||||||
Serial.print(", "); Serial.println(Feedback2.boardTemp);
|
Serial.print(", "); Serial.println(Feedback2.boardTemp);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (millis()>30000 && POWERBUTTON_DOWN) {
|
if (millis()>30000 && STARTBUTTON_DOWN) {
|
||||||
poweroff();
|
poweroff();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -370,7 +513,7 @@ void poweronBoards() {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
void selfTest() {
|
void selfTest() {
|
||||||
|
|
@ -379,8 +522,8 @@ void selfTest() {
|
||||||
#define TESTDELAY 1000 //delay between test
|
#define TESTDELAY 1000 //delay between test
|
||||||
#define TESTTIME 500 //time to keep tested pin on
|
#define TESTTIME 500 //time to keep tested pin on
|
||||||
|
|
||||||
delay(TESTDELAY); Serial.println("PIN_POWERLED");
|
delay(TESTDELAY); Serial.println("PIN_STARTLED");
|
||||||
digitalWrite(PIN_POWERLED,HIGH); delay(TESTTIME); digitalWrite(PIN_POWERLED,LOW);
|
digitalWrite(PIN_STARTLED,HIGH); delay(TESTTIME); digitalWrite(PIN_STARTLED,LOW);
|
||||||
|
|
||||||
delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN");
|
delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN");
|
||||||
digitalWrite(PIN_MODELED_GREEN,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_GREEN,HIGH);
|
digitalWrite(PIN_MODELED_GREEN,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_GREEN,HIGH);
|
||||||
|
|
@ -395,13 +538,13 @@ void selfTest() {
|
||||||
digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISREAR,LOW);
|
digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISREAR,LOW);
|
||||||
|
|
||||||
delay(TESTDELAY); Serial.println("ALL ON");
|
delay(TESTDELAY); Serial.println("ALL ON");
|
||||||
digitalWrite(PIN_POWERLED,HIGH);
|
digitalWrite(PIN_STARTLED,HIGH);
|
||||||
digitalWrite(PIN_MODELED_GREEN,LOW);
|
digitalWrite(PIN_MODELED_GREEN,LOW);
|
||||||
digitalWrite(PIN_MODELED_RED,LOW);
|
digitalWrite(PIN_MODELED_RED,LOW);
|
||||||
digitalWrite(PIN_RELAISFRONT,HIGH);
|
digitalWrite(PIN_RELAISFRONT,HIGH);
|
||||||
digitalWrite(PIN_RELAISREAR,HIGH);
|
digitalWrite(PIN_RELAISREAR,HIGH);
|
||||||
delay(TESTTIME*5);
|
delay(TESTTIME*5);
|
||||||
digitalWrite(PIN_POWERLED,LOW);
|
digitalWrite(PIN_STARTLED,LOW);
|
||||||
digitalWrite(PIN_MODELED_GREEN,HIGH);
|
digitalWrite(PIN_MODELED_GREEN,HIGH);
|
||||||
digitalWrite(PIN_MODELED_RED,HIGH);
|
digitalWrite(PIN_MODELED_RED,HIGH);
|
||||||
digitalWrite(PIN_RELAISFRONT,LOW);
|
digitalWrite(PIN_RELAISFRONT,LOW);
|
||||||
|
|
@ -413,7 +556,7 @@ void selfTest() {
|
||||||
while(true) { //Keep printing input values forever
|
while(true) { //Keep printing input values forever
|
||||||
delay(100);
|
delay(100);
|
||||||
Serial.print("millis="); Serial.print(millis()); Serial.print(", throttle ADC="); Serial.println(analogRead(PIN_THROTTLE));
|
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);
|
Serial.print("powerbutton down="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
||||||
|
|
||||||
while (millis()>=60000) {
|
while (millis()>=60000) {
|
||||||
digitalWrite(PIN_ENABLE, LOW); //poweroff own latch
|
digitalWrite(PIN_ENABLE, LOW); //poweroff own latch
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,129 @@
|
||||||
|
|
||||||
|
// ########################## 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));
|
||||||
|
lastValidDataSerial1_time = millis();
|
||||||
|
}
|
||||||
|
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));
|
||||||
|
lastValidDataSerial2_time = millis();
|
||||||
|
}
|
||||||
|
idx2 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update previous states
|
||||||
|
incomingBytePrev2 = incomingByte2;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue