Giúp mình mình về PID

[cuongbn89]

không phụ thuộc vào V_set. Mọi người xem giúp mình với. Code của mình đây:
#include "D:\My Documents\ccc\sdaada.h"
int16 count;
int8 a;
int16 xung1=0,xung2=0;
int16 count1=0,count2=0;
int32 dem=0;
float V_now1=0,V_sum1=0,V_last1=0,V_del1=0;
long V_real1=0,V_set1=200;
float duty1=0;
float V_now2=0,V_sum2=0,V_last2=0,V_del2=0;
long V_real2=0,V_set2=50;
float duty2=0;
int kp=8,ki=10,kd=1;
void demxung2()
{
if (input(PIN_A5)==0)
{
while(input(PIN_A5)==0);

count2=count2+1;
xung2=count2;
break;}
else
{
xung2=count2;
break;}
}
void demxung1()
{
if (input(PIN_A3)==0)
{
while(input(PIN_A3)==0);

count1=count1+1;
xung1=count1;
break;}
else
{
xung1=count1;
break;}
}
#int_TIMER0
void TIMER0_isr()
{
set_timer0(6);
++dem;
if(dem == 500) // 2000*500us = 1000000us = 1s
{
dem=0;
V_real1=xung1;
count1=0;
V_now1=V_set1-V_real1;
V_sum1=V_now1+V_last1;
V_del1=V_now1-V_last1;
V_last1=V_now1;
duty1=duty1+Kp*V_now1+Ki*V_sum1+Kd*V_del1;
if(duty1>255) duty1=255;
if(duty1<0) duty1=0 ;
delay_ms(200) ;
set_pwm1_duty(duty1);
V_real2=xung2;
count2=0;
V_now2=V_set2-V_real2;
V_sum2=V_now2+V_last2;
V_del2=V_now2-V_last2;
V_last2=V_now2;
duty2=duty2+Kp*V_now2+Ki*V_sum2+Kd*V_del2;
if(duty2>255) duty2=255;
if(duty2<0) duty2=0 ;
delay_ms(200);
set_pwm2_duty(duty2);

}
}



void main()
{

setup_adc_ports(NO_ANALOGS);
setup_adc(ADC_OFF);
setup_spi(FALSE);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DIV_BY_16,255,1);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
enable_interrupts(INT_TIMER0);
enable_interrupts(GLOBAL);
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
set_tris_c(0x00);

// TODO: USER CODE!!
while(true)
{
demxung1();
demxung2();


//output_c(0xff);
//delay_ms(200);
//output_low(pin_c4);
}
}