Well, as Jason S said, this question is old :). But below is my approach. I've implemented this on a PIC16F616 running at 8MHz internal oscillator, using XC8 compiler. The code should explain itself in the comments, if not, ask me. Also, I can share the whole project, as I will do in my website later on.
/*
* applyEncoder Task:
* -----------------
* Calculates the PID (proportional-integral-derivative) to set the motor
* speed.
*
* PID_error = setMotorSpeed - currentMotorSpeed
* PID_sum = PID_Kp * (PID_error) + PID_Ki * ∫(PID_error) + PID_Kd * (ΔPID_error)
*
* or if the motor is speedier than it is set;
*
* PID_error = currentMotorSpeed - setMotorSpeed
* PID_sum = - PID_Kp * (PID_error) - PID_Ki * ∫(PID_error) - PID_Kd * (ΔPID_error)
*
* Maximum value of PID_sum will be about:
* 127*255 + 63*Iul + 63*255 = 65500
*
* Where Iul is Integral upper limit and is about 250.
*
* If we divide by 256, we scale that down to about 0 to 255, that is the scale
* of the PWM value.
*
* This task takes about 750us. Real figure is at the debug pin.
*
* This task will fire when the startPID bit is set. This happens when a
* sample is taken, about every 50 ms. When the startPID bit is not set,
* the task yields the control of the CPU for other tasks' use.
*/
void applyPID(void)
{
static unsigned int PID_sum = 0; // Sum of all PID terms.
static unsigned int PID_integral = 0; // Integral for the integral term.
static unsigned char PID_derivative = 0; // PID derivative term.
static unsigned char PID_error; // Error term.
static unsigned char PID_lastError = 0; // Record of the previous error term.
static unsigned int tmp1; // Temporary register for holding miscellaneous stuff.
static unsigned int tmp2; // Temporary register for holding miscellaneous stuff.
OS_initializeTask(); // Initialize the task. Needed by RTOS. See RTOS header file for the details.
while (1)
{
while (!startPID) // Wait for startPID bit to be 1.
{
OS_yield(); // If startPID is not 1, yield the CPU to other tasks in the mean-time.
}
DebugPin = 1; // We will measure how much time it takes to implement a PID controller.
if (currentMotorSpeed > setMotorSpeed) // If the motor is speedier than it is set,
{
// PID error is the difference between set value and current value.
PID_error = (unsigned char) (currentMotorSpeed - setMotorSpeed);
// Integrate errors by subtracting them from the PID_integral variable.
if (PID_error < PID_integral) // If the subtraction will not underflow,
PID_integral -= PID_error; // Subtract the error from the current error integration.
else
PID_integral = 0; // If the subtraction will underflow, then set it to zero.
// Integral term is: Ki * ∫error
tmp1 = PID_Ki * PID_integral;
// Check if PID_sum will overflow in the addition of integral term.
tmp2 = 0xFFFF - tmp1;
if (PID_sum < tmp2)
PID_sum += tmp1; // If it will not overflow, then add it.
else
PID_sum = 0xFFFF; // If it will, then saturate it.
if (PID_error >= PID_lastError) // If current error is bigger than last error,
PID_derivative = (unsigned char) (PID_error - PID_lastError);
// then calculate the derivative by subtracting them.
else
PID_derivative = (unsigned char) (PID_lastError - PID_error);
// Derivative term is : Kd * d(Δerror)
tmp1 = PID_Kd * PID_derivative;
// Check if PID_sum will overflow in the addition of derivative term.
if (tmp1 < PID_sum) // Check if subtraction will underflow PID_sum
PID_sum -= tmp1;
else PID_sum = 0; // If the subtraction will underflow, then set it to zero.
// Proportional term is: Kp * error
tmp1 = PID_Kp * PID_error; // Calculate the proportional term.
if (tmp1 < PID_sum) // Check if subtraction will underflow PID_sum
PID_sum -= tmp1;
else PID_sum = 0; // If the subtraction will underflow, then set it to zero.
}
else // If the motor is slower than it is set,
{
PID_error = (unsigned char) (setMotorSpeed - currentMotorSpeed);
// Proportional term is: Kp * error
PID_sum = PID_Kp * PID_error;
PID_integral += PID_error; // Add the error to the integral term.
if (PID_integral > PID_integralUpperLimit) // If we have reached the upper limit of the integral,
PID_integral = PID_integralUpperLimit; // then limit it there.
// Integral term is: Ki * ∫error
tmp1 = PID_Ki * PID_integral;
// Check if PID_sum will overflow in the addition of integral term.
tmp2 = 0xFFFF - tmp1;
if (PID_sum < tmp2)
PID_sum += tmp1; // If it will not overflow, then add it.
else
PID_sum = 0xFFFF; // If it will, then saturate it.
if (PID_error >= PID_lastError) // If current error is bigger than last error,
PID_derivative = (unsigned char) (PID_error - PID_lastError);
// then calculate the derivative by subtracting them.
else
PID_derivative = (unsigned char) (PID_lastError - PID_error);
// Derivative term is : Kd * d(Δerror)
tmp1 = PID_Kd * PID_derivative;
// Check if PID_sum will overflow in the addition of derivative term.
tmp2 = 0xFFFF - tmp1;
if (PID_sum < tmp2)
PID_sum += tmp1; // If it will not overflow, then add it.
else
PID_sum = 0xFFFF; // If it will, then saturate it.
}
// Scale the sum to 0 - 255 from 0 - 65535 , dividing by 256, or right shifting 8.
PID_sum >>= 8;
// Set the duty cycle to the calculated and scaled PID_sum.
PWM_dutyCycle = (unsigned char) PID_sum;
PID_lastError = PID_error; // Make the current error the last error, since it is old now.
startPID = 0; // Clear the flag. That will let this task wait for the flag.
DebugPin = 0; // We are finished with the PID control block.
}
}
