|
|
/**
|
|
|
* \file
|
|
|
* \brief Robot turning interface.
|
|
|
* \author Erich Styger, erich.styger@hslu.ch
|
|
|
* \license SPDX-License-Identifier: BSD-3-Clause
|
|
|
* This module implements turning of the robot.
|
|
|
*/
|
|
|
|
|
|
#include "platform.h"
|
|
|
#if PL_HAS_TURN
|
|
|
#include "Turn.h"
|
|
|
#include "McuWait.h"
|
|
|
#include "Motor.h"
|
|
|
#include "McuUtility.h"
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
|
#include "McuShell.h"
|
|
|
#include "Shell.h"
|
|
|
#endif
|
|
|
#if PL_CONFIG_USE_LINE_SENSOR
|
|
|
#include "Reflectance.h"
|
|
|
#endif
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
#include "QuadCounter.h"
|
|
|
#include "Pid.h"
|
|
|
#endif
|
|
|
#if PL_CONFIG_USE_DRIVE
|
|
|
#include "Drive.h"
|
|
|
#endif
|
|
|
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
#if PL_IS_ROUND_ROBOT
|
|
|
#define TURN_STEPS_90 20
|
|
|
/*!< number of steps for a 90 degree turn */
|
|
|
#define TURN_STEPS_FW_BW 20
|
|
|
/*!< number of steps for one inch forward/backward */
|
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT
|
|
|
#define TURN_STEPS_90 34
|
|
|
/*!< number of steps for a 90 degree turn */
|
|
|
#define TURN_STEPS_LINE 12
|
|
|
/*!< number of steps stepping over the line */
|
|
|
#define TURN_STEPS_POST_LINE 7
|
|
|
/*!< number of steps after the line, before making a turn */
|
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT2
|
|
|
#define TURN_STEPS_90 36
|
|
|
/*!< number of steps for a 90 degree turn */
|
|
|
#define TURN_STEPS_LINE 11
|
|
|
/*!< number of steps stepping over the line */
|
|
|
#define TURN_STEPS_POST_LINE 7
|
|
|
/*!< number of steps after the line, before making a turn */
|
|
|
#define TURN_STEPS_90_TIMEOUT_MS 1000
|
|
|
#define TURN_STEPS_LINE_TIMEOUT_MS 200
|
|
|
#define TURN_STEPS_POST_LINE_TIMEOUT_MS 200
|
|
|
#define TURN_STEPS_STOP_TIMEOUT_MS 150
|
|
|
#elif PL_IS_INTRO_ZUMO_K22 && PL_HAS_LIPO
|
|
|
#define TURN_STEPS_90 600
|
|
|
/*!< number of steps for a 90 degree turn */
|
|
|
#define TURN_STEPS_LINE 230
|
|
|
/*!< number of steps stepping over the line */
|
|
|
#define TURN_STEPS_POST_LINE 160
|
|
|
/*!< number of steps after the line, before making a turn */
|
|
|
#define TURN_STEPS_90_TIMEOUT_MS 1000
|
|
|
#define TURN_STEPS_LINE_TIMEOUT_MS 500
|
|
|
#define TURN_STEPS_POST_LINE_TIMEOUT_MS 500
|
|
|
#define TURN_STEPS_STOP_TIMEOUT_MS 150
|
|
|
#elif PL_IS_INTRO_ZUMO_K22 /* no LiPo */
|
|
|
#define TURN_STEPS_90 650//720
|
|
|
/*!< number of steps for a 90 degree turn */
|
|
|
#define TURN_STEPS_LINE 170//100 /*230*/
|
|
|
/*!< number of steps stepping over the line */
|
|
|
#define TURN_STEPS_POST_LINE 240//150
|
|
|
/*!< number of steps after the line, before making a turn */
|
|
|
#define TURN_STEPS_90_TIMEOUT_MS 1000
|
|
|
#define TURN_STEPS_LINE_TIMEOUT_MS 200
|
|
|
#define TURN_STEPS_POST_LINE_TIMEOUT_MS 200
|
|
|
#define TURN_STEPS_STOP_TIMEOUT_MS 150
|
|
|
#else
|
|
|
#error "unknown configuration!"
|
|
|
#endif
|
|
|
#else /* no quadrature, use timed move */
|
|
|
#if PL_IS_ZUMO_ROBOT && PL_IS_MOTOR_1_100
|
|
|
#define TURN_90_WAIT_TIME_MS 680
|
|
|
/*!< ms to wait for a 90 degree turn */
|
|
|
#define TURN_STEP_LINE_MS 170
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_STEP_POST_LINE_MS 100
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_MOTOR_DUTY_PERCENT 100
|
|
|
/*!< maximum motor duty for turn operation */
|
|
|
#elif PL_IS_ZUMO_ROBOT
|
|
|
#define TURN_90_WAIT_TIME_MS 400
|
|
|
/*!< ms to wait for a 90 degree turn */
|
|
|
#define TURN_STEP_LINE_MS 90
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_STEP_POST_LINE_MS 60
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_MOTOR_DUTY_PERCENT 60
|
|
|
/*!< maximum motor duty for turn operation */
|
|
|
#elif PL_IS_ROUND_ROBOT
|
|
|
#define TURN_90_WAIT_TIME_MS 250
|
|
|
/*!< ms to wait for a 90 degree turn */
|
|
|
#define TURN_STEP_LINE_MS 50
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_STEP_POST_LINE_MS 50
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_MOTOR_DUTY_PERCENT 20
|
|
|
/*!< maximum motor duty for turn operation */
|
|
|
#elif PL_IS_TRACK_ROBOT
|
|
|
#define TURN_90_WAIT_TIME_MS 1050
|
|
|
/*!< ms to wait for a 90 degree turn */
|
|
|
#define TURN_STEP_LINE_MS 160
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_STEP_POST_LINE_MS 170
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_MOTOR_DUTY_PERCENT 25
|
|
|
/*!< maximum motor duty for turn operation */
|
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT
|
|
|
#define TURN_90_WAIT_TIME_MS 470
|
|
|
/*!< ms to wait for a 90 degree turn */
|
|
|
#define TURN_STEP_LINE_MS 80
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_STEP_POST_LINE_MS 90
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_MOTOR_DUTY_PERCENT 40
|
|
|
/*!< maximum motor duty for turn operation */
|
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22
|
|
|
#define TURN_90_WAIT_TIME_MS 300
|
|
|
/*!< ms to wait for a 90 degree turn */
|
|
|
#define TURN_STEP_LINE_MS 80
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_STEP_POST_LINE_MS 90
|
|
|
/*!< ms to do a step over the line */
|
|
|
#define TURN_MOTOR_DUTY_PERCENT 40
|
|
|
/*!< maximum motor duty for turn operation */
|
|
|
#else
|
|
|
#error "unknown configuration!"
|
|
|
#endif
|
|
|
#endif /* PL_CONFIG_USE_QUADRATURE */
|
|
|
|
|
|
#if PL_IS_ZUMO_ROBOT
|
|
|
#define TURN_WAIT_AFTER_STEP_MS 0 /* wait this time after a step to have the motor PWM effective */
|
|
|
#elif PL_IS_ROUND_ROBOT
|
|
|
#define TURN_WAIT_AFTER_STEP_MS 50 /* wait this time after a step to have the motor PWM effective */
|
|
|
#elif PL_IS_TRACK_ROBOT
|
|
|
#define TURN_WAIT_AFTER_STEP_MS 50 /* wait this time after a step to have the motor PWM effective */
|
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT || PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22
|
|
|
#define TURN_WAIT_AFTER_STEP_MS 0 /* >0: wait this time after a step to have the motor PWM effective */
|
|
|
#else
|
|
|
#error "unknown configuration!"
|
|
|
#endif
|
|
|
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
static int32_t TURN_Steps90 = TURN_STEPS_90;
|
|
|
static int32_t TURN_StepsLine = TURN_STEPS_LINE;
|
|
|
static int32_t TURN_StepsPostLine = TURN_STEPS_POST_LINE;
|
|
|
#else
|
|
|
static uint8_t TURN_DutyPercent = TURN_MOTOR_DUTY_PERCENT;
|
|
|
static uint16_t TURN_Time90ms = TURN_90_WAIT_TIME_MS;
|
|
|
static uint16_t TURN_StepLineMs = TURN_STEP_LINE_MS;
|
|
|
static uint16_t TURN_StepPostLineMs = TURN_STEP_POST_LINE_MS;
|
|
|
#endif
|
|
|
|
|
|
void TURN_SetSteps(int32_t steps90, int32_t stepsLine, int32_t stepsPostLine) {
|
|
|
TURN_StepsLine = stepsLine;
|
|
|
TURN_StepsPostLine = stepsPostLine;
|
|
|
TURN_Steps90 = steps90;
|
|
|
}
|
|
|
|
|
|
/*!
|
|
|
* \brief Translate a turn kind into a string
|
|
|
* \return Returns a descriptive string
|
|
|
*/
|
|
|
const unsigned char *TURN_TurnKindStr(TURN_Kind kind) {
|
|
|
switch(kind) {
|
|
|
case TURN_LEFT45: return (const unsigned char*)"LEFT45";
|
|
|
case TURN_LEFT90: return (const unsigned char*)"LEFT90";
|
|
|
case TURN_RIGHT45: return (const unsigned char*)"RIGHT45";
|
|
|
case TURN_RIGHT90: return (const unsigned char*)"RIGHT90";
|
|
|
case TURN_LEFT180: return (const unsigned char*)"LEFT180";
|
|
|
case TURN_RIGHT180: return (const unsigned char*)"RIGHT180";
|
|
|
case TURN_STRAIGHT: return (const unsigned char*)"STRAIGHT";
|
|
|
case TURN_STEP_LINE_FW: return (const unsigned char*)"STEP_LINE_FW";
|
|
|
case TURN_STEP_LINE_BW: return (const unsigned char*)"STEP_LINE_BW";
|
|
|
case TURN_STEP_POST_LINE_FW: return (const unsigned char*)"STEP_POST_LINE_FW";
|
|
|
case TURN_STEP_POST_LINE_BW: return (const unsigned char*)"STEP_POST_LINE_BW";
|
|
|
case TURN_STEP_LINE_FW_AND_POST_LINE: return (const unsigned char*)"STEP_LINE_FW_POST_LINE";
|
|
|
case TURN_STEP_LINE_BW_AND_POST_LINE: return (const unsigned char*)"STEP_LINE_BW_POST_LINE";
|
|
|
case TURN_STOP: return (const unsigned char*)"STOP";
|
|
|
case TURN_FINISH: return (const unsigned char*)"FINISH";
|
|
|
default: return (const unsigned char*)"TURN_UNKNOWN!";
|
|
|
}
|
|
|
}
|
|
|
|
|
|
#if !PL_CONFIG_USE_QUADRATURE
|
|
|
uint8_t TURN_SetDutyPercent(uint8_t duty) {
|
|
|
uint8_t old;
|
|
|
|
|
|
old = TURN_DutyPercent;
|
|
|
TURN_DutyPercent = duty;
|
|
|
return old;
|
|
|
}
|
|
|
|
|
|
#endif
|
|
|
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
void TURN_MoveToPos(int32_t targetLPos, int32_t targetRPos, bool wait, TURN_StopFct stopIt, int32_t timeoutMs) {
|
|
|
#if PL_CONFIG_USE_DRIVE
|
|
|
(void)DRV_SetPos(targetLPos, targetRPos);
|
|
|
(void)DRV_SetMode(DRV_MODE_POS);
|
|
|
#else
|
|
|
(void)targetLPos;
|
|
|
(void)targetRPos;
|
|
|
#warning "not possible to move to a position without drive enabled!"
|
|
|
#endif
|
|
|
for(;;) { /* breaks */
|
|
|
if (stopIt!=NULL) {
|
|
|
if (stopIt()) { /* check stop condition */
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
McuWait_WaitOSms(1); /* wait some time, and give Drive module enough time to use new value */
|
|
|
timeoutMs--;
|
|
|
if (timeoutMs<=0) {
|
|
|
break; /* timeout */
|
|
|
}
|
|
|
#if PL_CONFIG_USE_DRIVE
|
|
|
if (wait && DRV_HasTurned()) {
|
|
|
break;
|
|
|
}
|
|
|
#endif
|
|
|
if (!wait) {
|
|
|
break;
|
|
|
}
|
|
|
} /* for */
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
|
if (timeoutMs<=0) {
|
|
|
SHELL_SendString((unsigned char*)"MoveToPos Timeout.\r\n");
|
|
|
}
|
|
|
#endif
|
|
|
}
|
|
|
|
|
|
static void StepsTurn(int32_t stepsL, int32_t stepsR, TURN_StopFct stopIt, int32_t timeOutMS) {
|
|
|
int32_t currLPos, currRPos, targetLPos, targetRPos;
|
|
|
#if PL_CONFIG_USE_DRIVE
|
|
|
#if 0 /* stop before turn */
|
|
|
#if 1
|
|
|
int timeout = TURN_STEPS_STOP_TIMEOUT_MS;
|
|
|
|
|
|
DRV_SetMode(DRV_MODE_STOP); /* stop it */
|
|
|
McuWait_WaitOSms(5);
|
|
|
|
|
|
while (timeout>0 && !DRV_IsStopped()) {
|
|
|
/* wait until stopped */
|
|
|
timeout-=5;
|
|
|
McuWait_WaitOSms(5);
|
|
|
}
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
|
if (timeout<=0) {
|
|
|
SHELL_SendString((unsigned char*)"StepsTurn Stopping Timeout.\r\n");
|
|
|
}
|
|
|
#endif
|
|
|
#else
|
|
|
DRV_SetMode(DRV_MODE_STOP); /* stop it */
|
|
|
McuWait_WaitOSms(200); /* give it time to stop */
|
|
|
#endif
|
|
|
#endif
|
|
|
#endif
|
|
|
currLPos = QuadCounter_GetPosLeft();
|
|
|
currRPos = QuadCounter_GetPosRight();
|
|
|
targetLPos = currLPos+stepsL;
|
|
|
targetRPos = currRPos+stepsR;
|
|
|
TURN_MoveToPos(targetLPos, targetRPos, TRUE, stopIt, timeOutMS); /* go to final position */
|
|
|
}
|
|
|
#endif
|
|
|
|
|
|
static void PostTurn(void) {
|
|
|
#if !PL_CONFIG_USE_QUADRATURE /* with quadrature counter we already stopped the motors */
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
|
|
|
#endif
|
|
|
#if TURN_WAIT_AFTER_STEP_MS > 0
|
|
|
#if PL_CONFIG_USE_DRIVE
|
|
|
DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
|
|
|
#endif
|
|
|
McuWait_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
|
|
|
#endif /* TURN_WAIT_AFTER_STEP_MS */
|
|
|
}
|
|
|
|
|
|
#if !PL_CONFIG_USE_QUADRATURE
|
|
|
static void TimeTurn(bool isLeft, uint8_t duty, uint16_t val) {
|
|
|
if (isLeft) {
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -duty);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), duty);
|
|
|
} else {
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), duty);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -duty);
|
|
|
}
|
|
|
McuWait_WaitOSms(val); /* waiting time */
|
|
|
}
|
|
|
#endif
|
|
|
|
|
|
void TURN_Turn(TURN_Kind kind, TURN_StopFct stopIt) {
|
|
|
switch(kind) {
|
|
|
case TURN_LEFT45:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-TURN_Steps90/2, TURN_Steps90/2, stopIt, TURN_STEPS_90_TIMEOUT_MS/2);
|
|
|
#else
|
|
|
TimeTurn(TRUE, TURN_DutyPercent, TURN_Time90ms/2);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_RIGHT45:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(TURN_Steps90/2, -TURN_Steps90/2, stopIt, TURN_STEPS_90_TIMEOUT_MS/2);
|
|
|
#else
|
|
|
TimeTurn(FALSE, TURN_DutyPercent, TURN_Time90ms/2);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_LEFT90:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-TURN_Steps90, TURN_Steps90, stopIt, TURN_STEPS_90_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_Time90ms); /* only use waiting time */
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_RIGHT90:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(TURN_Steps90, -TURN_Steps90, stopIt, TURN_STEPS_90_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_Time90ms); /* only use waiting time */
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_LEFT180:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-(2*TURN_Steps90), 2*TURN_Steps90, stopIt, TURN_STEPS_90_TIMEOUT_MS*2);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(2*TURN_Time90ms);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_RIGHT180:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(2*TURN_Steps90, -(2*TURN_Steps90), stopIt, TURN_STEPS_90_TIMEOUT_MS*2);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(2*TURN_Time90ms);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_BORDER_BW:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-(3*TURN_StepsLine), -(3*TURN_StepsLine), stopIt, TURN_STEPS_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(3*TURN_StepLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_LINE_FW:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(TURN_StepsLine, TURN_StepsLine, stopIt, TURN_STEPS_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_LINE_BW:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-TURN_StepsLine, -TURN_StepsLine, stopIt, TURN_STEPS_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_POST_LINE_FW:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(TURN_StepsPostLine, TURN_StepsPostLine, stopIt, TURN_STEPS_POST_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepPostLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_POST_LINE_BW:
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-TURN_StepsPostLine, -TURN_StepsPostLine, stopIt, TURN_STEPS_POST_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepPostLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_LINE_FW_AND_POST_LINE: /* combination of TURN_STEP_LINE_FW and TURN_STEP_POST_LINE_FW */
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(TURN_StepsLine+TURN_StepsPostLine, TURN_StepsLine+TURN_StepsPostLine, stopIt, TURN_STEPS_LINE_TIMEOUT_MS+TURN_STEPS_POST_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepLineMs);
|
|
|
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepPostLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STEP_LINE_BW_AND_POST_LINE: /* combination of TURN_STEP_LINE_BW and TURN_STEP_POST_LINE_BW */
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-TURN_StepsLine-TURN_StepsPostLine, -TURN_StepsLine-TURN_StepsPostLine, stopIt, TURN_STEPS_LINE_TIMEOUT_MS+TURN_STEPS_POST_LINE_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepLineMs);
|
|
|
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(TURN_StepPostLineMs);
|
|
|
#endif
|
|
|
break;
|
|
|
case TURN_STOP_LEFT:
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
|
|
|
break;
|
|
|
case TURN_STOP_RIGHT:
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
|
|
|
break;
|
|
|
case TURN_STOP:
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
|
|
|
break;
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
/*lint -save -e522 Highest operation, function 'PostTurn', lacks side-effect. */
|
|
|
PostTurn(); /* perform post-turn action */
|
|
|
/*lint -restore Highest operation, function 'PostTurn', lacks side-effect. */
|
|
|
}
|
|
|
|
|
|
void TURN_TurnAngle(int16_t angle, TURN_StopFct stopIt) {
|
|
|
bool isLeft = angle<0;
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
int32_t steps;
|
|
|
#else
|
|
|
uint32_t time;
|
|
|
#endif
|
|
|
|
|
|
if (isLeft) {
|
|
|
angle = -angle; /* make it positive */
|
|
|
}
|
|
|
angle %= 360; /* keep it inside 360<EFBFBD> */
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
steps = (angle*TURN_Steps90)/90;
|
|
|
#else
|
|
|
time = (angle*TURN_Time90ms)/90;
|
|
|
#endif
|
|
|
if (isLeft) {
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(-steps, steps, stopIt, ((angle/90)+1)*TURN_STEPS_90_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(time);
|
|
|
#endif
|
|
|
} else { /* right */
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
StepsTurn(steps, -steps, stopIt, ((angle/90)+1)*TURN_STEPS_90_TIMEOUT_MS);
|
|
|
#else
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
|
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
|
|
|
McuWait_WaitOSms(time);
|
|
|
#endif
|
|
|
}
|
|
|
/*lint -save -e522 Highest operation, function 'PostTurn', lacks side-effect. */
|
|
|
PostTurn(); /* perform post-turn action */
|
|
|
/*lint -restore Highest operation, function 'PostTurn', lacks side-effect. */
|
|
|
}
|
|
|
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
|
static void TURN_PrintHelp(const McuShell_StdIOType *io) {
|
|
|
McuShell_SendHelpStr((unsigned char*)"turn", (unsigned char*)"Group of turning commands\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows turn help or status\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" <angle>", (unsigned char*)"Turn the robot by angle, negative is counter-clockwise, e.g. 'turn -90'\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" forward", (unsigned char*)"Move one step forward\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" forward postline", (unsigned char*)"Move one step forward post the line\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" backward", (unsigned char*)"Move one step backward\r\n", io->stdOut);
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
McuShell_SendHelpStr((unsigned char*)" steps90 <steps>", (unsigned char*)"Number of steps for a 90 degree turn\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" stepsline <steps>", (unsigned char*)"Number of steps for stepping over line\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" stepspostline <steps>", (unsigned char*)"Number of steps for a step post the line\r\n", io->stdOut);
|
|
|
#else
|
|
|
McuShell_SendHelpStr((unsigned char*)" duty <percent>", (unsigned char*)"Turning motor PWM duty percent\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" time90 <ms>", (unsigned char*)"Time in milli-seconds for 90 degree\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" timeline <ms>", (unsigned char*)"Time in milli-seconds for a single line step\r\n", io->stdOut);
|
|
|
McuShell_SendHelpStr((unsigned char*)" timepostline <ms>", (unsigned char*)"Time in milli-seconds for a step post the line\r\n", io->stdOut);
|
|
|
#endif
|
|
|
}
|
|
|
|
|
|
static void TURN_PrintStatus(const McuShell_StdIOType *io) {
|
|
|
unsigned char buf[32];
|
|
|
|
|
|
McuShell_SendStatusStr((unsigned char*)"turn", (unsigned char*)"Turning status\r\n", io->stdOut);
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), TURN_Steps90);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" 90 degree", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), TURN_StepsLine);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" line", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), TURN_StepsPostLine);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" postline", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), QuadCounter_GetPosLeft());
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", ");
|
|
|
McuUtility_strcatNum16u(buf, sizeof(buf), QuadCounter_GetErrorsLeft());
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" left pos", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), QuadCounter_GetPosRight());
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", ");
|
|
|
McuUtility_strcatNum16u(buf, sizeof(buf), QuadCounter_GetErrorsRight());
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" right pos", buf, io->stdOut);
|
|
|
#else
|
|
|
McuUtility_Num16uToStr(buf, sizeof(buf), TURN_DutyPercent);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" duty", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num16uToStr(buf, sizeof(buf), TURN_Time90ms);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" time90", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num16uToStr(buf, sizeof(buf), TURN_StepLineMs);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" line", buf, io->stdOut);
|
|
|
|
|
|
McuUtility_Num16uToStr(buf, sizeof(buf), TURN_StepPostLineMs);
|
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
|
|
|
McuShell_SendStatusStr((unsigned char*)" postline", buf, io->stdOut);
|
|
|
#endif
|
|
|
}
|
|
|
|
|
|
static bool isNumberStart(uint8_t ch) {
|
|
|
if (ch=='-') { /* negative number start */
|
|
|
return TRUE;
|
|
|
} else if (ch>='0' && ch<='9') {
|
|
|
return TRUE;
|
|
|
}
|
|
|
return FALSE;
|
|
|
}
|
|
|
|
|
|
uint8_t TURN_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
|
|
|
uint8_t res = ERR_OK;
|
|
|
const unsigned char *p;
|
|
|
#if !PL_CONFIG_USE_QUADRATURE
|
|
|
uint8_t val8u;
|
|
|
#endif
|
|
|
uint16_t val16u;
|
|
|
|
|
|
if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"turn help")==0) {
|
|
|
TURN_PrintHelp(io);
|
|
|
*handled = TRUE;
|
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"turn status")==0) {
|
|
|
TURN_PrintStatus(io);
|
|
|
*handled = TRUE;
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn ", sizeof("turn ")-1)==0 && isNumberStart(cmd[sizeof("turn ")-1])) {
|
|
|
int32_t angle;
|
|
|
|
|
|
p = cmd+sizeof("turn ")-1;
|
|
|
if (McuUtility_xatoi(&p, &angle)==ERR_OK) {
|
|
|
TURN_TurnAngle((int16_t)angle, NULL);
|
|
|
TURN_Turn(TURN_STOP, NULL);
|
|
|
DRV_SetSpeed(100, 100);
|
|
|
DRV_SetMode(DRV_MODE_SPEED);
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"turn forward postline")==0) {
|
|
|
TURN_Turn(TURN_STEP_LINE_FW_AND_POST_LINE, NULL);
|
|
|
TURN_Turn(TURN_STOP, NULL);
|
|
|
*handled = TRUE;
|
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"turn forward")==0) {
|
|
|
TURN_Turn(TURN_STEP_LINE_FW, NULL);
|
|
|
TURN_Turn(TURN_STOP, NULL);
|
|
|
*handled = TRUE;
|
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"turn backward")==0) {
|
|
|
TURN_Turn(TURN_STEP_LINE_BW, NULL);
|
|
|
TURN_Turn(TURN_STOP, NULL);
|
|
|
*handled = TRUE;
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn steps90 ", sizeof("turn steps90 ")-1)==0) {
|
|
|
p = cmd+sizeof("turn steps90");
|
|
|
if (McuUtility_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) {
|
|
|
TURN_Steps90 = val16u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn stepspostline ", sizeof("turn stepspostline ")-1)==0) {
|
|
|
p = cmd+sizeof("turn stepspostline");
|
|
|
if (McuUtility_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) {
|
|
|
TURN_StepsPostLine = val16u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn stepsline ", sizeof("turn stepsline ")-1)==0) {
|
|
|
p = cmd+sizeof("turn stepsline");
|
|
|
if (McuUtility_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) {
|
|
|
TURN_StepsLine = val16u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
#else
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn duty ", sizeof("turn duty ")-1)==0) {
|
|
|
p = cmd+sizeof("turn duty");
|
|
|
if (McuUtility_ScanDecimal8uNumber(&p, &val8u)==ERR_OK && val8u<=100) {
|
|
|
TURN_DutyPercent = val8u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument, must be in the range 0..100\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn time90 ", sizeof("turn time90 ")-1)==0) {
|
|
|
p = cmd+sizeof("turn time90");
|
|
|
if (McuUtility_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) {
|
|
|
TURN_Time90ms = val16u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn timeline ", sizeof("turn timeline ")-1)==0) {
|
|
|
p = cmd+sizeof("turn timeline");
|
|
|
if (McuUtility_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) {
|
|
|
TURN_StepLineMs = val16u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"turn timepostline ", sizeof("turn timepostline ")-1)==0) {
|
|
|
p = cmd+sizeof("turn timepostline");
|
|
|
if (McuUtility_ScanDecimal16uNumber(&p, &val16u)==ERR_OK) {
|
|
|
TURN_StepPostLineMs = val16u;
|
|
|
*handled = TRUE;
|
|
|
} else {
|
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
|
res = ERR_FAILED;
|
|
|
}
|
|
|
#endif
|
|
|
}
|
|
|
return res;
|
|
|
}
|
|
|
#endif /* PL_CONFIG_USE_SHELL */
|
|
|
|
|
|
void TURN_Init(void) {
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
|
TURN_Steps90 = TURN_STEPS_90;
|
|
|
TURN_StepsPostLine = TURN_STEPS_POST_LINE;
|
|
|
TURN_StepsLine = TURN_STEPS_LINE;
|
|
|
#else
|
|
|
TURN_DutyPercent = TURN_MOTOR_DUTY_PERCENT;
|
|
|
TURN_Time90ms = TURN_90_WAIT_TIME_MS;
|
|
|
TURN_StepLineMs = TURN_STEP_LINE_MS;
|
|
|
TURN_StepPostLineMs = TURN_STEP_POST_LINE_MS;
|
|
|
#endif
|
|
|
}
|
|
|
#endif /* PL_HAS_TURN */
|
|
|
|
