PID Controller

/*
 * File:   main.c
 * Author: abdullah kahraman
 *
 * Created on 30 Haziran 2012 Cumartesi, 16:45
 */
#include <xc.h> // Include the header file needed by the compiler
#include "RTOS_Debug.h" // Include the header for co-operative RTOS.
#define _XTAL_FREQ 8000000 // XTAL frequency is 8MHz. This is needed for the __delay_us() function of the XC8.
__CONFIG(FOSC_INTOSCIO & WDTE_ON & PWRTE_ON & MCLRE_OFF & CP_ON & IOSCFS_8MHZ & BOREN_ON);
/*
 * CONFIGURATION WORD:
 * -------------------
 * Internal Oscillator is set to 8 MHz.
 * I/O function on both of the oscillator pins, i.e on RA4 and on RA5.
 * Master clear is disabled. RA3 pin is digital input.
 * Power-up timer is enabled.
 * Watchdog timer is enabled.
 * Code protection is enabled.
 * Brown-out reset is enabled.
 */
/***** ADC Channel DEFINITIONS *****/
#define KpValueAnalogChannel  5 // These are the channels that are connected to the
#define KiValueAnalogChannel  4 // PID gain setting potentiometers. Value represents
#define KdValueAnalogChannel  1 // the channel number. For example "1" means "AN1".
#define setValueAnalogChannel 0 // The potentiometer for setting the desired motor speed.
/* END OF ADC Channel DEFINITIONS */

#define DebugPin               RA4 // Pin to output debug information
#define PWM_dutyCycle          CCPR1L // Give the PWM duty cycle setter variable a more readable name.
#define PID_integralUpperLimit 2000 // Maximum limit for PID Integral

/**** RTOS DEFINITIONS  ****/
unsigned char OS_currentTask; // This register holds the current task's place in the array OS_tasks
unsigned char OS_tasks[6]; /* This array holds PCL and PCLATH for tasks. This array will have
                              (number of tasks)*2 elements, since every task occupies 2 places.*/
/* END OF RTOS DEFINITIONS */

/**** TIMING VARIABLES ****/
unsigned char _50_us = 0; // This is the variable that the program will store the time in fifty microseconds.
unsigned char _10_ms = 0; // This is the variable that the program will store the time in milliseconds.
unsigned char _10_ms_ctr = 0; // This variable will count the _50_us so that we can get 10 ms in every 200 x _50_us.
unsigned char _50_ms_ctr = 0; // This variable will count _10_ms so that we can get 50ms in every 5 x _1_ms
unsigned char _100_ms_ctr = 0; // This variable will count _10_ms so that we can get 100ms in every 10 x _1_ms
/**** END OF TIMING VARIABLES ****/

/*** SYSTEM VARIABLES ****/
bit startPID; // This bit controls when the applyPID task will do its job.
bit sampleADCs; // Control bit for the ADC sampling task. The task will wait for this bit to be 1.
bit calculateSpeed; // Notifies readEncoder task to calculate the speed, every 50ms.
unsigned char risingEdges; // Whenever an interrupt occurs on INT pin, this variable will be incremented.
unsigned char currentMotorSpeed; // Holds the speed of the motor which is gathered from the encoder on RA2 pin.
unsigned char setMotorSpeed; // The desired motor speed set by the user via a potentiometer.
unsigned char PID_Kp; // Proportional error constant variable for PID control, set by a pot.
unsigned char PID_Ki; // Integral error constant variable for PID control, set by a pot.
unsigned char PID_Kd; // Derivative error constant variable for PID control, set by a pot.
/*** END OF SYSTEM VARIABLES ****/

/**** Interrupt Function ****/
void interrupt myInterrupt(void)
{
    if (TMR0IF) // If we have a TMR2 interrupt
    {
        // Interrupt will come up every 50uS
        asm("INCF __50_us, F"); // Increment the 50us counter.
        asm("INCF __10_ms_ctr, F"); // Increment the 1ms counter.
        if (_10_ms_ctr == 200) // If _10_ms_ctr is 200, (50us x 200)=10ms
        {
            asm("INCF __10_ms, F"); // Increment the 10ms counter.
            asm("INCF __100_ms_ctr, F"); // Increment the 100ms counter.
            asm("INCF __50_ms_ctr, F"); // Increment the 50ms counter.
            if (_50_ms_ctr == 5) // If it is 5, (10msx5) = 50ms
            {
                calculateSpeed = 1; // Set this bit so that readEncoder task can function.
                _50_ms_ctr = 0; // Reset the 50ms counter.
            }
            if (_100_ms_ctr == 10) // If it is 10, (10ms x 10) = 100ms
            {
                sampleADCs = 1; // Set sampleADCs bit so that ADC sampling task can fire and sample.
                _100_ms_ctr = 0; // Reset the 100ms counter.
            }
            _10_ms_ctr = 0; // Reset the 10ms counter.
        }
        TMR0 = 187; // Reload the value for 50us period.
        TMR0IF = 0; // Clear the TMR0 interrupt flag.
    }
    if (INTF) // If we have a rising edge on RA2 (INT)
    {
        if (risingEdges < 255) // Ensure that risingEdges will saturate at 255 and will not overflow.
            asm("INCF _risingEdges, F"); // Increment risingEdges variable. Used in readEncoder task.
        INTF = 0; // Clear the INTF interrupt flag.
    }
}

/*
 * ADC_Sampler Task:
 * -----------------
 * Checks the ADC channels that are defined above in order,
 * scales them and puts the result in the corresponding variables.
 *
 * This function waits for sampleADCs bit to be set. This bit is
 * set every 100ms. Check the interrupt routine above for more detail.
 *
 * When the sampleADCs bit is not set, the task yields the control of
 * the CPU for other tasks' use.
 */
void ADC_Sampler(void)
{
    static unsigned int ADC_ResultBuffer; // This is the buffer variable to combine ADRESL and ADRESH.
    OS_initializeTask(); // Initialize the task. Needed by RTOS. See RTOS header file for the details.
    while (1)
    {
        while (!sampleADCs) // Wait for sampleADCs to be 1.
        {
            OS_yield(); // If sampleADCs is not 1, yield the CPU to other tasks.
        }

        ADON = 1; // Turn the ADC ON

        ADCON0bits.CHS = KpValueAnalogChannel; // Select the appropriate analog channel.
        __delay_us(25); // Wait for the 25us ADC aquisition time.
        GO_nDONE = 1; // Start the conversion. This bit will be cleared when the conversation is done.
        while (GO_nDONE) // Wait for the GO_nDONE bit to be zero.
        {
            OS_yield(); // If GO_nDONE is not 0, yield the CPU to other tasks.
        }
        ADC_ResultBuffer = (unsigned int) (ADRESH << 8) + ADRESL; // Combine 2 bit "ADRESH" and  8 bit "ADRESL" to one 16 bit register.
        ADC_ResultBuffer >>= 3; // The result was 0-1023. Divide by 8 so that we get a range of 0-127.
        PID_Kp = (unsigned char) (ADC_ResultBuffer); // Put the result into variable for proportional constant.

        OS_yield(); // We have used the CPU enough, yield the control to other tasks.

        ADCON0bits.CHS = KiValueAnalogChannel; // Select the appropriate analog channel.
        __delay_us(25); // Wait for the 25us ADC aquisition time.
        GO_nDONE = 1; // Start the conversion. This bit will be cleared when the conversation is done.
        while (GO_nDONE) // Wait for the GO_nDONE bit to be zero.
        {
            OS_yield(); // If GO_nDONE is not 0, yield the CPU to other tasks.
        }
        ADC_ResultBuffer = (unsigned int) (ADRESH << 8) + ADRESL; // Combine 2 bit "ADRESH" and  8 bit "ADRESL" to one 16 bit register.
        ADC_ResultBuffer >>= 5; // The result was 0-1023. Divide by 32 so that we get a range of 0-32.
        PID_Ki = (unsigned char) (ADC_ResultBuffer); // Put the result into variable for integral constant.

        OS_yield(); // We have used the CPU enough, yield the control to other tasks.

        ADCON0bits.CHS = KdValueAnalogChannel; // Select the appropriate analog channel.
        __delay_us(25); // Wait for the 25us ADC aquisition time.
        GO_nDONE = 1; // Start the conversion. This bit will be cleared when the conversation is done.
        while (GO_nDONE) // Wait for the GO_nDONE bit to be zero.
        {
            OS_yield(); // If GO_nDONE is not 0, yield the CPU to other tasks.
        }
        ADC_ResultBuffer = (unsigned int) (ADRESH << 8) + ADRESL; // Combine 2 bit "ADRESH" and  8 bit "ADRESL" to one 16 bit register.
        ADC_ResultBuffer >>= 5; // The result was 0-1023. Divide by 32 so that we get a range of 0-32.
        PID_Kd = (unsigned char) (ADC_ResultBuffer); // Put the result into variable for derivative constant.

        OS_yield(); // We have used the CPU enough, yield the control to other tasks.

        ADCON0bits.CHS = setValueAnalogChannel; // Select the appropriate analog channel.
        __delay_us(25); // Wait for the 25us ADC aquisition time.
        GO_nDONE = 1; // Start the conversion. This bit will be cleared when the conversation is done.
        while (GO_nDONE) // Wait for the GO_nDONE bit to be zero.
        {
            OS_yield(); // If GO_nDONE is not 0, yield the CPU to other tasks.
        }
        ADC_ResultBuffer = (unsigned int) (ADRESH << 8) + ADRESL; // Combine 2 bit "ADRESH" and  8 bit "ADRESL" to one 16 bit register.
        ADC_ResultBuffer >>= 2; // The result was 0-1023. Divide by 4 so that we get a range of 0-255.
        setMotorSpeed = (unsigned char) (ADC_ResultBuffer); // Put the result into variable for derivative constant.

        ADON = 0; // Turn the ADC OFF.
        sampleADCs = 0; // Do not come to this task until an another call is made.
    }
}

/*
 * readEncoder Task:
 * -----------------
 * Measures the speed of the motor, that is fed into RA2(INT) from an
 * encoder.
 *
 * It can measure from 25 Hz, up to 5 kHz.
 *
 * The task first waits for the calculateSpeed flag to be set. That will
 * occur every 50 ms. Then, when the flag is raised, the task updates
 * the motor speed with risingEdges count, and clears risingEdges register.
 * Afterwards, the task raises startPID flag, letting applyPID task function.
 * In the end, it clears its own control flag and waits for an another 50ms.
 *
 * risingEdges variable is updated by the hardware interrupt on pin
 * RA2(INT). Every time a rising edge occurs, it triggers the interrupt and
 * the interrupt increments risingEdges. In the end, speed will be determined
 * by the amount of rising edges that happened in 50ms.
 *
 * This task will fire when the calculateSpeed bit is set. This happens
 * every 50ms. When the calculateSpeed bit is not set, the task
 * yields the control of the CPU for other tasks' use.
 */
void readEncoder(void)
{
    OS_initializeTask(); // Initialize the task. Needed by RTOS. See RTOS header file for the details.
    while (1)
    {
        while (!calculateSpeed) // Wait for calculateSpeed to be 1.
        {
            OS_yield(); // If calculateSpeed is not 1, yield the CPU to other tasks.
        }
        currentMotorSpeed = risingEdges; // Get the value of the risingEdges.
        risingEdges = 0; // Clear it, so that a fresh sample can be taken.
        startPID = 1; // Set this bit so that applyPID task can now function, since we have a sample of the speed now.
        calculateSpeed = 0; // Clear the bit, so that task can wait for the ISR to set this bit, after 50ms.
    }
}

/*
 * 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.
    }
}

void main(void)
{
    TRISA = 0x2F; // RA0: Input / RA1: Input / RA2: Input / RA3: Input / RA4: Output / RA5: Input.
    TRISC = 0x1F; // RC0: Input / RC1: Input / RC2: Input / RC3: Input / RC4: Input  / RC5: Output.

    ANSEL = 0x33; // AN0(RA0), AN1(RA1), AN4(RC0) and AN5(RC1) pins are analog inputs.
    ADFM = 1; // ADFM: A/D Conversion Result Format Select bit: 1 = Right justified, 0 = Left justified
    ADCON1bits.ADCS = 0x05; // ADCS<2:0>: A/D Conversion Clock Select bits : 101 = FOSC/16, 2us conversion time

    PSA = 1; // Prescaler Assignment bit : 1 = Prescaler is assigned to the WDT, 0 = Prescaler is assigned to the Timer0 module
    OPTION_REGbits.PS = 0x07; // WDT Prescaler= 1 : 128 . Timeout period: ~2.3 seconds.
    T0CS = 0; // T0CS: TMR0 Clock Source Select bit : 0 = Internal instruction cycle clock (FOSC/4), 1 = Transition on T0CKI pin

    T2CON = 0x04; /* bit 7                 Unimplemented: Read as ‘0’
                  *  bit 6-3  TOUTPS<3:0>: Timer2 Output Postscaler Select bits
                  *           x0000xxx     0000 = 1:1 Postscaler
                  *  bit 2    TMR2ON:      Timer2 On bit
                  *           xxxxx1xx     1 = Timer2 is on
                  *  bit 1-0  T2CKPS<1:0>: Timer2 Clock Prescale Select bits
                  *           xxxxxx00     00 = Prescaler is 1           */
    CCP1CON = 0x0C; /* bit 7-6  P1M<1:0>:   PWM Output Configuration bits
                                00xxxxxx    00 = Single output; P1A modulated; P1B, P1C, P1D assigned as Port pins
                     * bit 5-4  DC1B<1:0>:  PWM Duty Cycle Least Significant bits
                     *          xx00xxxx    These bits are the two LSbs of the PWM duty cycle. The eight MSbs are found in CCPR1L.
                     * bit 3-0  CCP1M<3:0>: ECCP Mode Select bits
                     *          xxxx1100    PWM mode; P1A, P1C active-high; P1B, P1D active-high          */
    PR2 = 150; // Timer2 Module Period Register is 100, yielding to a PWM period of : (Tosc*4) * 100 = 75usec
    CCPR1L = 100; // This is the PWM duty-cycle setter. 0 to 150.

    OPTION_REGbits.INTEDG = 1; // INTEDG: Interrupt Edge Select bit. 1 = Interrupt on rising edge of RA2/INT pin.
    T0IE = 1; //T0IE: Timer0 Overflow Interrupt Enable bit
    GIE = 1; // GIE: Global Interrupt Enable bit

    sampleADCs = 0; // Clear the bits in the initialization.
    calculateSpeed = 0;

    OS_currentTask = 0; // Current task pointer points to the first task.
    ADC_Sampler(); // Run all the tasks to initialize them one by one.
    OS_currentTask += 2; // Increment task pointer by two since every task occupies 2 places in the array.
    readEncoder();
    OS_currentTask += 2;
    applyPID();
    OS_runTasks(6); // Run the tasks in order. The argument of this macro takes is: (Number of tasks) * 2
}