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,8 +199,12 @@ 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()
 {
@@ -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,7 +296,20 @@ 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
+          if (currentmode == booting) { //coming from booting mode
+            currentmode=idle; //switch directly without powering boards
+            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
@@ -302,17 +348,22 @@ void handleModeChange() {
                 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
@@ -329,6 +380,12 @@ void handleModeChange() {
                 state_modechange=0;
                 break;
             }
+          }else{
+            currentmode=requestmode;
+            state_modechange=0; //for safety
+            //Should not happen
+            Serial.print("Warning: power cycle not needed. board1Enabled="); Serial.print(board1Enabled); Serial.print("board2Enabled="); Serial.println(board2Enabled);
+          }
           
         break;
   
@@ -350,7 +407,12 @@ boolean serial2Active() {
 }
 
 void modeloops() {
+  if (loopmillis - last_looptime >= LOOPTIME) {
+    last_looptime=loopmillis;
     switch (requestmode) { //mode changes
+      case booting: 
+        //TODO: LED effect
+        break;
       case idle: 
         loop_idle();
         break;
@@ -363,7 +425,7 @@ void modeloops() {
       case off:
         loop_off();
         break;
-      
+    }
   }
 }
 
@@ -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";
+}