// Interrupt function
//==========================================================================
static void interrupt isr(void)
{
signed int set_value = 0;
signed int feedback_value = 0;
signed long error_value = 0;
static signed long pre_error = 0;
static signed long integral = 0;
signed long derivative = 0;
signed long output = 0;
static signed long pre_output = 0;
signed int motor_direction = 0;
unsigned char pwm_value=0;
if(TMR1IF == 1) // If TMR1 register overflowed.
{
TMR1IF = 0; // Clear TMR1 overflow flag.
feedback_value = ai_read(1); // Get the feedback value.
set_value = ai_read(0); // Get the set value.
error_value = set_value - feedback_value; // Calculate the error.
if ((pre_output != 255) && (pre_output != -255))
{
integral = integral + error_value; // Calculate integral.
}
derivative = error_value - pre_error; // Calculate derivative.
output = (kp * error_value) + (ki * integral) + (kd * derivative); // Calculate the output, pwm.
if (output > 255) output = 255; // Limit the output to maximum 255.
else if (output < -255) output = -255;
if (output < 209 && output >0 ) output = 210; //limit the output to minimum 210
if (output > -2099 && output <0) output =-210;
if (output > 0) motor_right((unsigned char)output); // When output is positive, turn right.
else if (output < 0) motor_left((unsigned char)(-output)); // When error is negative, turn left.
else motor_stop(); // When ouput is 0, stop the motor.
pre_error = error_value; // Save as previous error.
pre_output = output; // Save as previous output.
}
Users browsing this forum: No registered users and 11 guests