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;
}
