hi
i think this coad help you to implement the PID.
//this code initializes a 16 bit timer for use as a pwm and implements a pid controller
//I use this code for temperature correction, with the plant value being an adc thermistor reading,
//and the pwm hooked up to a heater (through a mosfet switch)
//this code is good for small plant values within a signed 16 bit range that require a direct pwm output
#ifndef __pid_h__
#define __pid_h__
#include <avr/io.h>
#include <inttypes.h>
//the desired plant value
const uint16_t SET_POINT = 512;
//stores the integral sum
int32_t integral;
//the previous error
int16_t prev_error;
void pid_setup(void) {
//pwm data direction
DDRB |= _BV(PB1);
//set the prescaler and wgm modes for ICR1 top fast pwm with no prescaling
TCCR1A |= _BV(COM1A1) | _BV(WGM11);
TCCR1B |= _BV(WGM13) | _BV(WGM12) | _BV(CS10);
//the higher the compare value, the longer the pulse stays high
//0 is completely off, 0xFFFF is completely on
OCR1A = 0;
ICR1 = 0xFFFF;
}
//apply a pid correction
//input the current plant value
//the gains are implemented as constants, but could easily be made global variables
void pid_correct(int16_t plant_value) {
//the plant error
//make sure error is positive when you need to apply a larger pwm value (wider positive width)
int16_t error = plant_value - SET_POINT;
//int16_t error = ((int16_t) SET_POINT) - plant_value;
//proportional * pGain
int32_t proportional = error * 200;
//integral * iGain
int32_t temp_sum = integral + ( error * 10 );
//check that the integral doesn't get out of the desired range
if ( temp_sum > 0xFFFF ) integral = 0xFFFF;
else
if (temp_sum < (-0xFFFF) ) integral = -0xFFFF;
else
integral = temp_sum;
//differential * dGain
//if you used the other error calculation, use the other differential as well
int32_t differential = ( (error - prev_error) * 20 );
//int32_t differential = ( (prev_error - error) * 20 );
//the final sum, with an overflow check
int32_t temp_pid_sum = proportional + integral + differential;
uint16_t pid_total;
//set max pwm value at ~95% of 2^16 (change this to whatever you like)
if ( temp_pid_sum > 62258 ) pid_total = 62258;
else
if ( temp_pid_sum < 0 ) pid_total = 0;
else
pid_total = (uint16_t) temp_pid_sum;
//set pwm compare value to change the duty cycle
OCR1A = pid_total;
//store the previous error
prev_error = error;
}
#endif