PID vận tốc với PIC!

[dvnccbmacbt]

Chào mọi người!
Hiện tại mình đang làm điều khiển PID phản hồi 2 vòng cho động cơ. Phần PID điều khiển vị trí thì đã ok. Nhưng còn PID cho vận tốc thì mình chưa làm được. Mong mọi người giúp đỡ. Không biết ai đã làm hoàn chỉnh phần code cho PIC để điều khiển PID vận tốc rồi, cho mình tham khảo với. Còn đây là code của mình. MÌnh nghĩ không biết nó bị lỗi chỗ nào nhưng khi điều khiển thì đồ thị rất xấu. (vận tốc lên xuông liên tục dạng răng cưa). Mình cảm ơn nhiều.

#include <18f4550.h>
#device ADC=10
//#fuses HS,NOWDT,NOPROTECT,NOLVP,xt
#FUSES HS                       //High speed Osc (> 4mhz)
#FUSES PUT                      //Power Up Timer
#FUSES NOPROTECT                //Code not protected from reading
#FUSES NOBROWNOUT               //No brownout reset
#FUSES NOLVP                    //Low Voltage Programming on B3(PIC16) or B5(PIC18)
#FUSES NOCPD                    //No EE protection
#FUSES NODEBUG                  //No Debug mode for ICD
#FUSES NOWDT

#use delay(clock=20000000)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8) // Giao tiep RS232
//#use rs232(baud=9600, parity=N, bits=8, xmit=PIN_A1,rcv=PIN_A2)

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>

#define step_time 0.04
#define can_tren 200
#define can_duoi 60

signed int32 so_xung = 0,vec =0 , so_xung_temp = 0;
signed int32 err = 0, err_sum = 0, err_temp = 0 ;
signed int32 p_r1 = 0, err2 = 0, err2_sum = 0, err2_temp = 0, err_vec =0, err_sum_vec =0, err_temp_vec =0;
float vmax = 0;
int8 Kp2=0,Ki2=0,Kd2=0;

signed int32 P2_term = 0, I2_term = 0, D2_term = 0;

signed int16 u_t_vec =0;
 
#INT_RDA
void ngat_serial()
{ gets(str);
  if ((str[strlen(str)-2]=='1')&&(str[strlen(str)-1]=='p'))       fini = 1;
  else if ((str[strlen(str)-2]=='1')&&(str[strlen(str)-1]=='i'))     fini = 2;
  else if ((str[strlen(str)-2]=='1')&&(str[strlen(str)-1]=='d'))     fini = 3;
  else if ((str[strlen(str)-2]=='1')&&(str[strlen(str)-1]=='v'))     fini = 4;
  else if ((str[strlen(str)-2]=='2')&&(str[strlen(str)-1]=='p'))     fini = 5;
  else if ((str[strlen(str)-2]=='2')&&(str[strlen(str)-1]=='i'))     fini = 6;
  else if ((str[strlen(str)-2]=='2')&&(str[strlen(str)-1]=='d'))     fini = 7;
  else if ((str[strlen(str)-2]=='3')&&(str[strlen(str)-1]=='p'))     fini = 8;
  else if ((str[strlen(str)-2]=='3')&&(str[strlen(str)-1]=='i'))     fini = 9;
  else if ((str[strlen(str)-2]=='3')&&(str[strlen(str)-1]=='d'))     fini = 10;
  else if ((str[strlen(str)-2]=='v')&&(str[strlen(str)-1]=='m'))     fini = 11;
  else if ((str[strlen(str)-2]=='a')&&(str[strlen(str)-1]=='m'))     fini = 12;
  else if ((str[0]=='o')&&(str[1]=='n'))   {  on = 1;} // enable_interrupts(INT_TIMER1);}
  else if ((str[0]=='o')&&(str[1]=='f')&&(str[2]=='f'))   {  disable_interrupts(INT_TIMER1); }
  strncpy(value,str,strlen(str)-2);
  for(n=0;n<=9;n++)  str[n]=NULL;

  if (fini == 1)      { Kp1 = (float)atof(value);  }
  else if (fini == 2) { Ki1 = (float)atof(value);  }
  else if (fini == 3) { Kd1 = (float)atof(value);  }
  else if (fini == 4) { p_r = (float)atof(value);  }
  else if (fini == 5) { Kp2 = (float)atof(value);  }
  else if (fini == 6) { Ki2 = (float)atof(value);  }
  else if (fini == 7) { Kd2 = (float)atof(value);  }
  else if (fini == 8) { Kp3 = (float)atof(value);  }
  else if (fini == 9) { Ki3 = (float)atof(value);  }
  else if (fini == 10){ Kd3 = (float)atof(value);  }
  else if (fini == 11){ vmax = (float)atof(value); }
  else if (fini == 12){ amax = (float)atof(value); }
  fini=0;
  for(n=0;n<=4;n++)  value[n]=NULL;
 }



#INT_EXT2
void ngat_EXT2()
{
 if (input(PIN_B0) == 1)  {so_xung++;chieu_quay = 1;}
 else                     {so_xung--;chieu_quay = 0;}
}

