/*! * \brief Performs a turn. * \return Returns TRUE while turn is still in progress. */ uint8_t MAZE_EvaluteTurn(bool *finished, bool rule) { REF_LineKind historyLineKind, currLineKind; TURN_Kind turn; if (MAZE_IsSolved()) { if (!FollowSegment()) { if (index < pathLength) { //TURN_Turn(TURN_STEP_LINE_FW_POST_LINE,MAZE_SampleTurnStopFunction); TURN_Turn(TURN_STEP_LINE_FW_POST_LINE, NULL); TURN_Turn(MAZE_GetSolvedTurn(&index), NULL); } else { TURN_Turn(TURN_STEP_LINE_FW_POST_LINE, NULL); LF_StopFollowing(); index = 0; } } return ERR_OK; } else { *finished = FALSE; currLineKind = REF_GetLineKind(); if (currLineKind == REF_LINE_NONE) { /* nothing, must be dead end */ MAZE_AddPath(TURN_LEFT180); 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, rule); } if (turn == TURN_FINISHED) { *finished = TRUE; //LF_StopFollowing(); return ERR_OK; } else if (turn == TURN_STRAIGHT) { //SHELL_SendString((unsigned char*) "going straight\r\n"); return ERR_OK; } else if (turn == TURN_STOP) { /* should not happen here? */ LF_StopFollowing(); return ERR_FAILED; /* error case */ } else { /* turn or do something */ TURN_Turn(turn, NULL); return ERR_OK; /* turn finished */ } } }
/*! * \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 */ } }
/*! * \brief Performs a turn while doing backward line following. * \return Returns TRUE while turn is still in progress. */ static uint8_t EvaluateTurnBw(void) { REF_LineKind historyLineKind, currLineKind; TURN_Kind turn; REF_ClearHistory(); /* clear values */ REF_SampleHistory(); /* store current values */ TURN_Turn(TURN_STEP_LINE_BW); /* make step over line */ historyLineKind = REF_HistoryLineKind(); /* new read new values */ currLineKind = REF_GetLineKind(); #if LINE_DEBUG REF_DumpHistory(); CLS1_SendStr((unsigned char*)" history: ", CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)REF_LineKindStr(historyLineKind), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)" curr: ", CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)REF_LineKindStr(currLineKind), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut); #endif turn = MAZE_SelectTurnBw(historyLineKind, currLineKind); if (turn==TURN_STOP) { /* should not happen here? */ ChangeState(STATE_STOP); CLS1_SendStr((unsigned char*)"stopped\r\n", CLS1_GetStdio()->stdOut); return ERR_FAILED; /* error case */ } else { /* turn or do something */ #if LINE_DEBUG CLS1_SendStr((unsigned char*)"bw turning ", CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)TURN_TurnKindStr(turn), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut); #endif TURN_Turn(TURN_STEP_LINE_FW); /* step over intersection */ TURN_Turn(TURN_STEP_POST_LINE_FW); /* step past intersection */ TURN_Turn(turn); /* make turn */ MAZE_AddPath(MirrorTurn(turn)); MAZE_SimplifyPath(); return ERR_OK; /* turn finished */ } }
/*! * \brief Performs a turn. * \return Returns TRUE while turn is still in progress. */ static uint8_t EvaluteTurn(bool *finished, bool *deadEndGoBw) { REF_LineKind historyLineKind, currLineKind; TURN_Kind turn; *finished = FALSE; /* defaults */ *deadEndGoBw = FALSE; /* default */ currLineKind = REF_GetLineKind(); if (currLineKind==REF_LINE_NONE) { /* nothing, must be dead end */ #if PL_GO_DEADEND_BW TURN_Turn(TURN_STEP_BW); /* step back so we are again on the line for line following */ turn = TURN_STRAIGHT; *deadEndGoBw = TRUE; #else turn = TURN_LEFT180; #endif } else { REF_ClearHistory(); /* clear values */ REF_SampleHistory(); /* store current values */ TURN_Turn(TURN_STEP_LINE_FW); /* make forward step over line */ historyLineKind = REF_HistoryLineKind(); /* new read new values */ currLineKind = REF_GetLineKind(); #if LINE_DEBUG REF_DumpHistory(); CLS1_SendStr((unsigned char*)" history: ", CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)REF_LineKindStr(historyLineKind), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)" curr: ", CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)REF_LineKindStr(currLineKind), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut); #endif turn = MAZE_SelectTurn(historyLineKind, currLineKind); } if (turn==TURN_FINISHED) { *finished = TRUE; ChangeState(STATE_STOP); CLS1_SendStr((unsigned char*)"finished!\r\n", CLS1_GetStdio()->stdOut); return ERR_OK; } else if (turn==TURN_STRAIGHT && *deadEndGoBw) { MAZE_AddPath(TURN_LEFT180); /* would have been a turn around */ MAZE_SimplifyPath(); CLS1_SendStr((unsigned char*)"going backward\r\n", CLS1_GetStdio()->stdOut); return ERR_OK; } else if (turn==TURN_STRAIGHT) { MAZE_AddPath(turn); MAZE_SimplifyPath(); CLS1_SendStr((unsigned char*)"going straight\r\n", CLS1_GetStdio()->stdOut); return ERR_OK; } else if (turn==TURN_STOP) { /* should not happen here? */ ChangeState(STATE_STOP); CLS1_SendStr((unsigned char*)"stopped\r\n", CLS1_GetStdio()->stdOut); return ERR_FAILED; /* error case */ } else { /* turn or do something */ #if LINE_DEBUG CLS1_SendStr((unsigned char*)"turning ", CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)TURN_TurnKindStr(turn), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut); #endif if (turn==TURN_LEFT90 || turn==TURN_RIGHT90) { TURN_Turn(TURN_STEP_POST_LINE_FW); /* step before doing the turn so we turn on the middle of the intersection */ } TURN_Turn(turn); /* make turn */ MAZE_AddPath(turn); MAZE_SimplifyPath(); return ERR_OK; /* turn finished */ } }
void MAZE_SetSolved(void) { isSolved = TRUE; /*! \todo here the path could be reverted */ MAZE_RevertPath(); MAZE_AddPath(TURN_STOP); /* add an action to stop */ }
void MAZE_SetSolved(void) { isSolved = TRUE; MAZE_RevertPath(); MAZE_AddPath(TURN_STOP); /* add an action to stop */ }
/* rule: True for left hand on the wall, false for right hand on the wall */ TURN_Kind MAZE_SelectTurn(REF_LineKind prev, REF_LineKind curr, bool rule) { if (prev == REF_LINE_NONE && curr == REF_LINE_NONE) { /* dead end */ MAZE_AddPath(TURN_RIGHT180); return TURN_RIGHT180; /* make U turn */ } else if (prev == REF_LINE_LEFT && curr == REF_LINE_NONE) { MAZE_AddPath(TURN_LEFT90); return TURN_LEFT90; } else if (prev == REF_LINE_RIGHT && curr == REF_LINE_NONE) { MAZE_AddPath(TURN_RIGHT90); return TURN_RIGHT90; } else if (prev == REF_LINE_FULL && curr == REF_LINE_NONE) { if (rule) { MAZE_AddPath(TURN_LEFT90); return TURN_LEFT90; /* left hand on the wall */ } else { MAZE_AddPath(TURN_RIGHT90); return TURN_RIGHT90; /* right hand on the wall */ } } else if (prev == REF_LINE_FULL && curr == REF_LINE_STRAIGHT) { if (rule) { MAZE_AddPath(TURN_LEFT90); return TURN_LEFT90; } else { MAZE_AddPath(TURN_RIGHT90); return TURN_RIGHT90; } } else if (prev == REF_LINE_LEFT && curr == REF_LINE_STRAIGHT) { if (rule) { MAZE_AddPath(TURN_LEFT90); return TURN_LEFT90; } else { MAZE_AddPath(TURN_STRAIGHT); return TURN_STRAIGHT; } } else if (prev == REF_LINE_RIGHT && curr == REF_LINE_STRAIGHT) { if (rule) { MAZE_AddPath(TURN_STRAIGHT); return TURN_STRAIGHT; } else { MAZE_AddPath(TURN_RIGHT90); return TURN_RIGHT90; } } else if (prev == REF_LINE_FULL && curr == REF_LINE_FULL) { return TURN_FINISHED; } return TURN_STOP; /* error case */ }