void REF_Measure(void) {
ReadCalibrated(SensorCalibrated, SensorRaw);
#if PL_APP_LINE_MAZE
REF_SampleHistory();
#endif
refCenterLineVal = ReadLine(SensorCalibrated, SensorRaw, FALSE);
if (isCalibrated) {
refLineKind = ReadLineKind(SensorCalibrated);
}
}
/*!
* \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 */
}
}
