PIC Vietnam

Go Back   PIC Vietnam > Robotics > Điều khiển

Tài trợ cho PIC Vietnam
Trang chủ Đăng Kí Hỏi/Ðáp Thành Viên Lịch Tìm Kiếm Bài Trong Ngày Ðánh Dấu Ðã Ðọc Vi điều khiển

Điều khiển Lý thuyết điều khiển và ứng dụng lý thuyết điều khiển trong những trường hợp thực tế

 
 
Ðiều Chỉnh Xếp Bài
Prev Previous Post   Next Post Next
Old 11-12-2013, 05:36 PM   #1
nupakachi
Đệ tử 1 túi
 
Tham gia ngày: Jan 2013
Bài gửi: 21
:
Giải thuật PID

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;
}
}
nupakachi vẫn chưa có mặt trong diễn đàn   Trả Lời Với Trích Dẫn
 

Ðiều Chỉnh
Xếp Bài

Quyền Sử Dụng Ở Diễn Ðàn
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

BB code is Mở
Smilies đang Mở
[IMG] đang Mở
HTML đang Tắt

Chuyển đến


Múi giờ GMT. Hiện tại là 12:02 AM.


Được sáng lập bởi Đoàn Hiệp
Powered by vBulletin®
Page copy protected against web site content infringement by Copyscape
Copyright © PIC Vietnam