static void StateMachine(void) {
bool finished = FALSE;
switch (LF_currState) {
case STATE_IDLE:
break;
case STATE_FOLLOW_SEGMENT:
if (!FollowSegment()) {
LF_currState = STATE_TURN; /* Lost line, do U-turn */
}
break;
case STATE_TURN:
if(MAZE_EvaluteTurn(&finished, rule)== ERR_FAILED){
LF_currState = STATE_STOP;
break;
}
if(finished == TRUE){
LF_currState = STATE_FINISHED;
}
else {
LF_currState = STATE_FOLLOW_SEGMENT;
break;
}
case STATE_FINISHED:
TURN_Turn(TURN_RIGHT180, NULL); /*to do: U-Turn invert track go back */
//SHELL_SendString("LINE: Finished!\r\n");
MAZE_SetSolved();
LF_currState=STATE_FOLLOW_SEGMENT;
break;
case STATE_STOP:
//SHELL_SendString("LINE: Stop!\r\n");
TURN_Turn(TURN_STOP, NULL);
LF_currState = STATE_IDLE;
break;
} /* switch */
}
TURN_Kind MAZE_SelectTurn(REF_LineKind prev, REF_LineKind curr) {
if (prev==REF_LINE_NONE && curr==REF_LINE_NONE) { /* dead end */
return TURN_RIGHT180; /* make U turn */
}
else if (prev==REF_LINE_LEFT && curr==REF_LINE_NONE){ // Linkskurve
return TURN_LEFT90;
}
else if(prev==REF_LINE_LEFT && curr == REF_LINE_STRAIGHT){ // Links und gerade
if (LEFT_WALL){
return TURN_LEFT90;
}
else{
return TURN_STRAIGHT;
}
}
else if(prev==REF_LINE_RIGHT && curr == REF_LINE_STRAIGHT){ // Rechts und gerade
if(LEFT_WALL){
return TURN_STRAIGHT;
}
else{
return TURN_RIGHT90;
}
}
else if (prev==REF_LINE_RIGHT && curr==REF_LINE_NONE){ // Rechtskurve
return TURN_RIGHT90;
}
else if (prev==REF_LINE_FULL && curr==REF_LINE_STRAIGHT){ // Kreuzung
if (LEFT_WALL){
return TURN_LEFT90;
}
else{
return TURN_RIGHT90;
}
}
else if (prev==REF_LINE_FULL && curr==REF_LINE_NONE){
if(LEFT_WALL){
return TURN_LEFT90;
}
else{
return TURN_RIGHT90;
}
}
else if (prev==REF_LINE_FULL && curr==REF_LINE_FULL){
MAZE_SetSolved(); // Ziel gefunden
return TURN_LEFT180; // Umkeheren
}
else{
/*! \todo Implement all cases for turning */
return TURN_STOP; /* error case */
}
}
/*!
* \brief Performs a turn.
* \return Returns TRUE while turn is still in progress.
*/
uint8_t MAZE_EvaluteTurn(bool *finished, bool lefthand) {
REF_LineKind historyLineKind, currLineKind;
TURN_Kind turn;
*finished = FALSE;
currLineKind = REF_GetLineKind();
if (currLineKind == REF_LINE_NONE) { /* nothing, must be dead end */
turn = TURN_LEFT180;
} else {
MAZE_ClearSensorHistory(); /* clear history values */
MAZE_SampleSensorHistory(); /* store current values */
TURN_Turn(TURN_STEP_LINE_FW_POST_LINE, MAZE_SampleTurnStopFunction); /* do the line and beyond in one step */
historyLineKind = MAZE_HistoryLineKind(); /* new read new values */
currLineKind = REF_GetLineKind();
turn = MAZE_SelectTurn(historyLineKind, currLineKind, lefthand);
}
if(turn != TURN_FINISHED){
MAZE_AddPath(turn);
}
if (turn == TURN_FINISHED) {
MAZE_SimplifyPath();
MAZE_RevertPath();
MAZE_SetSolved();
TURN_Turn(TURN_LEFT180, NULL);
*finished = TRUE;
SHELL_SendString((unsigned char*) "MAZE: finished!\r\n");
return ERR_OK;
} else if (turn == TURN_STRAIGHT) {
/*! \todo Extend if necessary */
SHELL_SendString((unsigned char*) "going straight\r\n");
return ERR_OK;
} else if (turn == TURN_STOP) { /* should not happen here? */
//LF_StopFollowing();
SHELL_SendString((unsigned char*) "Failure, stopped!!!\r\n");
return ERR_FAILED; /* error case */
} else { /* turn or do something */
/*! \todo Extend if necessary */
TURN_Turn(turn, NULL);
return ERR_OK; /* turn finished */
}
}
static void StateMachine(void) {
switch (LF_currState) {
case STATE_IDLE:
break;
case STATE_FOLLOW_SEGMENT:
if (!FollowSegment(LINE_FOLLOW_FW)) {
#if PL_APP_LINE_MAZE
LF_currState = STATE_TURN; /* make turn */
#else
LF_currState = STATE_STOP; /* stop if we do not have a line any more */
#endif
}
break;
#if PL_APP_LINE_MAZE
case STATE_FOLLOW_SEGMENT_BW:
if (!FollowSegment(FALSE)) {
TURN_Turn(TURN_STOP);
if (EvaluateTurnBw()==ERR_OK) {
LF_currState = STATE_FOLLOW_SEGMENT;
} else {
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_APP_LINE_MAZE
case STATE_TURN:
if (MAZE_IsSolved()) {
TURN_Kind turn;
turn = MAZE_GetSolvedTurn(&LF_solvedIdx);
if (turn==TURN_STOP) { /* last turn reached */
TURN_Turn(turn);
LF_currState = STATE_FINISHED;
} else { /* perform turning */
TURN_Turn(TURN_STEP_LINE_FW); /* Step over line */
TURN_Turn(TURN_STEP_POST_LINE_FW); /* step before doing the turn */
TURN_Turn(turn);
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* still evaluating maze */
bool deadEndGoBw = FALSE;
bool finished = FALSE;
if (EvaluteTurn(&finished, &deadEndGoBw)==ERR_OK) { /* finished turning */
if (finished) {
LF_currState = STATE_FINISHED;
MAZE_SetSolved();
#if PL_TURN_ON_FINISH
/* turn the robot */
TURN_Turn(TURN_LEFT180);
#endif
TURN_Turn(TURN_STOP);
/* now ready to do line following */
} else if (deadEndGoBw) {
LF_currState = STATE_FOLLOW_SEGMENT_BW;
} else {
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* error case */
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_APP_LINE_MAZE
case STATE_FINISHED:
#if PL_HAS_BUZZER
{
uint8_t i;
for(i=0;i<4;i++) {
(void)BUZ_Beep(300, 100);
WAIT1_WaitOSms(500);
}
}
#endif
LF_currState = STATE_STOP;
break;
#endif
case STATE_STOP:
TURN_Turn(TURN_STOP);
LF_currState = STATE_IDLE;
break;
} /* switch */
}
