From 05c42ccbc5b8da1181ddcbfdfed2ecb021bec217 Mon Sep 17 00:00:00 2001 From: Fisch Date: Wed, 11 Dec 2019 21:22:54 +0100 Subject: [PATCH] add state machine for modes --- controller/controller.ino | 495 ++++++++++++++++++++++------------- controller/serialControl.ino | 129 +++++++++ 2 files changed, 448 insertions(+), 176 deletions(-) create mode 100644 controller/serialControl.ino diff --git a/controller/controller.ino b/controller/controller.ino index 85fcab7..f71b4dc 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -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 -// -// ******************************************************************* -// 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 @@ -41,12 +17,13 @@ //#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 ADC_CALIB_THROTTLE_LOWEST 1900 //a bit above maximum adc value if throttle it not touched +#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_POWERBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed -#define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON) +#define PIN_STARTLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor +#define PIN_STARTBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed +#define STARTBUTTON_DOWN digitalRead(PIN_STARTBUTTON) #define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial #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_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; @@ -73,6 +79,7 @@ uint16_t bufStartFrame1; // Buffer Start Frame byte *p1; // Pointer declaration for the new received data byte incomingByte1; byte incomingBytePrev1; +long lastValidDataSerial1_time; //Same for Serial2 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 incomingByte2; byte incomingBytePrev2; +long lastValidDataSerial2_time; typedef struct{ @@ -108,21 +116,31 @@ SerialFeedback NewFeedback1; SerialFeedback Feedback2; 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 ########################## 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); + Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector + Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector + + // Pin Setup + pinMode(PIN_STARTLED, OUTPUT); pinMode(PIN_ENABLE, OUTPUT); digitalWrite(PIN_ENABLE, HIGH); //keep power on - pinMode(PIN_POWERBUTTON, INPUT_PULLUP); + pinMode(PIN_STARTBUTTON, INPUT_PULLUP); pinMode(PIN_MODESWITCH, INPUT_PULLUP); pinMode(PIN_MODELED_GREEN, OUTPUT); pinMode(PIN_MODELED_RED, OUTPUT); @@ -132,150 +150,275 @@ void setup() pinMode(PIN_BRAKE, INPUT); 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 ########################## - - - 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 ReceiveSerial1(); // Check for new received data - if (millis()>2000 && POWERBUTTON_DOWN) { + if (millis()>2000 && STARTBUTTON_DOWN) { poweronBoards(); } 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); @@ -296,7 +439,7 @@ void loop() { last_send = millis(); - digitalWrite(PIN_POWERLED, !digitalRead(PIN_POWERLED)); + digitalWrite(PIN_STARTLED, !digitalRead(PIN_STARTLED)); if (testcounter%3==0) { digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN)); } @@ -336,7 +479,7 @@ void loop() { Serial.print(", "); Serial.println(Feedback2.boardTemp); } - if (millis()>30000 && POWERBUTTON_DOWN) { + if (millis()>30000 && STARTBUTTON_DOWN) { poweroff(); } @@ -370,7 +513,7 @@ void poweronBoards() { } - +*/ void selfTest() { @@ -379,8 +522,8 @@ void 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_STARTLED"); + digitalWrite(PIN_STARTLED,HIGH); delay(TESTTIME); digitalWrite(PIN_STARTLED,LOW); delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN"); 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); delay(TESTDELAY); Serial.println("ALL ON"); - digitalWrite(PIN_POWERLED,HIGH); + digitalWrite(PIN_STARTLED,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_STARTLED,LOW); digitalWrite(PIN_MODELED_GREEN,HIGH); digitalWrite(PIN_MODELED_RED,HIGH); digitalWrite(PIN_RELAISFRONT,LOW); @@ -413,7 +556,7 @@ void selfTest() { 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); + Serial.print("powerbutton down="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN); while (millis()>=60000) { digitalWrite(PIN_ENABLE, LOW); //poweroff own latch diff --git a/controller/serialControl.ino b/controller/serialControl.ino new file mode 100644 index 0000000..86eebe0 --- /dev/null +++ b/controller/serialControl.ino @@ -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; +}