You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
555 lines
17 KiB
555 lines
17 KiB
/**
|
|
* \file
|
|
* \brief Module to drive the robot.
|
|
* \author Erich Styger, erich.styger@hslu.ch
|
|
* \license SPDX-License-Identifier: BSD-3-Clause
|
|
* This module allows to drive the robot and to perform turns.
|
|
*/
|
|
#include "platform.h"
|
|
#if PL_CONFIG_USE_DRIVE
|
|
#include "Drive.h"
|
|
#include "McuRTOS.h"
|
|
#include "FreeRTOS.h"
|
|
#include "McuUtility.h"
|
|
#if PL_CONFIG_USE_TACHO
|
|
#include "Tacho.h"
|
|
#endif
|
|
#include "Pid.h"
|
|
#include "Motor.h"
|
|
#if PL_CONFIG_USE_SHELL
|
|
#include "McuShell.h"
|
|
#endif
|
|
#if configUSE_TRACE_HOOKS
|
|
#include "McuPercepio.h"
|
|
#endif
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
#include "QuadCounter.h"
|
|
#endif
|
|
#include "Shell.h"
|
|
#include "McuWait.h"
|
|
|
|
#define PRINT_DRIVE_INFO 0 /* if we print debug info */
|
|
|
|
struct {
|
|
DRV_Mode mode;
|
|
struct {
|
|
int32_t left, right;
|
|
} speed;
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
struct {
|
|
int32_t left, right;
|
|
} pos;
|
|
#endif
|
|
} DRV_Status;
|
|
|
|
typedef enum {
|
|
DRV_SET_MODE,
|
|
DRV_SET_SPEED,
|
|
#if PL_CONFIG_USE_POS_PID
|
|
DRV_SET_POS,
|
|
#endif
|
|
} DRV_Commands;
|
|
|
|
typedef struct {
|
|
DRV_Commands cmd;
|
|
union {
|
|
DRV_Mode mode; /* DRV_SET_MODE */
|
|
struct {
|
|
int32_t left, right;
|
|
} speed; /* DRV_SET_SPEED */
|
|
#if PL_CONFIG_USE_POS_PID
|
|
struct {
|
|
int32_t left, right;
|
|
} pos; /* DRV_SET_POS */
|
|
#endif
|
|
} u;
|
|
} DRV_Command;
|
|
|
|
#define QUEUE_LENGTH 4 /* number of items in queue, that's my buffer size */
|
|
#define QUEUE_ITEM_SIZE sizeof(DRV_Command) /* each item is a single drive command */
|
|
static QueueHandle_t DRV_Queue;
|
|
|
|
bool DRV_IsStopped(void) {
|
|
#if PL_CONFIG_USE_TACHO
|
|
QuadCounter_QuadCntrType leftPos;
|
|
QuadCounter_QuadCntrType rightPos;
|
|
#endif
|
|
|
|
if (uxQueueMessagesWaiting(DRV_Queue)>0) {
|
|
return FALSE; /* still messages in command queue, so there is something pending */
|
|
}
|
|
#if PL_CONFIG_USE_TACHO
|
|
/* do *not* use/calculate speed: too slow! Use position encoder instead */
|
|
leftPos = QuadCounter_GetPosLeft();
|
|
rightPos = QuadCounter_GetPosRight();
|
|
if (DRV_Status.mode==DRV_MODE_POS) {
|
|
#if PL_CONFIG_HAS_HIGH_RES_ENCODER
|
|
if (DRV_Status.pos.left!=(int32_t)leftPos) {
|
|
return FALSE;
|
|
}
|
|
if (DRV_Status.pos.right!=(int32_t)rightPos) {
|
|
return FALSE;
|
|
}
|
|
#else
|
|
if (DRV_Status.pos.left!=(int32_t)leftPos) {
|
|
return FALSE;
|
|
}
|
|
if (DRV_Status.pos.right!=(int32_t)rightPos) {
|
|
return FALSE;
|
|
}
|
|
#endif
|
|
return TRUE;
|
|
} if (DRV_Status.mode==DRV_MODE_STOP) {
|
|
return TRUE;
|
|
} else {
|
|
/* ???? what to do otherwise ???? */
|
|
return FALSE;
|
|
}
|
|
#else
|
|
#warning "NYI"
|
|
return TRUE;
|
|
#endif
|
|
}
|
|
|
|
uint8_t DRV_Stop(int32_t timeoutMs) {
|
|
(void)DRV_SetMode(DRV_MODE_STOP); /* stop it */
|
|
do {
|
|
if (DRV_IsStopped()) {
|
|
break;
|
|
}
|
|
McuWait_WaitOSms(5);
|
|
timeoutMs -= 5;
|
|
} while (timeoutMs>0);
|
|
if (timeoutMs<0) {
|
|
return ERR_BUSY; /* timeout */
|
|
}
|
|
return ERR_OK;
|
|
}
|
|
|
|
bool DRV_IsDrivingBackward(void) {
|
|
return DRV_Status.mode==DRV_MODE_SPEED
|
|
&& DRV_Status.speed.left<0
|
|
&& DRV_Status.speed.right<0;
|
|
}
|
|
|
|
#if PL_CONFIG_USE_POS_PID
|
|
static bool match(int16_t pos, int16_t target) {
|
|
#if PL_CONFIG_HAS_HIGH_RES_ENCODER
|
|
#define MATCH_MARGIN 50
|
|
#else
|
|
#define MATCH_MARGIN 0
|
|
#endif
|
|
#if MATCH_MARGIN>0
|
|
return (pos>=target-MATCH_MARGIN && pos<=target+MATCH_MARGIN);
|
|
#else
|
|
return pos==target;
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
bool DRV_HasTurned(void) {
|
|
#if PL_CONFIG_USE_POS_PID
|
|
int16_t pos;
|
|
|
|
if (uxQueueMessagesWaiting(DRV_Queue)>0) {
|
|
return FALSE; /* still messages in command queue, so there is something pending */
|
|
}
|
|
if (DRV_Status.mode==DRV_MODE_POS) {
|
|
#if PL_CONFIG_USE_TACHO
|
|
#if PL_CONFIG_HAS_HIGH_RES_ENCODER
|
|
#define DRV_TURN_SPEED_LOW 50
|
|
#else
|
|
#define DRV_TURN_SPEED_LOW 5
|
|
#endif
|
|
int32_t speedL, speedR;
|
|
|
|
speedL = TACHO_GetSpeed(TRUE);
|
|
speedR = TACHO_GetSpeed(FALSE);
|
|
if (speedL>-DRV_TURN_SPEED_LOW && speedL<DRV_TURN_SPEED_LOW && speedR>-DRV_TURN_SPEED_LOW && speedR<DRV_TURN_SPEED_LOW) { /* speed close to zero */
|
|
#endif
|
|
pos = QuadCounter_GetPosLeft();
|
|
if (match(pos, DRV_Status.pos.left)) {
|
|
pos = QuadCounter_GetPosRight();
|
|
if (match(pos, DRV_Status.pos.right)) {
|
|
return TRUE;
|
|
}
|
|
}
|
|
#if PL_CONFIG_USE_TACHO
|
|
}
|
|
#endif
|
|
return FALSE;
|
|
} /* if */
|
|
return TRUE;
|
|
#else
|
|
return TRUE;
|
|
#endif
|
|
}
|
|
|
|
DRV_Mode DRV_GetMode(void) {
|
|
return DRV_Status.mode;
|
|
}
|
|
|
|
uint8_t DRV_SetMode(DRV_Mode mode) {
|
|
DRV_Command cmd;
|
|
|
|
#if PL_CONFIG_USE_DRIVE_STOP_POS
|
|
if (mode==DRV_MODE_STOP) {
|
|
(void)DRV_SetPos(QuadCounter_GetPosLeft(), QuadCounter_GetPosRight()); /* set current position */
|
|
PID_Start(); /* reset PID, especially integral counters */
|
|
mode = DRV_MODE_POS;
|
|
}
|
|
#endif
|
|
cmd.cmd = DRV_SET_MODE;
|
|
cmd.u.mode = mode;
|
|
if (xQueueSendToBack(DRV_Queue, &cmd, portMAX_DELAY)!=pdPASS) {
|
|
return ERR_FAILED;
|
|
}
|
|
taskYIELD(); /* yield so drive task has a chance to read message */
|
|
return ERR_OK;
|
|
}
|
|
|
|
uint8_t DRV_SetSpeed(int32_t left, int32_t right) {
|
|
DRV_Command cmd;
|
|
|
|
cmd.cmd = DRV_SET_SPEED;
|
|
cmd.u.speed.left = left;
|
|
cmd.u.speed.right = right;
|
|
if (xQueueSendToBack(DRV_Queue, &cmd, portMAX_DELAY)!=pdPASS) {
|
|
return ERR_FAILED;
|
|
}
|
|
taskYIELD(); /* yield so drive task has a chance to read message */
|
|
return ERR_OK;
|
|
}
|
|
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
uint8_t DRV_SetPos(int32_t left, int32_t right) {
|
|
DRV_Command cmd;
|
|
|
|
cmd.cmd = DRV_SET_POS;
|
|
cmd.u.pos.left = left;
|
|
cmd.u.pos.right = right;
|
|
if (xQueueSendToBack(DRV_Queue, &cmd, portMAX_DELAY)!=pdPASS) {
|
|
return ERR_FAILED;
|
|
}
|
|
taskYIELD(); /* yield so drive task has a chance to read message */
|
|
return ERR_OK;
|
|
}
|
|
#endif
|
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
static uint8_t *DRV_GetModeStr(DRV_Mode mode) {
|
|
switch(mode) {
|
|
case DRV_MODE_NONE: return (uint8_t*)"NONE";
|
|
case DRV_MODE_STOP: return (uint8_t*)"STOP";
|
|
case DRV_MODE_SPEED: return (uint8_t*)"SPEED";
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
case DRV_MODE_POS: return (uint8_t*)"POS";
|
|
#endif
|
|
default: return (uint8_t*)"UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static void DRV_PrintHelp(const McuShell_StdIOType *io) {
|
|
McuShell_SendHelpStr((unsigned char*)"drive", (unsigned char*)"Group of drive commands\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows drive help or status\r\n", io->stdOut);
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
McuShell_SendHelpStr((unsigned char*)" mode <mode>", (unsigned char*)"Set driving mode (none|stop|speed|pos)\r\n", io->stdOut);
|
|
#else
|
|
McuShell_SendHelpStr((unsigned char*)" mode <mode>", (unsigned char*)"Set driving mode (none|stop|speed)\r\n", io->stdOut);
|
|
#endif
|
|
McuShell_SendHelpStr((unsigned char*)" speed <left> <right>", (unsigned char*)"Move left and right motors with given speed\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" incSpeed <left> <right>", (unsigned char*)"Increase left and right motors with given speed\r\n", io->stdOut);
|
|
#if PL_CONFIG_USE_POS_PID
|
|
McuShell_SendHelpStr((unsigned char*)" pos <left> <right>", (unsigned char*)"Move left and right wheels to given position\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" pos reset", (unsigned char*)"Reset drive and wheel position\r\n", io->stdOut);
|
|
#endif
|
|
}
|
|
|
|
static void DRV_PrintStatus(const McuShell_StdIOType *io) {
|
|
uint8_t buf[24];
|
|
|
|
McuShell_SendStatusStr((unsigned char*)"drive", (unsigned char*)"Drive status\r\n", io->stdOut);
|
|
|
|
McuShell_SendStatusStr((unsigned char*)" mode", DRV_GetModeStr(DRV_Status.mode), io->stdOut);
|
|
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.left);
|
|
#if PL_CONFIG_USE_TACHO
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec\r\n");
|
|
#else
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" % PWM\r\n");
|
|
#endif
|
|
McuShell_SendStatusStr((unsigned char*)" speed left", buf, io->stdOut);
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.right);
|
|
#if PL_CONFIG_USE_TACHO
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec\r\n");
|
|
#else
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" % PWM\r\n");
|
|
#endif
|
|
McuShell_SendStatusStr((unsigned char*)" speed right", buf, io->stdOut);
|
|
|
|
#if PL_CONFIG_USE_POS_PID
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.left);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" (curr: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), (int32_t)QuadCounter_GetPosLeft());
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
|
|
McuShell_SendStatusStr((unsigned char*)" pos left", buf, io->stdOut);
|
|
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.right);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" (curr: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), (int32_t)QuadCounter_GetPosRight());
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
|
|
McuShell_SendStatusStr((unsigned char*)" pos right", buf, io->stdOut);
|
|
#endif
|
|
}
|
|
|
|
uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
|
|
uint8_t res = ERR_OK;
|
|
const unsigned char *p;
|
|
int32_t val1, val2;
|
|
|
|
if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"drive help")==0) {
|
|
DRV_PrintHelp(io);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"drive status")==0) {
|
|
DRV_PrintStatus(io);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"drive speed ", sizeof("drive speed ")-1)==0) {
|
|
p = cmd+sizeof("drive speed");
|
|
if (McuUtility_xatoi(&p, &val1)==ERR_OK) {
|
|
if (McuUtility_xatoi(&p, &val2)==ERR_OK) {
|
|
if (DRV_SetSpeed(val1, val2)!=ERR_OK) {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
}else if (McuUtility_strncmp((char*)cmd, (char*)"drive incSpeed ", sizeof("drive incSpeed ")-1)==0) {
|
|
p = cmd+sizeof("drive incSpeed");
|
|
if (McuUtility_xatoi(&p, &val1)==ERR_OK) {
|
|
if (McuUtility_xatoi(&p, &val2)==ERR_OK) {
|
|
int32_t leftSpeed = DRV_Status.speed.left+val1;
|
|
int32_t rightSpeed = DRV_Status.speed.right+val2;
|
|
if (DRV_SetSpeed(leftSpeed, rightSpeed)!=ERR_OK) {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
#if PL_CONFIG_USE_POS_PID
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) {
|
|
QuadCounter_SetPosLeft(0);
|
|
QuadCounter_SetPosRight(0);
|
|
if (DRV_SetPos(0, 0)!=ERR_OK) {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"drive pos ", sizeof("drive pos ")-1)==0) {
|
|
p = cmd+sizeof("drive pos");
|
|
if (McuUtility_xatoi(&p, &val1)==ERR_OK) {
|
|
if (McuUtility_xatoi(&p, &val2)==ERR_OK) {
|
|
if (DRV_SetPos(val1, val2)!=ERR_OK) {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
#endif
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"drive mode ", sizeof("drive mode ")-1)==0) {
|
|
p = cmd+sizeof("drive mode");
|
|
if (McuUtility_strcmp((char*)p, (char*)"none")==0) {
|
|
if (DRV_SetMode(DRV_MODE_NONE)!=ERR_OK) {
|
|
res = ERR_FAILED;
|
|
}
|
|
} else if (McuUtility_strcmp((char*)p, (char*)"stop")==0) {
|
|
if (DRV_SetMode(DRV_MODE_STOP)!=ERR_OK) {
|
|
res = ERR_FAILED;
|
|
}
|
|
} else if (McuUtility_strcmp((char*)p, (char*)"speed")==0) {
|
|
if (DRV_SetMode(DRV_MODE_SPEED)!=ERR_OK) {
|
|
res = ERR_FAILED;
|
|
}
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
} else if (McuUtility_strcmp((char*)p, (char*)"pos")==0) {
|
|
if (DRV_SetMode(DRV_MODE_POS)!=ERR_OK) {
|
|
res = ERR_FAILED;
|
|
}
|
|
#endif
|
|
} else {
|
|
res = ERR_FAILED;
|
|
}
|
|
if (res!=ERR_OK) {
|
|
McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
|
|
}
|
|
*handled = TRUE;
|
|
}
|
|
return res;
|
|
}
|
|
#endif /* PL_CONFIG_USE_SHELL */
|
|
|
|
static uint8_t GetCmd(void) {
|
|
DRV_Command cmd;
|
|
portBASE_TYPE res;
|
|
|
|
res = xQueueReceive(DRV_Queue, &cmd, 0);
|
|
if (res==errQUEUE_EMPTY) {
|
|
return ERR_RXEMPTY; /* no command */
|
|
}
|
|
/* process command */
|
|
taskENTER_CRITICAL();
|
|
if (cmd.cmd==DRV_SET_MODE) {
|
|
#if PL_CONFIG_USE_PID
|
|
PID_Start(); /* reset PID, especially integral counters */
|
|
#endif
|
|
DRV_Status.mode = cmd.u.mode;
|
|
} else if (cmd.cmd==DRV_SET_SPEED) {
|
|
DRV_Status.speed.left = cmd.u.speed.left;
|
|
DRV_Status.speed.right = cmd.u.speed.right;
|
|
#if PL_CONFIG_USE_POS_PID
|
|
} else if (cmd.cmd==DRV_SET_POS) {
|
|
DRV_Status.pos.left = cmd.u.pos.left;
|
|
DRV_Status.pos.right = cmd.u.pos.right;
|
|
#endif
|
|
}
|
|
taskEXIT_CRITICAL();
|
|
#if PRINT_DRIVE_INFO
|
|
{
|
|
uint8_t buf[32];
|
|
|
|
if (cmd.cmd==DRV_SET_MODE) {
|
|
McuUtility_strcpy(buf, sizeof(buf), "SETMODE: ");
|
|
McuUtility_strcat(buf, sizeof(buf), DRV_GetModeStr(DRV_Status.mode));
|
|
} else if (cmd.cmd==DRV_SET_SPEED) {
|
|
McuUtility_strcpy(buf, sizeof(buf), "SETSPEED: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), DRV_Status.speed.left);
|
|
McuUtility_strcat(buf, sizeof(buf), ", ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), DRV_Status.speed.right);
|
|
} else if (cmd.cmd==DRV_SET_POS) {
|
|
McuUtility_strcpy(buf, sizeof(buf), "SETPOS: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), DRV_Status.pos.left);
|
|
McuUtility_strcat(buf, sizeof(buf), ", ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), DRV_Status.pos.right);
|
|
} else {
|
|
McuUtility_strcpy(buf, sizeof(buf), "ERROR!");
|
|
}
|
|
McuUtility_strcat(buf, sizeof(buf), "\r\n");
|
|
SHELL_SendString(buf);
|
|
}
|
|
#endif
|
|
return ERR_OK;
|
|
}
|
|
|
|
static void DriveTask(void *pvParameters) {
|
|
TickType_t xLastWakeTime;
|
|
#if !PL_CONFIG_USE_TACHO
|
|
DRV_Mode prevMode;
|
|
#endif
|
|
|
|
(void)pvParameters;
|
|
xLastWakeTime = xTaskGetTickCount();
|
|
#if !PL_CONFIG_USE_TACHO
|
|
prevMode = DRV_MODE_NONE;
|
|
#endif
|
|
for(;;) {
|
|
while (GetCmd()==ERR_OK) { /* returns ERR_RXEMPTY if queue is empty */
|
|
/* process incoming commands */
|
|
}
|
|
#if PL_CONFIG_USE_TACHO
|
|
TACHO_CalcSpeed();
|
|
#endif
|
|
if (DRV_Status.mode==DRV_MODE_SPEED) {
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
PID_Speed(TACHO_GetSpeed(TRUE), DRV_Status.speed.left, TRUE);
|
|
PID_Speed(TACHO_GetSpeed(FALSE), DRV_Status.speed.right, FALSE);
|
|
#else
|
|
{
|
|
MOT_SpeedPercent speedL, speedR;
|
|
|
|
if (DRV_Status.speed.left<-100) {
|
|
speedL = -60; /* limit to avoid battery drop! */
|
|
} else if (DRV_Status.speed.left>100) {
|
|
speedL = 60; /* limit to avoid battery drop! */
|
|
} else {
|
|
speedL = DRV_Status.speed.left;
|
|
}
|
|
if (DRV_Status.speed.right<-100) {
|
|
speedR = -60;
|
|
} else if (DRV_Status.speed.right>100) {
|
|
speedR = 60;
|
|
} else {
|
|
speedR = DRV_Status.speed.right;
|
|
}
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
|
|
}
|
|
#endif
|
|
} else if (DRV_Status.mode==DRV_MODE_STOP) {
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
PID_Speed(TACHO_GetSpeed(TRUE), 0, TRUE);
|
|
PID_Speed(TACHO_GetSpeed(FALSE), 0, FALSE);
|
|
#elif !PL_CONFIG_USE_TACHO
|
|
if (prevMode!=DRV_MODE_STOP) { /* stop motors */
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
|
|
}
|
|
#endif
|
|
#if PL_CONFIG_USE_POS_PID
|
|
} else if (DRV_Status.mode==DRV_MODE_POS) {
|
|
PID_Pos(QuadCounter_GetPosLeft(), DRV_Status.pos.left, TRUE);
|
|
PID_Pos(QuadCounter_GetPosRight(), DRV_Status.pos.right, FALSE);
|
|
#endif
|
|
} else if (DRV_Status.mode==DRV_MODE_NONE) {
|
|
/* do nothing */
|
|
}
|
|
#if !PL_CONFIG_USE_TACHO
|
|
prevMode = DRV_Status.mode;
|
|
#endif
|
|
vTaskDelayUntil(&xLastWakeTime, 5/portTICK_PERIOD_MS);
|
|
} /* for */
|
|
}
|
|
|
|
void DRV_Deinit(void) {
|
|
vQueueDelete(DRV_Queue);
|
|
}
|
|
|
|
void DRV_Init(void) {
|
|
#if PL_CONFIG_USE_REMOTE
|
|
DRV_Status.mode = DRV_MODE_SPEED;
|
|
#else
|
|
DRV_Status.mode = DRV_MODE_NONE;
|
|
#endif
|
|
DRV_Status.speed.left = 0;
|
|
DRV_Status.speed.right = 0;
|
|
#if PL_CONFIG_USE_QUADRATURE
|
|
DRV_Status.pos.left = 0;
|
|
DRV_Status.pos.right = 0;
|
|
#endif
|
|
DRV_Queue = xQueueCreate(QUEUE_LENGTH, QUEUE_ITEM_SIZE);
|
|
if (DRV_Queue==NULL) {
|
|
for(;;){} /* out of memory? */
|
|
}
|
|
vQueueAddToRegistry(DRV_Queue, "Drive");
|
|
#if configUSE_TRACE_HOOKS
|
|
PTRC1_vTraceSetQueueName(DRV_Queue, "Drive");
|
|
#endif
|
|
if (xTaskCreate(DriveTask, "Drive", 500/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+3, NULL) != pdPASS) {
|
|
for(;;){} /* error */
|
|
}
|
|
}
|
|
#endif /* PL_CONFIG_USE_DRIVE */
|
|
|