diff --git a/controller/controller.ino b/controller/controller.ino index 62290db..2e75963 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -76,6 +76,8 @@ long last_send = 0; boolean board1Enabled=false; boolean board2Enabled=false; +#define EXPECT_FEEDBACK_ABOVE_SPEED 250 //for error checking +#define SERIALACTIVECHECKTIME 3000 //time to say a board is online/active when last message received in the last x ms // Global variables for serial communication uint8_t idx1 = 0; // Index for new data pointer @@ -84,6 +86,8 @@ byte *p1; // Pointer declaration for the new rece byte incomingByte1; byte incomingBytePrev1; long lastValidDataSerial1_time; +long lastSendSerial1expectFeedback; //to check time difference between last >0 speed send and last received feedback +long lastBoard1PowerChange; //Same for Serial2 uint8_t idx2 = 0; // Index for new data pointer @@ -92,7 +96,8 @@ byte *p2; // Pointer declaration for the new rece byte incomingByte2; byte incomingBytePrev2; long lastValidDataSerial2_time; - +long lastSendSerial2expectFeedback; //to check time difference between last >0 speed send and last received feedback +long lastBoard2PowerChange; typedef struct{ uint16_t start; @@ -140,8 +145,8 @@ void setup() Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) - 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 + Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear) + Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front) // Pin Setup pinMode(PIN_STARTLED, OUTPUT); @@ -203,6 +208,19 @@ void loop() { Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); Serial.print(", speedFL="); Serial.println(out_speedFL); } } + + //for lastValidSerialData calculation + if (abs(out_speedRL)>EXPECT_FEEDBACK_ABOVE_SPEED || abs(out_speedRR)>EXPECT_FEEDBACK_ABOVE_SPEED) { //Rear is serial 1 + lastSendSerial1expectFeedback=loopmillis; + } + if (abs(out_speedFL)>EXPECT_FEEDBACK_ABOVE_SPEED || abs(out_speedFR)>EXPECT_FEEDBACK_ABOVE_SPEED) { //Front is serial 2 + lastSendSerial2expectFeedback=loopmillis; + } + + + if (currentmode!=error) { //keep last errormessage + failChecks(); + } } @@ -346,31 +364,24 @@ void handleModeChange() { 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++; - boardsPowered(); //check boards powered to set variable board1/2Enabled to true - } - if (loopmillis - state_modechange_time > 5000) { //timeout - currentmode=error; //error - requestmode=currentmode; - errormessage="No feedback from board(s) on startup"; - state_modechange=0; - board1Enabled=false; - board2Enabled=false; - } + state_modechange++; + board1Enabled=true; //assume board is online + board2Enabled=true; //assume board is online + lastBoard1PowerChange=loopmillis; + lastBoard2PowerChange=loopmillis; + lastSendSerial1expectFeedback=loopmillis; //expect maybe feedback on power on + lastSendSerial2expectFeedback=loopmillis; + lastValidDataSerial1_time=loopmillis; //assume a feedback was received (boards do not send feedback on poweron) + lastValidDataSerial2_time=loopmillis; //assume a feedback was received (boards do not send feedback on poweron) // ### 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++; - board1Enabled=false; - board2Enabled=false; - } - if (loopmillis - state_modechange_time > 5000) { //timeout - currentmode=error; //error - requestmode=currentmode; - errormessage="Boards did not turn off"; - state_modechange=0; - } + }else if(requestmode==idle || requestmode==off) { + state_modechange++; + board1Enabled=false; //assume board is offline + board2Enabled=false; //assume board is offline + lastBoard1PowerChange=loopmillis; + lastBoard2PowerChange=loopmillis; + lastSendSerial1expectFeedback=loopmillis; //expect maybe feedback on power on + lastSendSerial2expectFeedback=loopmillis; }else{ //if changed off from error mode state_modechange++; } @@ -399,11 +410,30 @@ void handleModeChange() { } } + boolean serial1Active() { - return loopmillis-lastValidDataSerial1_time < 2000; + if (loopmillis-lastBoard1PowerChange<=SERIALACTIVECHECKTIME) { + return board1Enabled; //unclear if mcu just started or board is just starting up. reply with exprected answer + } + if (lastValidDataSerial1_time==0 && lastSendSerial1expectFeedback==0) { //no valid data received and send ever + return false; + } + if (loopmillis-lastSendSerial1expectFeedback <= SERIALACTIVECHECKTIME){ //enough time passed since last exprected feedback + return board1Enabled; //unclear, return exprected value + } + return loopmillis-lastValidDataSerial1_time < SERIALACTIVECHECKTIME; } boolean serial2Active() { - return loopmillis-lastValidDataSerial2_time < 2000; + if (loopmillis-lastBoard2PowerChange<=SERIALACTIVECHECKTIME) { + return board2Enabled; //unclear if mcu just started or board is just starting up. reply with exprected answer + } + if (lastValidDataSerial2_time==0 && lastSendSerial2expectFeedback==0) { //no valid data received and send ever + return false; + } + if (loopmillis-lastSendSerial2expectFeedback <= SERIALACTIVECHECKTIME){ //enough time passed since last exprected feedback + return board2Enabled; //unclear, return exprected value + } + return loopmillis-lastValidDataSerial2_time < SERIALACTIVECHECKTIME; } void modeloops() { @@ -459,16 +489,41 @@ void loop_off() { boolean boardsPowered() { - if (serial1Active()){ - board1Enabled=true; - } - if (serial2Active()){ - board2Enabled=true; - } - return (board1Enabled && board2Enabled); //true if both boards enabled } +void failChecks() +{ + if (serial1Active() && !board1Enabled){ //serial is active and board should be offline + errormessage="Board 1 is online but shouldnt"; + requestmode=error; + } + if (serial2Active() && !board2Enabled){ //serial is active and board should be offline + errormessage="Board 2 is online but shouldnt"; + requestmode=error; + Serial.print("loopmillis="); Serial.println(loopmillis); + Serial.print("lastBoard2PowerChange="); Serial.println(lastBoard2PowerChange); + Serial.print("lastSendSerial2expectFeedback="); Serial.println(lastSendSerial2expectFeedback); + Serial.print("lastValidDataSerial2_time="); Serial.println(lastValidDataSerial2_time); + } + if (out_speedRL != 0 || out_speedRR != 0) { //Rear is Serial1 + if (!serial1Active() && board1Enabled){ //serial is not active and board should be online. only check this if feedback can be exprected (out_speedFL/R > 0) + errormessage="Board 1 is offline but should be online"; + requestmode=error; + } + } + if (out_speedFL != 0 || out_speedFR != 0) { //Front is Serial2 + if (!serial2Active() && board2Enabled){ //serial is not active and board should be online. only check this if feedback can be exprected (out_speedFL/R > 0) + errormessage="Board 2 is offline but should be online"; + requestmode=error; + Serial.print("loopmillis="); Serial.println(loopmillis); + Serial.print("lastBoard2PowerChange="); Serial.println(lastBoard2PowerChange); + Serial.print("lastSendSerial2expectFeedback="); Serial.println(lastSendSerial2expectFeedback); + Serial.print("lastValidDataSerial2_time="); Serial.println(lastValidDataSerial2_time); + } + } + +} String modeToString(uint8_t m) { if (m==idle) return "idle";