Xây dựng hệ thống để giám sát điều khiển PID tốc độ động cơ (Phần 3)

#include <16F887.h>
#device adc=8

#FUSES NOWDT                    //No Watch Dog Timer
#FUSES RC                       //Resistor/Capacitor Osc with CLKOUT
#FUSES NOPUT                    //No Power Up Timer
#FUSES MCLR                     //Master Clear pin enabled
#FUSES NOPROTECT                //Code not protected from reading
#FUSES NOCPD                    //No EE protection
#FUSES BROWNOUT                 //Reset when brownout detected
#FUSES IESO                     //Internal External Switch Over mode enabled
#FUSES FCMEN                    //Fail-safe clock monitor enabled
#FUSES LVP                      //Low Voltage Programming on B3(PIC16) or B5(PIC18)
#FUSES NODEBUG                  //No Debug mode for ICD
#FUSES NOWRT                    //Program memory not write protected
#FUSES BORV40                   //Brownout reset at 4.0V

#use delay(clock=20000000)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)



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




#define SW1 PIN_B0
#define SW2 PIN_B4
#define DIR1 PIN_C0
#define PWM PIN_C2
#define DIR2 PIN_C1


char c_start,c_stop,c_tem;
char s1[21];
int k;
int biennhan;


char smthuan[]="qt";
int co_thuan;
char smnghich[]="qn";
int co_nghich;
char smngung[]="ng";
char smtocdodat[]="td";
char smpid[]="pid";
long tocdodat;
float ki,kp,kd;
float u=0,e=0;
float e1,e2,u1;
float a,b,c;
float tocdo1,sovong;
signed int32 TSW=0;
signed int32 soxungdangdem;
float ppwm=0;
float soxungdoduoc=0;
float soxungdemduoc;
char t,t1;



#int_rda
void RDA_isr(void)
{
c_tem=getc();
  if(c_tem=='@')
  {
   c_start = c_tem;
   k=0;
   s1[k]=c_start;
  }
  if(c_tem=='x')
  {
   c_stop=c_tem;
   s1[k]=c_stop;
  }
  if(c_start=='@'&&c_stop=='x')
  {
   k=0;
   biennhan=1;
   c_start=0;
   c_stop=0;
  }
  else
  {
   s1[k]=c_tem;
   k++;
    if(k>21)
    {
     k=0;
    }
  }
}



#INT_RB
void RB_isr(void)
{
   if(!input(SW1))
   {
    while(!input(SW1));
    if(input(SW2)==0)
    {
    TSW+=1;
    soxungdangdem+=1;
    }
    else
    {
     TSW--;
     soxungdangdem--;
    }
   }
}




#INT_TIMER1
void ngattimer1()
{
set_timer1(3035);
t++;
t1++;
soxungdoduoc=TSW;
TSW=0;
soxungdemduoc = soxungdangdem;
}



void tocdo()
{
         tocdo1=((float)soxungdoduoc*10*60)/360;
         sovong = ((float)soxungdemduoc)/360;
         printf("%s %03.2f %s\n\r","@TT",tocdo1,"x");
         printf("%s %03.2f %s\n\r","@SV",sovong,"x");
}

void naptocdodat()
{
char tocdat[3];
tocdat[0]=s1[4];
tocdat[1]=s1[5];
tocdat[2]=s1[6];
tocdodat = atol(tocdat);
}




void nappid()
{
long kp1,ki1,kd1;
char p[4],i[4],d[4];
p[0]=s1[5];
p[1]=s1[6];
p[2]=s1[7];
p[3]=s1[8];
kp1 = atoi32(p);
kp = ((float)kp1)/100;
i[0]=s1[10];
i[1]=s1[11];
i[2]=s1[12];
i[3]=s1[13];
ki1 = atoi32(i);
ki = ((float)ki1)/100;
d[0]=s1[15];
d[1]=s1[16];
d[2]=s1[17];
d[3]=s1[18];
kd1 = atoi32(d);
kd = ((float)kd1)/100;     
}



void pid()
{
tocdo();
e = tocdodat - abs(tocdo1);
a=kp+ki/2+kd;
b=-kp+ki/2-2*kd;
c=kd;
u =u1+a*e+b*e1+c*e2;
if(u>=1024){u=1024;}
if(u<=0){u=0;}
e2=e1;
e1=e;
u1=u;
ppwm =(float)(u/(1024))*512;
}




void ngung()
{
set_pwm1_duty(0);
output_bit(DIR1,0);
output_bit(DIR2,0);
setup_ccp1(CCP_OFF);
co_thuan=0;
co_nghich=0;
}




void quaythuan()
{
float pwm_t;
pwm_t=(float)ppwm;
setup_ccp1(CCP_PWM);
output_bit(DIR1,1);
output_bit(DIR2,0);
set_pwm1_duty((int16)pwm_t);
co_nghich=0;
co_thuan=1;
}




void quaynghich()
{
float pwm_n;
pwm_n=(float)ppwm;
setup_ccp1(CCP_PWM);
output_bit(DIR1,0);
output_bit(DIR2,1);
set_pwm1_duty((int16)pwm_n);
co_thuan=0;
co_nghich=1;
}




void xoanhan()
{
int j;
for(j = 0;j<=21;j++) {s1[j] = 0;}
}



void xetma()
{
  if (biennhan)
   {
    if (strncmp(strstr(s1,smtocdodat),smtocdodat,2)==0)                         
    {
       naptocdodat();
    }
    if (strncmp(strstr(s1,smpid),smpid,3)==0)
    {
        nappid();
    }
    if (strncmp(strstr(s1,smnghich),smnghich,2)==0)
    {
        quaynghich();
    }
    if (strncmp(strstr(s1,smthuan),smthuan,2)==0)
    {
        quaythuan();
    }
    if (strncmp(strstr(s1,smngung),smngung,2)==0)
    {
        ngung();
    }
xoanhan();
biennhan=0;
}
}



void main()
{
ext_int_edge(H_TO_L );
enable_interrupts(INT_RB0);
enable_interrupts(INT_RB4);
enable_interrupts(INT_RDA);
enable_interrupts(INT_TIMER1);
enable_interrupts(GLOBAL);
setup_timer_2(T2_DIV_BY_16,127,1);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);
set_timer1(3035);
delay_ms(10);
while (1)
{
    xetma();
    if(t)
    {
       pid();
       if(co_thuan)
       {
       quaythuan();
       }
       if(co_nghich)
       {
       quaynghich();
       }
       t = 0; 
     }     
}
}