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.
730 lines
25 KiB
730 lines
25 KiB
/**
|
|
* \file
|
|
* \brief This is the implementation of the PID Module
|
|
* \author Erich Styger, erich.styger@hslu.ch
|
|
* \license SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#if PL_CONFIG_USE_PID
|
|
#include "Pid.h"
|
|
#include "Motor.h"
|
|
#include "McuUtility.h"
|
|
#if PL_CONFIG_USE_LINE_SENSOR
|
|
#include "Reflectance.h"
|
|
#endif
|
|
|
|
#define PID_DEBUG 0 /* careful: this will slow down the PID loop frequency! */
|
|
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
static PID_Config lineFwConfig;
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
static PID_Config lineBwConfig;
|
|
#endif
|
|
#if PL_CONFIG_USE_POS_PID
|
|
static PID_Config posLeftConfig, posRightConfig;
|
|
#endif
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
static PID_Config speedLeftConfig, speedRightConfig;
|
|
#endif
|
|
|
|
uint8_t PID_GetPIDConfig(PID_ConfigType config, PID_Config **confP) {
|
|
switch(config) {
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
case PID_CONFIG_LINE_FW:
|
|
*confP = &lineFwConfig; break;
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
case PID_CONFIG_LINE_BW:
|
|
*confP = &lineBwConfig; break;
|
|
#endif
|
|
case PID_CONFIG_POS_LEFT:
|
|
*confP = &posLeftConfig; break;
|
|
case PID_CONFIG_POS_RIGHT:
|
|
*confP = &posRightConfig; break;
|
|
case PID_CONFIG_SPEED_LEFT:
|
|
*confP = &speedLeftConfig; break;
|
|
case PID_CONFIG_SPEED_RIGHT:
|
|
*confP = &speedRightConfig; break;
|
|
default:
|
|
*confP = NULL;
|
|
return ERR_FAILED;
|
|
}
|
|
return ERR_OK;
|
|
}
|
|
|
|
#if PL_CONFIG_USE_SPEED_PID || PL_CONFIG_USE_POS_PID || PL_CONFIG_USE_LINE_PID
|
|
static int32_t PID(int32_t currVal, int32_t setVal, PID_Config *config) {
|
|
int32_t error;
|
|
int32_t pid;
|
|
|
|
/* perform PID closed control loop calculation */
|
|
error = setVal-currVal; /* calculate error */
|
|
pid = (error*config->pFactor100)/100; /* P part */
|
|
config->integral += error; /* integrate error */
|
|
if (config->integral>config->iAntiWindup) {
|
|
config->integral = config->iAntiWindup;
|
|
} else if (config->integral<-config->iAntiWindup) {
|
|
config->integral = -config->iAntiWindup;
|
|
}
|
|
#if 1 /* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-reset-windup/ */
|
|
{
|
|
int32_t max;
|
|
|
|
max = 0xffff; /* max value of PWM */
|
|
if (config->integral > max) {
|
|
config->integral = max;
|
|
} else if (config->integral < -max) {
|
|
config->integral = -max;
|
|
}
|
|
}
|
|
#endif
|
|
pid += (config->integral*config->iFactor100)/100; /* add I part */
|
|
pid += ((error-config->lastError)*config->dFactor100)/100; /* add D part */
|
|
config->lastError = error; /* remember for next iteration of D part */
|
|
return pid;
|
|
}
|
|
#endif
|
|
|
|
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
|
|
static int32_t Limit(int32_t val, int32_t minVal, int32_t maxVal) {
|
|
if (val<minVal) {
|
|
return minVal;
|
|
} else if (val>maxVal) {
|
|
return maxVal;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
static MOT_Direction AbsSpeed(int32_t *speedP) {
|
|
if (*speedP<0) {
|
|
*speedP = -(*speedP);
|
|
return MOT_DIR_BACKWARD;
|
|
}
|
|
return MOT_DIR_FORWARD;
|
|
}
|
|
|
|
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
|
|
/*! \brief returns error (always positive) percent */
|
|
static uint8_t errorWithinPercent(int32_t error) {
|
|
if (error<0) {
|
|
error = -error;
|
|
}
|
|
error = error/(REF_MAX_LINE_VALUE/2/100);
|
|
if (error>100) {
|
|
error = 100;
|
|
}
|
|
return error;
|
|
}
|
|
#endif
|
|
|
|
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
|
|
static void PID_LineCfg(uint16_t currLine, uint16_t setLine, uint16_t currLineWidth, bool forward, PID_Config *config) {
|
|
int32_t pid, speed, speedL, speedR;
|
|
#if PID_DEBUG
|
|
unsigned char buf[16];
|
|
static uint8_t cnt = 0;
|
|
#endif
|
|
uint8_t errorPercent;
|
|
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
|
|
|
|
(void)currLineWidth;
|
|
|
|
pid = PID(currLine, setLine, config);
|
|
errorPercent = errorWithinPercent(currLine-setLine);
|
|
|
|
/* transform into different speed for motors. The PID is used as difference value to the motor PWM */
|
|
if (!forward) { /* going backward */
|
|
/* need to do it slow as sensor is on the 'back', and we cannot usual turns to get the sensor back 'on line' */
|
|
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
|
|
pid = Limit(pid, -speed, speed);
|
|
if (pid<0) { /* turn right */
|
|
speedR = speed;
|
|
speedL = speed-pid;
|
|
} else { /* turn left */
|
|
speedR = speed+pid;
|
|
speedL = speed;
|
|
}
|
|
#if 0 /* simple differential steering */
|
|
} else {
|
|
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
|
|
pid = Limit(pid, -speed, speed);
|
|
if (pid<0) { /* turn right */
|
|
speedR = speed;
|
|
speedL = speed-pid;
|
|
} else { /* turn left */
|
|
speedR = speed+pid;
|
|
speedL = speed;
|
|
}
|
|
}
|
|
#else
|
|
} else if (errorPercent <= 10) { /* pretty on center: move forward both motors with base speed */
|
|
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
|
|
pid = Limit(pid, -speed, speed);
|
|
if (pid<0) { /* turn right */
|
|
speedR = speed;
|
|
speedL = speed-pid;
|
|
} else { /* turn left */
|
|
speedR = speed+pid;
|
|
speedL = speed;
|
|
}
|
|
} else if (errorPercent <= 30) {
|
|
/* outside left/right halve position from center, slow down one motor and speed up the other */
|
|
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* 80% */
|
|
pid = Limit(pid, -speed, speed);
|
|
if (pid<0) { /* turn right */
|
|
speedR = speed+pid; /* decrease speed */
|
|
speedL = speed-pid; /* increase speed */
|
|
} else { /* turn left */
|
|
speedR = speed+pid; /* increase speed */
|
|
speedL = speed-pid; /* decrease speed */
|
|
}
|
|
} else {
|
|
/* line is far to the left or right: use backward motor motion */
|
|
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %100 */
|
|
if (pid<0) { /* turn right */
|
|
speedR = -speed+pid; /* decrease speed */
|
|
speedL = speed-pid; /* increase speed */
|
|
} else { /* turn left */
|
|
speedR = speed+pid; /* increase speed */
|
|
speedL = -speed-pid; /* decrease speed */
|
|
}
|
|
speedL = Limit(speedL, -speed, speed);
|
|
speedR = Limit(speedR, -speed, speed);
|
|
directionL = AbsSpeed(&speedL);
|
|
directionR = AbsSpeed(&speedR);
|
|
}
|
|
#endif
|
|
/* speed is now always positive, make sure it is within 16bit PWM boundary */
|
|
if (speedL>0xFFFF) {
|
|
speedL = 0xFFFF;
|
|
} else if (speedL<0) {
|
|
speedL = 0;
|
|
}
|
|
if (speedR>0xFFFF) {
|
|
speedR = 0xFFFF;
|
|
} else if (speedR<0) {
|
|
speedR = 0;
|
|
}
|
|
if (!forward) { /* swap direction/speed */
|
|
if (directionL==MOT_DIR_FORWARD) {
|
|
directionL=MOT_DIR_BACKWARD;
|
|
} else {
|
|
directionL=MOT_DIR_FORWARD;
|
|
}
|
|
if (directionR==MOT_DIR_FORWARD) {
|
|
directionR=MOT_DIR_BACKWARD;
|
|
} else {
|
|
directionR=MOT_DIR_FORWARD;
|
|
}
|
|
}
|
|
/* send new speed values to motor */
|
|
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
|
|
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
|
|
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
|
|
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
|
|
#if PID_DEBUG /* debug diagnostic */
|
|
{
|
|
cnt++;
|
|
if (cnt>10) { /* limit number of messages to the console */
|
|
McuShell_StdIO_OutErr_FctType ioOut = SHELL_GetStdio()->stdOut;
|
|
cnt = 0;
|
|
|
|
McuShell_SendStr((unsigned char*)"line:", ioOut);
|
|
buf[0] = '\0';
|
|
McuUtility_strcatNum16u(buf, sizeof(buf), currLine);
|
|
McuShell_SendStr(buf, ioOut);
|
|
|
|
McuShell_SendStr((unsigned char*)" sum:", ioOut);
|
|
buf[0] = '\0';
|
|
McuUtility_strcatNum32Hex(buf, sizeof(buf), integral);
|
|
McuShell_SendStr(buf, ioOut);
|
|
|
|
McuShell_SendStr((unsigned char*)" left:", ioOut);
|
|
McuShell_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
|
|
buf[0] = '\0';
|
|
McuUtility_strcatNum16Hex(buf, sizeof(buf), speedL);
|
|
McuShell_SendStr(buf, ioOut);
|
|
|
|
McuShell_SendStr((unsigned char*)" right:", ioOut);
|
|
McuShell_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
|
|
buf[0] = '\0';
|
|
McuUtility_strcatNum16Hex(buf, sizeof(buf), speedR);
|
|
McuShell_SendStr(buf, ioOut);
|
|
|
|
McuShell_SendStr((unsigned char*)"\r\n", ioOut);
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
#endif //PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
|
|
|
|
#endif
|
|
|
|
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
|
|
void PID_Line(uint16_t currLinePos, uint16_t setLinePos, uint16_t currLineWidth, bool forward) {
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
#if PL_GO_DEADEND_BW
|
|
if (forward) {
|
|
PID_LineCfg(currLinePos, setLinePos, forward, &lineFwConfig);
|
|
} else {
|
|
PID_LineCfg(currLinePos, setLinePos, forward, &lineBwConfig);
|
|
}
|
|
#else
|
|
(void)forward; /* not used */
|
|
PID_LineCfg(currLinePos, setLinePos, currLineWidth, forward, &lineFwConfig);
|
|
#endif
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
#if PL_CONFIG_USE_POS_PID
|
|
static void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
|
|
int32_t speed;
|
|
MOT_Direction direction=MOT_DIR_FORWARD;
|
|
MOT_MotorDevice *motHandle;
|
|
|
|
#if PL_HAS_HIGH_RES_ENCODER
|
|
int error;
|
|
|
|
error = setPos-currPos;
|
|
if (error>-10 && error<10) { /* avoid jitter around zero */
|
|
setPos = currPos;
|
|
}
|
|
#endif
|
|
speed = PID(currPos, setPos, config);
|
|
/* transform into motor speed */
|
|
speed *= 1000; /* scale PID, otherwise we need high PID constants */
|
|
if (speed>=0) {
|
|
direction = MOT_DIR_FORWARD;
|
|
} else { /* negative, make it positive */
|
|
speed = -speed; /* make positive */
|
|
direction = MOT_DIR_BACKWARD;
|
|
}
|
|
/* speed is now always positive, make sure it is within 16bit PWM boundary */
|
|
if (speed>0xFFFF) {
|
|
speed = 0xFFFF;
|
|
}
|
|
/* limit speed to maximum value */
|
|
speed = (speed*config->maxSpeedPercent)/100;
|
|
/* send new speed values to motor */
|
|
if (isLeft) {
|
|
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
|
|
} else {
|
|
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
|
|
}
|
|
MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
|
|
MOT_SetDirection(motHandle, direction);
|
|
MOT_UpdatePercent(motHandle, direction);
|
|
}
|
|
|
|
void PID_Pos(int32_t currPos, int32_t setPos, bool isLeft) {
|
|
if (isLeft) {
|
|
PID_PosCfg(currPos, setPos, isLeft, &posLeftConfig);
|
|
} else {
|
|
PID_PosCfg(currPos, setPos, isLeft, &posRightConfig);
|
|
}
|
|
}
|
|
#endif /* PL_CONFIG_USE_POS_PID */
|
|
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
static void PID_SpeedCfg(int32_t currSpeed, int32_t setSpeed, bool isLeft, PID_Config *config) {
|
|
int32_t speed;
|
|
MOT_Direction direction=MOT_DIR_FORWARD;
|
|
MOT_MotorDevice *motHandle;
|
|
|
|
if (setSpeed==0) { /* Actually this test is more of a hack, should not be needed! */
|
|
speed = 0;
|
|
/* reset PID I and D values */
|
|
config->integral = 0;
|
|
config->lastError = 0;
|
|
} else {
|
|
speed = PID(currSpeed, setSpeed, config);
|
|
}
|
|
if (speed>=0) {
|
|
direction = MOT_DIR_FORWARD;
|
|
} else { /* negative, make it positive */
|
|
speed = -speed; /* make positive */
|
|
direction = MOT_DIR_BACKWARD;
|
|
}
|
|
/* speed shall be positive here, make sure it is within 16bit PWM boundary */
|
|
if (speed>0xFFFF) {
|
|
speed = 0xFFFF;
|
|
}
|
|
/* send new speed values to motor */
|
|
if (isLeft) {
|
|
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
|
|
} else {
|
|
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
|
|
}
|
|
MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
|
|
MOT_SetDirection(motHandle, direction);
|
|
MOT_UpdatePercent(motHandle, direction);
|
|
}
|
|
|
|
void PID_Speed(int32_t currSpeed, int32_t setSpeed, bool isLeft) {
|
|
if (isLeft) {
|
|
PID_SpeedCfg(currSpeed, setSpeed, isLeft, &speedLeftConfig);
|
|
} else {
|
|
PID_SpeedCfg(currSpeed, setSpeed, isLeft, &speedRightConfig);
|
|
}
|
|
}
|
|
#endif /* PL_CONFIG_USE_SPEED_PID */
|
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
static void PID_PrintHelp(const McuShell_StdIOType *io) {
|
|
McuShell_SendHelpStr((unsigned char*)"pid", (unsigned char*)"Group of PID commands\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows PID help or status\r\n", io->stdOut);
|
|
#if PL_CONFIG_USE_POS_PID
|
|
McuShell_SendHelpStr((unsigned char*)" pos (p|i|d|w) <value>", (unsigned char*)"Sets P, I, D or anti-Windup position value\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" pos speed <value>", (unsigned char*)"Maximum speed % value\r\n", io->stdOut);
|
|
#endif
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
McuShell_SendHelpStr((unsigned char*)" speed (L|R) (p|i|d|w) <value>", (unsigned char*)"Sets P, I, D or anti-Windup position value\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" speed (L|R) speed <value>", (unsigned char*)"Maximum speed % value\r\n", io->stdOut);
|
|
#endif
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
McuShell_SendHelpStr((unsigned char*)" fw (p|i|d|w) <value>", (unsigned char*)"Sets P, I, D or anti-Windup line value\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" fw speed <value>", (unsigned char*)"Maximum speed % value\r\n", io->stdOut);
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
McuShell_SendHelpStr((unsigned char*)" bw (p|i|d|w) <value>", (unsigned char*)"Sets P, I, D or anti-Windup backward value\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" bw speed <value>", (unsigned char*)"Maximum backward speed % value\r\n", io->stdOut);
|
|
#endif
|
|
}
|
|
|
|
#if PL_CONFIG_USE_SPEED_PID || PL_CONFIG_USE_POS_PID || PL_CONFIG_USE_LINE_PID
|
|
static void PrintPIDstatus(PID_Config *config, const unsigned char *kindStr, const McuShell_StdIOType *io) {
|
|
unsigned char buf[48];
|
|
unsigned char kindBuf[16];
|
|
|
|
McuUtility_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
|
|
McuUtility_strcat(kindBuf, sizeof(buf), kindStr);
|
|
McuUtility_strcat(kindBuf, sizeof(buf), (unsigned char*)" PID");
|
|
McuUtility_strcpy(buf, sizeof(buf), (unsigned char*)"p: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), config->pFactor100);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" i: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), config->iFactor100);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" d: ");
|
|
McuUtility_strcatNum32s(buf, sizeof(buf), config->dFactor100);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
|
|
McuShell_SendStatusStr(kindBuf, buf, io->stdOut);
|
|
|
|
McuUtility_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
|
|
McuUtility_strcat(kindBuf, sizeof(buf), kindStr);
|
|
McuUtility_strcat(kindBuf, sizeof(buf), (unsigned char*)" windup");
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), config->iAntiWindup);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
|
|
McuShell_SendStatusStr(kindBuf, buf, io->stdOut);
|
|
|
|
McuUtility_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
|
|
McuUtility_strcat(kindBuf, sizeof(buf), kindStr);
|
|
McuUtility_strcat(kindBuf, sizeof(buf), (unsigned char*)" error");
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), config->lastError);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
|
|
McuShell_SendStatusStr(kindBuf, buf, io->stdOut);
|
|
|
|
McuUtility_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
|
|
McuUtility_strcat(kindBuf, sizeof(buf), kindStr);
|
|
McuUtility_strcat(kindBuf, sizeof(buf), (unsigned char*)" integral");
|
|
McuUtility_Num32sToStr(buf, sizeof(buf), config->integral);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
|
|
McuShell_SendStatusStr(kindBuf, buf, io->stdOut);
|
|
|
|
McuUtility_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
|
|
McuUtility_strcat(kindBuf, sizeof(buf), kindStr);
|
|
McuUtility_strcat(kindBuf, sizeof(buf), (unsigned char*)" speed");
|
|
McuUtility_Num8uToStr(buf, sizeof(buf), config->maxSpeedPercent);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
|
|
McuShell_SendStatusStr(kindBuf, buf, io->stdOut);
|
|
}
|
|
#endif
|
|
|
|
static void PID_PrintStatus(const McuShell_StdIOType *io) {
|
|
#if PL_CONFIG_USE_LINE_PID || PL_GO_DEADEND_BW || PL_CONFIG_USE_POS_PID || PL_CONFIG_USE_SPEED_PID
|
|
McuShell_SendStatusStr((unsigned char*)"pid", (unsigned char*)"PID control loop status\r\n", io->stdOut);
|
|
#endif
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
PrintPIDstatus(&lineFwConfig, (unsigned char*)"fw", io);
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
PrintPIDstatus(&lineBwConfig, (unsigned char*)"bw", io);
|
|
#endif
|
|
#if PL_CONFIG_USE_POS_PID
|
|
PrintPIDstatus(&posLeftConfig, (unsigned char*)"pos L", io);
|
|
PrintPIDstatus(&posRightConfig, (unsigned char*)"pos R", io);
|
|
#endif
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
PrintPIDstatus(&speedLeftConfig, (unsigned char*)"speed L", io);
|
|
PrintPIDstatus(&speedRightConfig, (unsigned char*)"speed R", io);
|
|
#endif
|
|
}
|
|
|
|
#if PL_CONFIG_USE_SPEED_PID || PL_CONFIG_USE_POS_PID || PL_CONFIG_USE_LINE_PID
|
|
static uint8_t ParsePidParameter(PID_Config *config, const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
|
|
const unsigned char *p;
|
|
uint32_t val32u;
|
|
uint8_t val8u;
|
|
uint8_t res = ERR_OK;
|
|
|
|
if (McuUtility_strncmp((char*)cmd, (char*)"p ", sizeof("p ")-1)==0) {
|
|
p = cmd+sizeof("p");
|
|
if (McuUtility_ScanDecimal32uNumber(&p, &val32u)==ERR_OK) {
|
|
config->pFactor100 = val32u;
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"i ", sizeof("i ")-1)==0) {
|
|
p = cmd+sizeof("i");
|
|
if (McuUtility_ScanDecimal32uNumber(&p, &val32u)==ERR_OK) {
|
|
config->iFactor100 = val32u;
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"d ", sizeof("d ")-1)==0) {
|
|
p = cmd+sizeof("d");
|
|
if (McuUtility_ScanDecimal32uNumber(&p, &val32u)==ERR_OK) {
|
|
config->dFactor100 = val32u;
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"w ", sizeof("w ")-1)==0) {
|
|
p = cmd+sizeof("w");
|
|
if (McuUtility_ScanDecimal32uNumber(&p, &val32u)==ERR_OK) {
|
|
config->iAntiWindup = val32u;
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"speed ", sizeof("speed ")-1)==0) {
|
|
p = cmd+sizeof("speed");
|
|
if (McuUtility_ScanDecimal8uNumber(&p, &val8u)==ERR_OK && val8u<=100) {
|
|
config->maxSpeedPercent = val8u;
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
#endif
|
|
|
|
uint8_t PID_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
|
|
uint8_t res = ERR_OK;
|
|
|
|
if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"pid help")==0) {
|
|
PID_PrintHelp(io);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"pid status")==0) {
|
|
PID_PrintStatus(io);
|
|
*handled = TRUE;
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"pid fw ", sizeof("pid fw ")-1)==0) {
|
|
res = ParsePidParameter(&lineFwConfig, cmd+sizeof("pid fw ")-1, handled, io);
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"pid bw ", sizeof("pid bw ")-1)==0) {
|
|
res = ParsePidParameter(&lineBwConfig, cmd+sizeof("pid bw ")-1, handled, io);
|
|
#endif
|
|
#if PL_CONFIG_USE_POS_PID
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"pid pos ", sizeof("pid pos ")-1)==0) {
|
|
res = ParsePidParameter(&posLeftConfig, cmd+sizeof("pid pos ")-1, handled, io);
|
|
if (res==ERR_OK) {
|
|
res = ParsePidParameter(&posRightConfig, cmd+sizeof("pid pos ")-1, handled, io);
|
|
}
|
|
#endif
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"pid speed L ", sizeof("pid speed L ")-1)==0) {
|
|
res = ParsePidParameter(&speedLeftConfig, cmd+sizeof("pid speed L ")-1, handled, io);
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"pid speed R ", sizeof("pid speed R ")-1)==0) {
|
|
res = ParsePidParameter(&speedRightConfig, cmd+sizeof("pid speed R ")-1, handled, io);
|
|
#endif
|
|
}
|
|
return res;
|
|
}
|
|
#endif /* PL_CONFIG_USE_SHELL */
|
|
|
|
void PID_Start(void) {
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
lineBwConfig.lastError = 0;
|
|
lineBwConfig.integral = 0;
|
|
#endif
|
|
#if PL_CONFIG_USE_POS_PID
|
|
posLeftConfig.lastError = 0;
|
|
posLeftConfig.integral = 0;
|
|
posRightConfig.lastError = 0;
|
|
posRightConfig.integral = 0;
|
|
#endif
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
speedLeftConfig.lastError = 0;
|
|
speedLeftConfig.integral = 0;
|
|
speedRightConfig.lastError = 0;
|
|
speedRightConfig.integral = 0;
|
|
#endif
|
|
}
|
|
|
|
void PID_Deinit(void) {
|
|
}
|
|
|
|
void PID_Init(void) {
|
|
#if PL_CONFIG_USE_LINE_PID
|
|
#if PL_IS_ZUMO_ROBOT && PL_IS_MOTOR_1_100
|
|
lineFwConfig.pFactor100 = 5500;
|
|
lineFwConfig.iFactor100 = 15;
|
|
lineFwConfig.dFactor100 = 100;
|
|
lineFwConfig.iAntiWindup = 100000;
|
|
lineFwConfig.maxSpeedPercent = 100;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#elif PL_IS_ZUMO_ROBOT
|
|
lineFwConfig.pFactor100 = 5500;
|
|
lineFwConfig.iFactor100 = 15;
|
|
lineFwConfig.dFactor100 = 100;
|
|
lineFwConfig.iAntiWindup = 100000;
|
|
lineFwConfig.maxSpeedPercent = 60;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#elif PL_IS_ROUND_ROBOT
|
|
lineFwConfig.pFactor100 = 200;
|
|
lineFwConfig.iFactor100 = 1;
|
|
lineFwConfig.dFactor100 = 50000;
|
|
lineFwConfig.iAntiWindup = 20000;
|
|
lineFwConfig.maxSpeedPercent = 15;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#elif PL_IS_TRACK_ROBOT
|
|
lineFwConfig.pFactor100 = 400;
|
|
lineFwConfig.iFactor100 = 1;
|
|
lineFwConfig.dFactor100 = 50000;
|
|
lineFwConfig.iAntiWindup = 20000;
|
|
lineFwConfig.maxSpeedPercent = 17;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT
|
|
lineFwConfig.pFactor100 = 5500;
|
|
lineFwConfig.iFactor100 = 15;
|
|
lineFwConfig.dFactor100 = 100;
|
|
lineFwConfig.iAntiWindup = 100000;
|
|
lineFwConfig.maxSpeedPercent = 15;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#elif PL_IS_INTRO_ZUMO_K22 && PL_HAS_LIPO
|
|
lineFwConfig.pFactor100 = 2000;
|
|
lineFwConfig.iFactor100 = 15;
|
|
lineFwConfig.dFactor100 = 100;
|
|
lineFwConfig.iAntiWindup = 100000;
|
|
lineFwConfig.maxSpeedPercent = 15;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#elif PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22
|
|
lineFwConfig.pFactor100 = 4000;//5500;
|
|
lineFwConfig.iFactor100 = 15;
|
|
lineFwConfig.dFactor100 = 500;//100;
|
|
lineFwConfig.iAntiWindup = 100000;
|
|
lineFwConfig.maxSpeedPercent = 25;//40;
|
|
lineFwConfig.lastError = 0;
|
|
lineFwConfig.integral = 0;
|
|
#else
|
|
#error "unknown configuration!"
|
|
#endif
|
|
#if PL_GO_DEADEND_BW
|
|
lineBwConfig.param = 0;
|
|
lineBwConfig.pFactor100 = 1000;
|
|
lineBwConfig.iFactor100 = 0;
|
|
lineBwConfig.dFactor100 = 0;
|
|
lineBwConfig.iAntiWindup = 100000;
|
|
lineBwConfig.maxSpeedPercent = 20;
|
|
lineBwConfig.lastError = 0;
|
|
lineBwConfig.integral = 0;
|
|
#endif
|
|
#endif /* PL_CONFIG_USE_LINE_PID */
|
|
|
|
#if PL_CONFIG_USE_POS_PID
|
|
#if PL_CONFIG_APP_SUMO && PL_SLOWER_SPEED
|
|
posLeftConfig.pFactor100 = 1000;
|
|
posLeftConfig.iFactor100 = 1;
|
|
posLeftConfig.dFactor100 = 50;
|
|
posLeftConfig.iAntiWindup = 200;
|
|
posLeftConfig.maxSpeedPercent = 30;
|
|
#elif PL_CONFIG_APP_SUMO /* sumo, high speed */
|
|
posLeftConfig.pFactor100 = 1000;
|
|
posLeftConfig.iFactor100 = 1;
|
|
posLeftConfig.dFactor100 = 50;
|
|
posLeftConfig.iAntiWindup = 200;
|
|
posLeftConfig.maxSpeedPercent = 100;
|
|
#elif (PL_CONFIG_DO_LINE_FOLLOWING || PL_DO_LINE_MAZE) && PL_SLOWER_SPEED /* line/maze, high speed */
|
|
posLeftConfig.pFactor100 = 1500;
|
|
posLeftConfig.iFactor100 = 2;
|
|
posLeftConfig.dFactor100 = 50;
|
|
posLeftConfig.iAntiWindup = 200;
|
|
posLeftConfig.maxSpeedPercent = 40;
|
|
#elif (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) /* line/maze, high speed */
|
|
posLeftConfig.pFactor100 = 2000;//1000;
|
|
posLeftConfig.iFactor100 = 2;
|
|
posLeftConfig.dFactor100 = 50;
|
|
posLeftConfig.iAntiWindup = 200;
|
|
posLeftConfig.maxSpeedPercent = 35;//40;
|
|
#else /* defaults */
|
|
posLeftConfig.pFactor100 = 1000;
|
|
posLeftConfig.iFactor100 = 1;
|
|
posLeftConfig.dFactor100 = 50;
|
|
posLeftConfig.iAntiWindup = 200;
|
|
posLeftConfig.maxSpeedPercent = 30;
|
|
#endif
|
|
posLeftConfig.lastError = 0;
|
|
posLeftConfig.integral = 0;
|
|
posRightConfig.pFactor100 = posLeftConfig.pFactor100;
|
|
posRightConfig.iFactor100 = posLeftConfig.iFactor100;
|
|
posRightConfig.dFactor100 = posLeftConfig.dFactor100;
|
|
posRightConfig.iAntiWindup = posLeftConfig.iAntiWindup;
|
|
posRightConfig.maxSpeedPercent = posLeftConfig.maxSpeedPercent;
|
|
posRightConfig.lastError = posLeftConfig.lastError;
|
|
posRightConfig.integral = posLeftConfig.integral;
|
|
#endif
|
|
#if PL_CONFIG_USE_SPEED_PID
|
|
#if PL_IS_INTRO_ZUMO_K22
|
|
speedLeftConfig.pFactor100 = 2000;
|
|
speedLeftConfig.iFactor100 = 80;
|
|
speedLeftConfig.dFactor100 = 0;
|
|
speedLeftConfig.iAntiWindup = 120000;
|
|
#if PL_SLOWER_SPEED
|
|
speedLeftConfig.maxSpeedPercent = 50;
|
|
#else
|
|
speedLeftConfig.maxSpeedPercent = 100;
|
|
#endif
|
|
#else
|
|
speedLeftConfig.pFactor100 = 15000;
|
|
speedLeftConfig.iFactor100 = 500;
|
|
speedLeftConfig.dFactor100 = 100;
|
|
speedLeftConfig.iAntiWindup = 15000;
|
|
#if PL_SLOWER_SPEED
|
|
speedLeftConfig.maxSpeedPercent = 30;
|
|
#else
|
|
speedLeftConfig.maxSpeedPercent = 50;
|
|
#endif
|
|
#endif
|
|
speedLeftConfig.lastError = 0;
|
|
speedLeftConfig.integral = 0;
|
|
speedRightConfig.pFactor100 = speedLeftConfig.pFactor100;
|
|
speedRightConfig.iFactor100 = speedLeftConfig.iFactor100;
|
|
speedRightConfig.dFactor100 = speedLeftConfig.dFactor100;
|
|
speedRightConfig.iAntiWindup = speedLeftConfig.iAntiWindup;
|
|
speedRightConfig.maxSpeedPercent = speedLeftConfig.maxSpeedPercent;
|
|
speedRightConfig.lastError = speedLeftConfig.lastError;
|
|
speedRightConfig.integral = speedLeftConfig.integral;
|
|
#endif
|
|
}
|
|
#endif /* PL_CONFIG_USE_PID */
|
|
|