From c56fd25c9ce367da56522797e40bf44fb63d0223 Mon Sep 17 00:00:00 2001 From: Fisch Date: Thu, 12 Dec 2019 22:46:30 +0100 Subject: [PATCH] power up/down and speed works. unreliable with power on check --- controller/controller.ino | 230 +++++++++++++++++++++++++------------- 1 file changed, 154 insertions(+), 76 deletions(-) diff --git a/controller/controller.ino b/controller/controller.ino index f71b4dc..62290db 100644 --- a/controller/controller.ino +++ b/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,8 +173,23 @@ 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(); if (loopmillis - last_send > SENDPERIOD) { @@ -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 @@ -213,7 +241,7 @@ void handleInputs() 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_lastchange=loopmillis; } millis_lastadc=loopmillis; } @@ -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) { - case 0: - if (requestmode==on && adc_throttle > ADC_CALIB_THROTTLE_LOWEST) { //requested to turn on but throttle is pressed + 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 + 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 - //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; + } + }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() { - 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"; +}