void PID_PosCfg(int16_t currPos, int16_t setPos, bool isLeft, PID_Config *config) {
int32_t pid, speed;
MOT_Direction direction=MOT_DIR_FORWARD;
pid = PID(currPos, setPos, config);
/* transform into motor speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* limit the speed */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* move forward */
speed -= pid;
direction = MOT_DIR_FORWARD;
} else { /* move backward */
speed += pid;
direction = MOT_DIR_BACKWARD;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
} else if (speed<0) {
speed = 0;
}
/* send new speed values to motor */
if (isLeft) {
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), direction);
} else {
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), direction);
}
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
/*! \todo Apply PID values to speed values */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed-pid;
}
/* 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;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
int32_t pwm;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
pwm = PID(currPos, setPos, config);
/* transform into motor pwm */
pwm *= 1000; /* scale PID, otherwise we need high PID constants */
if (pwm>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
pwm = -pwm; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* pwm is now always positive, make sure it is within 16bit PWM boundary */
if (pwm>0xFFFF) {
pwm = 0xFFFF;
}
/* send new pwm values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-pwm); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
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;
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 MOT_SetSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent percent) {
uint32_t val;
motor->currSpeedPercent = percent; /* store current value */
if (percent<0) {
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
percent = -percent;
} else {
MOT_SetDirection(motor, MOT_DIR_FORWARD);
}
if (percent>100) { /* make sure we are within 0..100 */
percent = 100;
}
val = ((100-percent)*0xffff)/100;
MOT_SetVal(motor, (uint16_t)val);
}
void MOT_SetSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent percent) {
/*! \todo See lab guide about this function */
uint32_t val;
if (percent>100) { /* make sure we are within 0..100 */
percent = 100;
} else if (percent<-100) {
percent = -100;
}
motor->currSpeedPercent = percent; /* store value */
if (percent<0) {
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
percent = -percent; /* make it positive */
} else {
MOT_SetDirection(motor, MOT_DIR_FORWARD);
}
val = ((100-percent)*0xffff)/100; /* H-Bridge is low active! */
MOT_SetVal(motor, (uint16_t)val);
}
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
int32_t speed, val;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
int error;
#define POS_FILTER 5
error = setPos-currPos;
if (error>-POS_FILTER && error<POS_FILTER) { /* avoid jitter around zero */
setPos = currPos;
}
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;
}
#if 1
/* limit speed to maximum value */
val = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
speed = Limit(speed, -val, val);
#else
/* limit speed to maximum value */
speed = (speed*config->maxSpeedPercent)/100;
#endif
/* 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);
}
/*
* \brief -100% bis 100% wobei Minuswerte Rückwärts drehen...
* \author Philippe Schenker
* \todo Performance verbessern
*/
void MOT_SetSpeedPercent(MOT_MotorSide side, MOT_SpeedPercent percent) {
uint16_t newRatio = 0;
MOT_MotorDevice* motor = MOT_GetMotorHandle(side);
if(side==MOT_MOTOR_RIGHT) {
percent *= -1;
}
if(percent >= 0) { /* FORWARD */
if(percent > 100) percent = 100;
percent = 100 - percent;
newRatio = (int) (655.35*percent);
MOT_SetDirection(motor, MOT_DIR_FORWARD);
} else if(percent < 0) { /* BACKWARD */
percent = abs(percent);
if(percent > 100) percent = 100;
percent = 100 - percent;
newRatio = (int) (655.35*percent);
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
}
MOT_SetVal(motor, newRatio);
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
uint8_t errorPercent;
errorPercent = errorWithinPercent(currLine-setLine);
/* change speed depending on error */
/* transform into different speed for motors. The PID is used as difference value to the motor PWM */
if (errorPercent <= 20) { /* 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 <= 40) {
/* outside left/right halve position from center, slow down one motor and speed up the other */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/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 if (errorPercent <= 70) {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-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; /* %80 */
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);
}
/* 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;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, 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;
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 (errorPercent <= 20) { /* 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 <= 40) {
/* outside left/right halve position from center, slow down one motor and speed up the other */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/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 if (errorPercent <= 70) {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-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; /* %80 */
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);
}
/* 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;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
#if PID_DEBUG /* debug diagnostic */
{
cnt++;
if (cnt>10) { /* limit number of messages to the console */
CLS1_StdIO_OutErr_FctType ioOut = CLS1_GetStdio()->stdOut;
cnt = 0;
CLS1_SendStr((unsigned char*)"line:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), currLine);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" sum:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum32Hex(buf, sizeof(buf), integral);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" left:", ioOut);
CLS1_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedL);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" right:", ioOut);
CLS1_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedR);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)"\r\n", ioOut);
}
}
#endif
}
