pid_struct_t PIDOutput;
unsigned int16 ADCReading;
signed int16 K1 = 7, K2 = -6, K3 = 0;
unsigned int16 SetPoint = 500;
unsigned int16 PWMDuty;
setup_pid(PID_MODE_PID, K1, K2, K3);
//Setup ADC
setup_adc_ports(sAN3, VSS_VDD);
setup_adc(ADC_CLOCK_INTERNAL);
set_adc_channel(3);
//Setup PWM 3
setup_timer_4(T4_CLK_INTERNAL | T4_DIV_BY_32, 249, 1); //1ms period, from 32 MHz clock
set_pwm3_duty(0); //0% duty
setup_pwm3(PWM_ENABLED | PWM_OUTPUT | PWM_TIMER4);
while(TRUE) {
delay_ms(50);
ADCReading = read_adc();
pid_get_result(SetPoint, ADCReading, &PIDOutput);
PIDOutput.u &= 0x07;
if(PIDOutput.u >= 4) //PIDOutput is negative, set PWMDuty to Minimum
PWMDuty = 0;
else if(PIDOutput.u != 0) //PIDOutput > Maximum, set PWMDuty to Maximum
PWMDuty = 1000;
else if(PIDOutput.l > 1000) //PIDOutput > Maximum, set PWMDuty to Maximum
PWMDuty = 1000;
else
PWMDuty = PIDOutput.l;
set_pwm3_duty(PWMDuty);
}