/*!
* \brief Performs a turn.
* \return Returns TRUE while turn is still in progress.
*/
uint8_t MAZE_EvaluteTurn(bool *finished) {
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);
}
if (turn==TURN_FINISHED) {
*finished = TRUE;
LF_StopFollowing();
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 */
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 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 */
}
}
}
uint8_t LF_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"line help")==0) {
LF_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"line status")==0) {
LF_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"line start")==0) {
LF_StartFollowing();
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"line stop")==0) {
LF_StopFollowing();
*handled = TRUE;
}
return res;
}
static void StateMachine(bool buttonPress) {
static uint8_t cnt;
switch (appState) {
case APP_STATE_INIT:
LEDG_Off();
LEDR_On();
if (buttonPress) {
APP_StateStartCalibrate();
}
break;
case APP_STATE_CALIBRATE:
cnt++;
if (cnt>50) {
cnt = 0;
LEDR_Neg();
}
if (buttonPress) {
APP_StateStopCalibrate();
}
break;
case APP_STATE_IDLE:
LEDR_Off();
LEDG_On();
if (buttonPress) {
LF_StartFollowing();
appState = APP_STATE_FOLLOW;
}
break;
case APP_STATE_FOLLOW:
LEDR_Off();
LEDG_Off();
if (!LF_IsFollowing()) {
appState = APP_STATE_IDLE;
}
if (buttonPress) {
LF_StopFollowing();
appState = APP_STATE_IDLE;
}
break;
} /* switch */
}
static void StateMachine(bool buttonPress) {
#if PL_APP_FOLLOW_OBSTACLE || PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
static uint8_t cnt;
#endif
switch (appState) {
case APP_STATE_INIT:
LEDG_Off();
LEDR_On();
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
if (buttonPress) {
APP_StateStartCalibrate();
}
#elif PL_APP_FOLLOW_OBSTACLE
appState = APP_STATE_IDLE;
#endif
break;
#if PL_APP_FOLLOW_OBSTACLE
case APP_STATE_FOLLOW_OBSTACLE:
cnt++;
if (cnt>50) {
cnt = 0;
LEDR_Neg();
}
if (buttonPress) {
followObstacle = FALSE;
appState = APP_STATE_IDLE;
}
break;
#endif
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
case APP_STATE_CALIBRATE:
cnt++;
if (cnt>50) {
cnt = 0;
LEDR_Neg();
}
if (buttonPress) {
APP_StateStopCalibrate();
}
break;
#endif
case APP_STATE_IDLE:
LEDR_Off();
LEDG_On();
if (buttonPress) {
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
LF_StartFollowing();
appState = APP_STATE_FOLLOW_LINE;
#elif PL_APP_FOLLOW_OBSTACLE
followObstacle = TRUE;
appState = APP_STATE_FOLLOW_OBSTACLE;
#endif
}
break;
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
case APP_STATE_FOLLOW_LINE:
LEDR_Off();
LEDG_Off();
if (!LF_IsFollowing()) {
appState = APP_STATE_IDLE;
}
if (buttonPress) {
LF_StopFollowing();
appState = APP_STATE_IDLE;
}
break;
#endif
} /* switch */
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-15 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button - disable remote, drive mode none */
DRV_SetMode(DRV_MODE_NONE);
REMOTE_SetOnOff(FALSE);
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
REMOTE_SetOnOff(TRUE);
#if PL_CONFIG_HAS_DRIVE
DRV_SetMode(DRV_MODE_SPEED);
#endif
SHELL_SendString("Remote ON\r\n");
}
#if PL_CONFIG_HAS_LINE_MAZE
else if (val=='A') { /* green 'A' button */
SHELL_SendString("Button A pressed\r\n");
// Start Maze solving
if(!LF_IsFollowing()){
LF_StartFollowing();
}
} else if (val=='K') { /* green A button longpress -> 'K' */
SHELL_SendString("Clear Maze\r\n");
// Clear old maze solution, ready for restart
MAZE_ClearSolution();
} else if (val =='B'){ /* yellow 'B' button */
SHELL_SendString("Right hand rule!\r\n");
LF_SetRule(FALSE);
} else if (val=='E') { /* button 'E' pressed */
SHELL_SendString("Stop Following! \r\n");
if(LF_IsFollowing()){
LF_StopFollowing();
}
} else if (val=='D') { /* blue 'D' button */
SHELL_SendString("Left hand rule!\r\n");
LF_SetRule(TRUE);
}
#endif
else if (val=='C') { /* red 'C' button */
NITRO = TRUE;
SHELL_SendString("Nitrooooooo!!!\r\n");
BUZ_Beep(1000,1000);
} else if (val=='J') { /* button 'C' released */
NITRO = FALSE;
SHELL_SendString("Stop Nitro \r\n");
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}