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