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;
  }
 }
}