Giải thuật PID

[nupakachi]

Xin chào mọi người.Nhờ mọi người gúp mình đoạn code về điều khiển động cơ dùng giải thuật PID này với.Mình làm mãi mà nó chưa chạy.Không hiểu nó sai ở đâu.Xin cảm ơn rất nhiều.
#include"p18f46k20.h"
#include"timers.h"
#include"pwm.h"
#include"lcd_lcd.h"
#pragma config FOSC = HS // Oscillator Selection bits (Internal oscillator block, port function on RA6 and RA7)
#pragma config FCMEN = OFF // Fail-Safe Clock Monitor Enable bit (Fail-Safe Clock Monitor disabled)
#pragma config IESO = OFF // Internal/External Oscillator Switchover bit (Oscillator Switchover mode disabled)

// CONFIG2L
#pragma config PWRT = OFF // Power-up Timer Enable bit (PWRT disabled)
#pragma config BOREN = SBORDIS // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware only (SBOREN is disabled))
#pragma config BORV = 18 // Brown Out Reset Voltage bits (VBOR set to 1.8 V nominal)

// CONFIG2H
#pragma config WDTEN = OFF // Watchdog Timer Enable bit (WDT is controlled by SWDTEN bit of the WDTCON register)
#pragma config WDTPS = 32768 // Watchdog Timer Postscale Select bits (1:32768)

// CONFIG3H
#pragma config CCP2MX = PORTC // CCP2 MUX bit (CCP2 input/output is multiplexed with RC1)
#pragma config PBADEN = OFF // PORTB A/D Enable bit (PORTB<4:0> pins are configured as digital I/O on Reset)
#pragma config LPT1OSC = OFF // Low-Power Timer1 Oscillator Enable bit (Timer1 configured for higher power operation)
#pragma config HFOFST = OFF // HFINTOSC Fast Start-up (The system clock is held off until the HFINTOSC is stable.)
#pragma config MCLRE = OFF // MCLR Pin Enable bit (RE3 input pin enabled; MCLR disabled)

// CONFIG4L
#pragma config STVREN = ON // Stack Full/Underflow Reset Enable bit (Stack full/underflow will cause Reset)
#pragma config LVP = OFF // Single-Supply ICSP Enable bit (Single-Supply ICSP disabled)
#pragma config XINST = OFF // Extended Instruction Set Enable bit (Instruction set extension and Indexed Addressing mode disabled (Legacy mode))

// CONFIG5L
#pragma config CP0 = OFF // Code Protection Block 0 (Block 0 (000800-003FFFh) not code-protected)
#pragma config CP1 = OFF // Code Protection Block 1 (Block 1 (004000-007FFFh) not code-protected)
#pragma config CP2 = OFF // Code Protection Block 2 (Block 2 (008000-00BFFFh) not code-protected)
#pragma config CP3 = OFF // Code Protection Block 3 (Block 3 (00C000-00FFFFh) not code-protected)

// CONFIG5H
#pragma config CPB = OFF // Boot Block Code Protection bit (Boot block (000000-0007FFh) not code-protected)
#pragma config CPD = OFF // Data EEPROM Code Protection bit (Data EEPROM not code-protected)

// CONFIG6L
#pragma config WRT0 = OFF // Write Protection Block 0 (Block 0 (000800-003FFFh) not write-protected)
#pragma config WRT1 = OFF // Write Protection Block 1 (Block 1 (004000-007FFFh) not write-protected)
#pragma config WRT2 = OFF // Write Protection Block 2 (Block 2 (008000-00BFFFh) not write-protected)
#pragma config WRT3 = OFF // Write Protection Block 3 (Block 3 (00C000h-00FFFFh) not write-protected)

// CONFIG6H
#pragma config WRTC = OFF // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) not write-protected)
#pragma config WRTB = OFF // Boot Block Write Protection bit (Boot Block (000000-0007FFh) not write-protected)
#pragma config WRTD = OFF // Data EEPROM Write Protection bit (Data EEPROM not write-protected)

// CONFIG7L
#pragma config EBTR0 = OFF // Table Read Protection Block 0 (Block 0 (000800-003FFFh) not protected from table reads executed in other blocks)
#pragma config EBTR1 = OFF // Table Read Protection Block 1 (Block 1 (004000-007FFFh) not protected from table reads executed in other blocks)
#pragma config EBTR2 = OFF // Table Read Protection Block 2 (Block 2 (008000-00BFFFh) not protected from table reads executed in other blocks)
#pragma config EBTR3 = OFF // Table Read Protection Block 3 (Block 3 (00C000-00FFFFh) not protected from table reads executed in other blocks)

#define Sampling_time 25
#define inv_Sampling_time 40
#define PWM_Period 5 // gia tri gan vao PR2 de tao tan so PWM =10KHz
#define Duty_cycle 2400 // gia tri duty cycle max

//prototype function
void interrupt_isr(void);
long int abs(float val);
void Motion_Speed_PID(long int des_Speed);
// bien toan cuc
long int Pulse=0,pre_Pulse=0;
float rSpeed=0,Err=0,pre_Err=0,Kp=5,Kd=0.6,Ki=2.4;// 3 thong so nay thu nghiem cua ro
float pPart=0,iPart=0,dPart=0;
long int Ctrl_Speed=5;
int Output;
unsigned long Sample_count=0,count=0;
char str1[]="Actual:";
char str2[]="Desires:";
long int abs(float val)
{
long int data;
if(val<0)
data =0-val;
else data=val;
return data;
}
/**********************************giai thuat PID*******************************/
void Motion_Speed_PID(long int des_Speed)
{
rSpeed=Pulse-pre_Pulse; //tinh van toc (trong sampling time)
pre_Pulse=Pulse;
Err=des_Speed-abs(rSpeed); //tinh error (loi)
pPart=Kp*Err;
dPart=Kd*(Err-pre_Err)*inv_Sampling_time;
iPart+=Ki*Sampling_time*(Err+pre_Err)/1000;
Output +=pPart+dPart+iPart; //cong thuc duoc bien doi vi la dieu khien van toc
if (Output >Duty_cycle) Output=Duty_cycle-2;
if (Output <=0) Output=1;
SetDCPWM2(Output); //gan duty cycle cho CCP1 update PWM
pre_Err=Err;
}
void main(void)
{
TRISBbits.TRISB0=1;
TRISBbits.TRISB1=1;
/********************setting timer0 .creat interrupt 25ms***************************/
OpenTimer0( TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_16 );//su dung timer0 16bit
WriteTimer0(63973); //dat gia tri vao TMR0 de tao ngat sau 25ms
/******************************* setting ext interrupt*****************************/
INTCON2bits.INTEDG1 =0;//ngat suon xuong
INTCON3bits.INT1IP =1;
INTCON3bits.INT1IE =1;
INTCON3bits.INT1IF =0;
/******************************timer 2 pwm***************************************/
OpenTimer2(TIMER_INT_OFF&T2_PS_1_16&T2_POST_1_1);
WriteTimer2(0);
/******************************setting pwm *****************************************/
OpenPWM2(PWM_Period); //Configure PWM module and initialize PWM period
SetDCPWM2(0);
/**********************************setting lcd************************************/
Init_PORTS();
Init_LCD();
Delay1KTCYx(10);
lcd_clear();
lcd_gotoxy(1,1);
lcd_putstr(str1);
lcd_gotoxy(1,2);
lcd_putstr(str2);

INTCONbits.GIE=1;
INTCONbits.PEIE=1;
while(1)
{
if(Sample_count>=10)
{
lcd_gotoxy(10,1);
lcd_int(Ctrl_Speed);
lcd_gotoxy(10,2);
lcd_int(rSpeed);
Sample_count=0;
}
};
}
#pragma code interrupt_vector=0x08
void interrupt_vector(void)
{
_asm
goto interrupt_isr
_endasm
}
#pragma code
#pragma interrupt interrupt_isr
void interrupt_isr(void)
{
if(INTCONbits.TMR0IF==1)
{
INTCONbits.TMR0IF =0;
T0CONbits.TMR0ON=0;
WriteTimer0(63973);
Sample_count++;
Motion_Speed_PID(Ctrl_Speed);
T0CONbits.TMR0ON=1;
}
if(INTCON3bits.INT1IF=1)
{
if(PORTBbits.RB0==1) Pulse++;
else Pulse--;
INTCON3bits.INT1IF=0;
}
}