void PID2()
{
 err_vec = (int16)vmax - vec;
 err_sum_vec += err_vec;
 err_temp_vec = err_vec;
 
 P2_term = Kp2 * err_vec;
 If ( P2_term >= 5000) P2_term = 5000;
 //-------dieu chinh I -------//
 I2_term = Ki2 * err_sum_vec;
 If ( I2_term >= 5000) I2_term = 5000;
 
 //-------dieu chinh P--------//
 u_t_vec += P2_term + I2_term; 
 
 if ( u_t_vec >= can_tren ) u_t_vec = can_tren;
 
 else if (( u_t_vec < can_duoi )) {
  u_t_vec = can_duoi;
 } 
 //u_t_vec = ceil (u_t_vec);
 output_d(can_tren + can_duoi - (int8)u_t_vec);
}


#INT_TIMER1
void interrupt_timer1() {
 set_timer1(15536);
 vec = so_xung - so_xung_temp;
 so_xung_temp = so_xung;
 printf("2%Ld\r",vec);
 //printf("1%Lu\r",err_vec);
 k=1;

}

void main()
{
 set_tris_b(0x3);
 set_tris_a(0x3);
 set_tris_e(0x00);
 set_tris_c(0x80);
 set_tris_d(0x00);
 output_d(100);
 SETUP_ADC(ADC_CLOCK_INTERNAL);
 enable_interrupts(INT_RDA);
 setup_timer_1(T1_INTERNAL | T1_DIV_BY_1 );
 set_timer1(15536);
 enable_interrupts(GLOBAL);
 //------- PID-----//
  output = 100;
  output_d(100);
  delay_ms(500);
 while ((output<200)&&(on==0)) {
   output_d(output);
   delay_ms(10);
   output++;
   if(output == 200) output=180;
 }
 enable_interrupts(INT_TIMER1);
 setup_adc_ports(AN0_TO_AN1);
 enable_interrupts(INT_EXT2);
 ext_int_edge(H_TO_L);
 so_xung = 0;
 /*Kp1 = 1;
 p_r = 15000;*/
// vmax = 3500;
 amax = 10000;
// initial();
 while(true) {
  if (k == 1) 
  {
   PID2();
   k=0;
  }
 }
}

[mienvan]

anh co the gui chuong trinh anh thuc hien PID cho mach vong van toc va vi tri cho em voi cam on anh nhieu.em dang rat can.email vanmien1@gmail.com

[hcmut.khoamaisi]

Có cao thủ nào đã làm 1 cái đồ án hoàn chỉnh DKDC co PID rồi có thể up len cho mọi người tham khảo không

[nhauruou]

up up em cũng đang rất cần để tài này. chủ topic cho em xin cái của anh để tham khảo với mail của em là nhaukhongxin@gmail.com . thank thank thank!

[vienhanlam01]

code bạn viết nhìn có vẻ void chóng mặt (int8 quá ) ; hôm sau mìnhh sẽ gởi code điều khiển vận tốc = pid cho động cơ nhé , có vẽ đồ thị bằng vb luôn . code của mình viết trực quan, đồ thị vẽ đáp ứng rất đẹp.

[arm_cortex01]

code nay minh thu ok ne,
#include <16f877a.h>
#fuses HS,PUT,NOWDT
#device adc=10
#use delay (clock = 20000000)
#use rs232(baud = 9600 , parity = n , xmit = pin_c6 , rcv = pin_c7 , bits = 9)
#include <LCD_4BIT_fix.c>

float e,e1,a,b,i,x;
int16 value,j , vttb ;

int8 c,d , tg;

int8 chuyendoi(int8 gt) ;
#int_timer0
void ngat()
{
i=get_timer1();
set_timer1(0x00);

e=x-i;
a = e - e1;
b = (e + e1) + b;
e1 = e;

value = (int16)(e + b/30);
c = (int8)(!(bit_test (value,15)));

value = value * c ;
d = (int8)((bit_test(value,11))|(bit_test(value,12))|
(bit_test(value,13))|(bit_test(value,14)));

value = value * (!d) + 1023 * d;

set_pwm1_duty(value);


}

[arm_cortex01]

void main ()
{
value=0;
i=0;
e=0;
e1=0;
a=0;
vttb=0;
b=0;


setup_adc(ADC_CLOCK_INTERNAL );
lcd_init(); //ham khoi tao LCD
lcd_setposition(line_1);
printf(lcd_putchar,"SET : RPM");

lcd_setposition(line_2);
printf(lcd_putchar,"SPEED : 0 RPM");


setup_ccp1(CCP_PWM);
setup_timer_2(T2_DIV_BY_1,255, 1);

output_e (0xf2);

enable_interrupts (INT_TIMER0);
enable_interrupts (GLOBAL);



while(true)
{

j= (Read_ADC())*17/10; // so vong / phut
x=(j*0.032768);
lcd_setposition(line_1);
printf(lcd_putchar,"SET : RPM");
LCD_PutCmd ( 0x88 );
printf(lcd_putchar,"%ld",j);
vttb=(int16)(i*30.51757812); // vantoc trung binh

lcd_setposition(line_2);
printf(lcd_putchar,"SPEED : RPM");
LCD_PutCmd ( 0xc8 );
printf(lcd_putchar,"%ld",vttb);
delay_ms (200);

}
}