//******************************************************************************
void ModesTask::run(void)
{
if (modes_push_button_.activated())
{
// Go to the next mode. This will automatically stop robot.
glo_modes_t next_modes;
next_modes.main_mode = (modes_.main_mode + 1) % NUM_MAIN_MODES;
next_modes.sub_mode = 0;
handle(next_modes);
}
switch (modes_.state)
{
case STATE_STOPPED:
stoppedState();
break;
case STATE_INITIALIZING:
initializingState();
break;
case STATE_NORMAL:
normalState();
break;
}
}
int State::setState(int newState) {
switch (m_state) {
case NOT_INITIALIZE: {
switch (newState) {
case RUNNING:
if (leavingNotInitializedState() != 0) {
setState_i(NOT_INITIALIZE);
return -1;
}
setState_i(STARTING);
if (enterRunningState() != 0) {
setState_i(NOT_INITIALIZE);
return -1;
}
setState_i(RUNNING);
if (runningState() != 0) {
setState_i(NOT_INITIALIZE);
return -1;
}
return 0;
case DESTROYED:
if (leavingStoppedState() != 0) {
setState_i(NOT_INITIALIZE);
return -1;
}
setState_i(DESTROYED);
if (enterDestroyedState() != 0) {
setState_i(NOT_INITIALIZE);
return -1;
}
//The final state
destroyedState();
return 0;
default:
return -1;
}
}
case RUNNING: {
switch (newState) {
case STOPPED:
if (leavingRunningState() != 0) {
setState_i(RUNNING);
return -1;
}
setState_i(STOPPING);
if (enterStoppedState() != 0) {
setState_i(RUNNING);
return -1;
}
setState_i(STOPPED);
if (stoppedState() != 0) {
setState_i(RUNNING);
return -1;
}
return 0;
default:
return -1;
}
}
case STOPPED: {
switch (newState) {
case RUNNING:
if (leavingStoppedState() != 0) {
setState_i(STOPPED);
return -1;
}
setState_i(STARTING);
if (enterRunningState() != 0) {
setState_i(STOPPED);
return -1;
}
setState_i(RUNNING);
if (runningState() != 0) {
setState_i(STOPPED);
return -1;
}
return 0;
case DESTROYED:
if (leavingStoppedState() != 0) {
setState_i(STOPPED);
return -1;
}
setState_i(DESTROYED);
if (enterDestroyedState() != 0) {
setState_i(STOPPED);
return -1;
}
//The final state
destroyedState();
return 0;
default:
return -1;
}
}
case DESTROYED: {
return -1;
}
default:
return -1;
}
}