static void StateMachine(void) { bool finished = FALSE; switch (LF_currState) { case STATE_IDLE: break; case STATE_FOLLOW_SEGMENT: if (!FollowSegment()) { LF_currState = STATE_TURN; /* Lost line, do U-turn */ } break; case STATE_TURN: if(MAZE_EvaluteTurn(&finished, rule)== ERR_FAILED){ LF_currState = STATE_STOP; break; } if(finished == TRUE){ LF_currState = STATE_FINISHED; } else { LF_currState = STATE_FOLLOW_SEGMENT; break; } case STATE_FINISHED: TURN_Turn(TURN_RIGHT180, NULL); /*to do: U-Turn invert track go back */ //SHELL_SendString("LINE: Finished!\r\n"); MAZE_SetSolved(); LF_currState=STATE_FOLLOW_SEGMENT; break; case STATE_STOP: //SHELL_SendString("LINE: Stop!\r\n"); TURN_Turn(TURN_STOP, NULL); LF_currState = STATE_IDLE; break; } /* switch */ }
static void CheckButton(void) { uint32_t timeTicks; /* time in ticks */ #define BUTTON_CNT_MS 100 /* iteration count for button */ bool autoCalibrate = FALSE; if (SW1_GetVal()==0) { /* button pressed */ /* short press (1 beep): start or stop line following if calibrated * 1 s press (2 beep): calibrate manually * 2 s press (3 beep): calibrate with auto-move * 3 s press (4 beep or more): clear path * */ FRTOS1_vTaskDelay(50/portTICK_RATE_MS); /* simple debounce */ if (SW1_GetVal()==0) { /* still pressed */ LEDG_On(); timeTicks = 0; while(SW1_GetVal()==0 && timeTicks<=6000/BUTTON_CNT_MS) { FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS); if ((timeTicks%(1000/BUTTON_CNT_MS))==0) { #if PL_HAS_BUZZER BUZ_Beep(300, 200); #endif } timeTicks++; } /* wait until released */ autoCalibrate = FALSE; if (timeTicks<1000/BUTTON_CNT_MS) { /* less than 1 second */ CLS1_SendStr((unsigned char*)"button press.\r\n", CLS1_GetStdio()->stdOut); StateMachine(TRUE); /* <1 s, short button press, according to state machine */ } else if (timeTicks>=(1000/BUTTON_CNT_MS) && timeTicks<(2000/BUTTON_CNT_MS)) { CLS1_SendStr((unsigned char*)"calibrate.\r\n", CLS1_GetStdio()->stdOut); APP_StateStartCalibrate(); /* 1-2 s: start calibration by hand */ } else if (timeTicks>=(2000/BUTTON_CNT_MS) && timeTicks<(3000/BUTTON_CNT_MS)) { CLS1_SendStr((unsigned char*)"auto calibrate.\r\n", CLS1_GetStdio()->stdOut); APP_StateStartCalibrate(); /* 2-3 s: start auto calibration */ autoCalibrate = TRUE; } else if (timeTicks>=(3000/BUTTON_CNT_MS)) { CLS1_SendStr((unsigned char*)"delete solution.\r\n", CLS1_GetStdio()->stdOut); MAZE_ClearSolution(); } while (SW1_GetVal()==0) { /* wait until button is released */ FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS); } if (autoCalibrate) { CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", CLS1_GetStdio()->stdOut); /* perform automatic calibration */ WAIT1_WaitOSms(1500); /* wait some time */ TURN_Turn(TURN_LEFT90); TURN_Turn(TURN_RIGHT90); TURN_Turn(TURN_RIGHT90); TURN_Turn(TURN_LEFT90); TURN_Turn(TURN_STOP); APP_StateStopCalibrate(); CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", CLS1_GetStdio()->stdOut); } } } /* if */ }
/*! * \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 */ } }
static void FollowObstacle(void) { #define DUTY_SLOW 16 #define DUTY_MEDIUM 20 #define DUTY_FAST 23 static uint8_t cnt; uint16_t cm, us; cnt++; /* get called with 100 Hz, reduce to 10 Hz */ if (cnt==10) { us = US_Measure_us(); cnt = 0; if (followObstacle) { cm = US_usToCentimeters(us, 22); if (cm<10) { /* back up! */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -DUTY_SLOW); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -DUTY_SLOW); } else if (cm>=10 && cm<=20) { /* stand still */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); TURN_Turn(TURN_RIGHT45); /* try to avoid obstacle */ } else if (cm>20 && cm<=40) { /* forward slowly */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_MEDIUM); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_MEDIUM); } else if (cm>40 && cm<100) { /* forward fast */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_FAST); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_FAST); } else { /* nothing in range */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); } } } }
static void AutoCalibrateReflectance(const CLS1_StdIOType *io) { CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", io->stdOut); /* perform automatic calibration */ APP_StateStartCalibrate(io); TURN_Turn(TURN_LEFT90); WAIT1_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_RIGHT90); WAIT1_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_RIGHT90); WAIT1_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_LEFT90); WAIT1_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_STOP); APP_StateStopCalibrate(io); CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", io->stdOut); }
/*! * \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 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 */ } }
static void StateMachine(void) { switch (LF_currState) { case STATE_IDLE: break; case STATE_FOLLOW_SEGMENT: if (!FollowSegment(LINE_FOLLOW_FW)) { #if PL_APP_LINE_MAZE LF_currState = STATE_TURN; /* make turn */ #else LF_currState = STATE_STOP; /* stop if we do not have a line any more */ #endif } break; #if PL_APP_LINE_MAZE case STATE_FOLLOW_SEGMENT_BW: if (!FollowSegment(FALSE)) { TURN_Turn(TURN_STOP); if (EvaluateTurnBw()==ERR_OK) { LF_currState = STATE_FOLLOW_SEGMENT; } else { LF_currState = STATE_STOP; } } break; #endif #if PL_APP_LINE_MAZE case STATE_TURN: if (MAZE_IsSolved()) { TURN_Kind turn; turn = MAZE_GetSolvedTurn(&LF_solvedIdx); if (turn==TURN_STOP) { /* last turn reached */ TURN_Turn(turn); LF_currState = STATE_FINISHED; } else { /* perform turning */ TURN_Turn(TURN_STEP_LINE_FW); /* Step over line */ TURN_Turn(TURN_STEP_POST_LINE_FW); /* step before doing the turn */ TURN_Turn(turn); LF_currState = STATE_FOLLOW_SEGMENT; } } else { /* still evaluating maze */ bool deadEndGoBw = FALSE; bool finished = FALSE; if (EvaluteTurn(&finished, &deadEndGoBw)==ERR_OK) { /* finished turning */ if (finished) { LF_currState = STATE_FINISHED; MAZE_SetSolved(); #if PL_TURN_ON_FINISH /* turn the robot */ TURN_Turn(TURN_LEFT180); #endif TURN_Turn(TURN_STOP); /* now ready to do line following */ } else if (deadEndGoBw) { LF_currState = STATE_FOLLOW_SEGMENT_BW; } else { LF_currState = STATE_FOLLOW_SEGMENT; } } else { /* error case */ LF_currState = STATE_STOP; } } break; #endif #if PL_APP_LINE_MAZE case STATE_FINISHED: #if PL_HAS_BUZZER { uint8_t i; for(i=0;i<4;i++) { (void)BUZ_Beep(300, 100); WAIT1_WaitOSms(500); } } #endif LF_currState = STATE_STOP; break; #endif case STATE_STOP: TURN_Turn(TURN_STOP); LF_currState = STATE_IDLE; break; } /* switch */ }
uint8_t TURN_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) { uint8_t res = ERR_OK; const unsigned char *p; uint8_t val8u; uint16_t val16u; if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"turn help")==0) { TURN_PrintHelp(io); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"turn status")==0) { TURN_PrintStatus(io); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn left")==0) { TURN_Turn(TURN_LEFT90, FALSE); TURN_Turn(TURN_STOP, FALSE); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn right")==0) { TURN_Turn(TURN_RIGHT90, FALSE); TURN_Turn(TURN_STOP, FALSE); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn around")==0) { TURN_Turn(TURN_LEFT180, FALSE); TURN_Turn(TURN_STOP, FALSE); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn forward")==0) { REF_ClearHistory(); /* clear values */ TURN_Turn(TURN_STEP_FW, FALSE); TURN_Turn(TURN_STOP, FALSE); CLS1_SendStr((unsigned char*)REF_LineKindStr(REF_HistoryLineKind()), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn backward")==0) { TURN_Turn(TURN_STEP_BW, FALSE); TURN_Turn(TURN_STOP, FALSE); *handled = TRUE; } else if (UTIL1_strncmp((char*)cmd, (char*)"turn duty ", sizeof("turn duty ")-1)==0) { p = cmd+sizeof("turn duty"); if (UTIL1_ScanDecimal8uNumber(&p, &val8u)==ERR_OK && val8u<=100) { TURN_DutyPercent = val8u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"turn time ", sizeof("turn time ")-1)==0) { p = cmd+sizeof("turn time"); if (UTIL1_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) { TURN_TimeMs = val16u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"turn step ", sizeof("turn step ")-1)==0) { p = cmd+sizeof("turn step"); if (UTIL1_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) { TURN_StepMs = val16u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr); res = ERR_FAILED; } } return res; }
uint8_t TURN_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) { uint8_t res = ERR_OK; const unsigned char *p; #if !PL_HAS_QUADRATURE uint8_t val8u; #endif uint16_t val16u; if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"turn help")==0) { TURN_PrintHelp(io); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"turn status")==0) { TURN_PrintStatus(io); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn left 45")==0) { TURN_Turn(TURN_LEFT45); TURN_Turn(TURN_STOP); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn left 90")==0) { TURN_Turn(TURN_LEFT90); TURN_Turn(TURN_STOP); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn right 45")==0) { TURN_Turn(TURN_RIGHT45); TURN_Turn(TURN_STOP); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn right 90")==0) { TURN_Turn(TURN_RIGHT90); TURN_Turn(TURN_STOP); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn around")==0) { TURN_Turn(TURN_LEFT180); TURN_Turn(TURN_STOP); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn forward")==0) { #if PL_APP_LINE_MAZE REF_ClearHistory(); /* clear values */ #endif TURN_Turn(TURN_STEP_FW); TURN_Turn(TURN_STOP); #if PL_APP_LINE_MAZE CLS1_SendStr((unsigned char*)REF_LineKindStr(REF_HistoryLineKind()), CLS1_GetStdio()->stdOut); CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut); #endif *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)"turn backward")==0) { TURN_Turn(TURN_STEP_BW); TURN_Turn(TURN_STOP); *handled = TRUE; #if PL_HAS_QUADRATURE } else if (UTIL1_strncmp((char*)cmd, (char*)"turn steps90 ", sizeof("turn steps90 ")-1)==0) { p = cmd+sizeof("turn steps90"); if (UTIL1_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) { TURN_Steps90 = val16u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"turn stepsfw ", sizeof("turn stepsfw ")-1)==0) { p = cmd+sizeof("turn stepsfw"); if (UTIL1_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) { TURN_StepsFwBw = val16u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr); res = ERR_FAILED; } #else } else if (UTIL1_strncmp((char*)cmd, (char*)"turn duty ", sizeof("turn duty ")-1)==0) { p = cmd+sizeof("turn duty"); if (UTIL1_ScanDecimal8uNumber(&p, &val8u)==ERR_OK && val8u<=100) { TURN_DutyPercent = val8u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"turn time90 ", sizeof("turn time90 ")-1)==0) { p = cmd+sizeof("turn time90"); if (UTIL1_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) { TURN_Time90ms = val16u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"turn timefw ", sizeof("turn timefw ")-1)==0) { p = cmd+sizeof("turn timefw"); if (UTIL1_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) { TURN_StepFwBwMs = val16u; *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr); res = ERR_FAILED; } #endif } return res; }