static portTASK_FUNCTION(LineTask, pvParameters) { (void)pvParameters; /* not used */ for(;;) { if (LF_stopIt) { ChangeState(STATE_STOP); LF_stopIt = FALSE; } StateMachine(); if (LF_IsFollowing()) { FRTOS1_vTaskDelay(5/portTICK_RATE_MS); } else { FRTOS1_vTaskDelay(100/portTICK_RATE_MS); } } }
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 */ }
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) { if(!LF_IsFollowing()){ 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) { #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; }