interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

This commit is contained in:
interfisch 2019-12-12 22:46:30 +01:00
parent 05c42ccbc5
commit c56fd25c9c
1 changed files with 154 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
controller/controller.ino
View File

@ -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";
}