I am trying to make a magnetic Suspension Device, very similar looking to this. Image
The objects position is sensed via IR-diode and buffered before feeding to ADC.
The PWM is setup at around 4Khz.
I am using PIC16f877A and also using LCD to display various diagnostic datas if required.
This is my first PID project, so, I may be missing the very basics.
Please, Have a look at this code, and teach me something.
Feel free to ask anything.
The objects position is sensed via IR-diode and buffered before feeding to ADC.
The PWM is setup at around 4Khz.
I am using PIC16f877A and also using LCD to display various diagnostic datas if required.
This is my first PID project, so, I may be missing the very basics.
Please, Have a look at this code, and teach me something.
Feel free to ask anything.
Code:
void main()
{
setup_ad();
init_LCD();
init_PWM(); //intialize PWM
set_duty(514U); // set PWM dutycyle = 514. ( 512 means 50%)
init_Compare(); // Use CCP2 compare module to set CCP2IF flag at 256Hz rate
// used to trigger PID loop
GIE = 0;
PEIE = 1;
TMR1IE=0;
unsigned int voltage = 0; // variable for the ADC value (gives the object position)
unsigned int ref = 512; // refrence postion (middle)
unsigned int K = 2; // some constant
signed int error = 0;
signed int Iterm,Dterm,Pterm;
signed int net_error=0;
signed int old_error=0;
signed int correction;
signed int new_duty=0;
while(1)
{
if(CCP2IF){ // wait for PID Sampling trigger(done at 256 Hz)
CCP2IF = 0;
// GoDone set automatically
while(GODONE);
voltage = ADRESH*256+ADRESL; // Objects Position Value
error = ref - voltage;
net_error += error;
if(net_error > 1023) net_error = 1023;
if(net_error < -1023) net_error = -1023;
Iterm = net_error/4;
Dterm = (error-old_error)*4;
Pterm = error/4;
correction = K*(Pterm + Iterm + Dterm);
new_duty = 512 - correction;
if(new_duty>1023) new_duty = 1023;
if(new_duty<0) new_duty = 0;
set_duty(new_duty);
LCD_Goto(2,9);
LCD_Write_Num(new_duty);
old_error = error;
}
}
}
Last edited: