power up/down and speed works. unreliable with power on check

controller/controller.ino

@@ -50,6 +50,8 @@ boolean button_hold_start=false;
 #define TIME_AUTOPOWEROFF 600000 //600000 = 10 minutes
 
 long loopmillis=0; //only use one millis reading each loop
+long last_looptime=0; //for looptiming
+#define LOOPTIME 10 //how often the loop(s) should run
 long millis_lastchange=0; //for poweroff after some time with no movement
 
 String errormessage=""; //store some error message to print
@@ -72,8 +74,10 @@ uint16_t out_speedRR=0;
 
 long last_send = 0;
 
+boolean board1Enabled=false;
+boolean board2Enabled=false;
 
-// Global variables
+// Global variables for serial communication
 uint8_t idx1 = 0;                        // Index for new data pointer
 uint16_t bufStartFrame1;                 // Buffer Start Frame
 byte *p1;                                // Pointer declaration for the new received data
@@ -117,7 +121,7 @@ SerialFeedback Feedback2;
 SerialFeedback NewFeedback2;
 
 
-enum mode{idle, on, error, off};
+enum mode{booting, idle, on, error, off};
 /*
  * idle: controller is on, hoverboards are off
  * on: hoverbaords are on and happy
@@ -127,6 +131,9 @@ enum mode{idle, on, error, off};
 mode currentmode; //current active mode
 mode requestmode; //change this variable to initiate a mode change
 
+mode last_requestmode=off; //for printout
+mode last_currentmode=off; //for printout
+
 // ########################## SETUP ##########################
 void setup() 
 {
@@ -151,7 +158,7 @@ void setup()
 
   Serial.println("Initialized"); 
 
-  currentmode = idle; //start in idle mode
+  currentmode = booting; //start in idle mode
   requestmode = currentmode;
 
   millis_lastchange=millis();
@@ -166,7 +173,22 @@ void loop() {
   ReceiveSerial2(); // Check for new received data
 
   handleInputs();
+  if (button_start) {
+    Serial.println("button_start");
+  }
+  if (button_hold_start) {
+    Serial.println("button_hold_start");
+  }
+  
   handleModeChange(); //mode changes
+  if (last_requestmode!=requestmode) {
+    Serial.print("requestmode="); Serial.println(modeToString(requestmode)); 
+    last_requestmode=requestmode;
+  }
+  if (last_currentmode!=currentmode) {
+    Serial.print("currentmode="); Serial.println(modeToString(currentmode));
+    last_currentmode=currentmode;
+  }
   
   modeloops();
 
@@ -177,9 +199,13 @@ void loop() {
       SendSerial2(out_speedFL,out_speedFR); //Front
       SendSerial1(out_speedRL,out_speedRR); //Rear
     }
+    if (currentmode==on) {
+      Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); Serial.print(", speedFL="); Serial.println(out_speedFL);
+    }
   }
 }
 
+
 void handleInputs()
 {
   //Short press (true when button short pressed, on release)
@@ -193,11 +219,13 @@ void handleInputs()
     //Trigger
     if (timebuttonpressed_start == 0 && STARTBUTTON_DOWN){ //first time pressed down. (low when pressed)
       timebuttonpressed_start=loopmillis; //set time of button press
+      millis_lastinput=loopmillis;
     }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
+      millis_lastinput=loopmillis;
     }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
@@ -224,12 +252,17 @@ void handleInputs()
 }
 
 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 booting: //on startup.  active while start button is still pressed
+        if (button_start) { //button first release
+          requestmode=idle; //start in idle state
+          state_modechange=0; //reset state for safety
+        }//TODO else if (button_hold_start) { requestmode=on; }
+        //TODO: led show
+        break;
       case idle:
         if (button_hold_start){  //long press
           requestmode=on; //long press switches betweeen idle and on
@@ -263,71 +296,95 @@ void handleModeChange() {
     }
   }else{ // ## Change requested ## 
     switch (requestmode) { //mode changes
+      case booting:
+        requestmode=error;
+        currentmode=requestmode;
+        errormessage="Change to booting mode cannot be requested";
+        break;
       case idle: case on: case off: //similar for on, idle and off
-          switch(state_modechange) {
+          if (currentmode == booting) { //coming from booting mode
-            case 0: 
+            currentmode=idle; //switch directly without powering boards
-              if (requestmode==on && adc_throttle > ADC_CALIB_THROTTLE_LOWEST) { //requested to turn on but throttle is pressed
+            requestmode=currentmode; //make shure it stay in this mode
+            state_modechange=0;
+            break;
+          }
+          if ( (state_modechange>0 || (requestmode==idle && boardsPowered()) || (requestmode==off && boardsPowered()) || (requestmode==on && !boardsPowered()) )) { //power cylce in progress OR need to power on/off boards
+            //Hoverboard powering
+            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++;
+                    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;
+                  }
+                // ### 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 changed off from error mode
+                  state_modechange++;
+                }
+                break;
+              default: //finished modechange
+                currentmode=requestmode;
                 state_modechange=0;
-                requestmode=currentmode; //abort modechange
+                break;
-                //TODO: led show aborted modechange
+            }
-              }else{ //everythings fine, turn on/off
+          }else{
-                digitalWrite(PIN_RELAISFRONT,HIGH); //simulate hoverboard power button press
+            currentmode=requestmode;
-                state_modechange++;
+            state_modechange=0; //for safety
-                state_modechange_time=loopmillis; //set to current time
+            //Should not happen
-              }
+            Serial.print("Warning: power cycle not needed. board1Enabled="); Serial.print(board1Enabled); Serial.print("board2Enabled="); Serial.println(board2Enabled);
-              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;
@@ -350,7 +407,12 @@ boolean serial2Active() {
 }
 
 void modeloops() {
-  switch (requestmode) { //mode changes
+  if (loopmillis - last_looptime >= LOOPTIME) {
+    last_looptime=loopmillis;
+    switch (requestmode) { //mode changes
+      case booting: 
+        //TODO: LED effect
+        break;
       case idle: 
         loop_idle();
         break;
@@ -363,8 +425,8 @@ void modeloops() {
       case off:
         loop_off();
         break;
-      
     }
+  }
 }
 
 void loop_idle() {
@@ -383,7 +445,7 @@ void loop_on() {
 
 void loop_error() {
   out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors
-
+  Serial.print("Error:"); Serial.println(errormessage);
   //TODO: blink error led
   
 }
@@ -395,10 +457,26 @@ void loop_off() {
   
 }
 
+boolean boardsPowered()
+{
+  if (serial1Active()){
+    board1Enabled=true;
+  }
+  if (serial2Active()){
+    board2Enabled=true;
+  }
+  
+  return (board1Enabled && board2Enabled); //true if both boards enabled
+}
 
 
-
+String modeToString(uint8_t m) {
-
+  if (m==idle) return "idle";
+  if (m==off) return "off";
+  if (m==error) return "error";
+  if (m==on) return "on";
+  if (m==booting) return "booting";
+}