/*! * \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; }