fuqep.c

关于2407控制电机及数据通讯的源代码。

/*位置模式下的梯形轨迹QEP1,正反转只需改动pid->position +=pid->v的加号，pid->mposition=(pid->count+65536*(pid->overflow)-1000)的65536的加号*/

#include"regs2407.h"
#include"math.h"
/*#include"stdio.h"*/
#define PERIOD 96
#define accel 1 
#define CLOCK 30

#pragma DATA_SECTION(fb,".con") 
int fb[150];

char uartcount;  /*记录本次串口接收个数 */
char c_end[]={13,10,0}; /*回车换行字符串结束*/
char data_rx[30]; 
unsigned int uartflag;   
unsigned int TxPR,pwm_cmpr,period_us; 
const char c_prompt[]="已设定PWM"; 

int FLAG,SAMPLE,i,j,m,t,ledcount;
 //*volatile int sample;*/
unsigned int a[8]={40,50,60,70,75,78,84,90};
unsigned int n;

typedef struct
{
	 unsigned int saturated;
         unsigned int phase;
         unsigned int move;
         unsigned int count;
         unsigned int overflow;
         unsigned int Kp;
         unsigned int Ki;
         unsigned int Kd;
         int PID_MAX;
         int PID_MIN;
         int DUTYCYCLE;
         int v;
         int mv;
         int vlimit;
         long e0;
         long e1;
         long de;
         long e_sum;
         long PID_OUT;
         long mposition;
         long position;
         long pdist;
         long even_cnt;
}PID;
PID control[2];
            

float atoff(char * c); 
void Tdata(char *str);
void PHANTOM(void);
void c_int2(void);
void c_int4(void);
void varinit(void);
void initPID(void);
void SYSINIT(void);
void UpdatePosition(PID*pid);
void UpdateTrajectory(PID*pid);
void calcPID(PID*pid,int k);
void cal(void);

float atoff(char * c)       /*相当于atof*/
{  
   int i=0,j=0,dot=0;
   int flag=0; float t3;    
   unsigned int flag1=0;
   t3=0;
   if(c[0]==45) flag=1; 
   for(i=(flag>0)?1:0;;i++)
   {
        if(c[i]==0||c[i]==0x20||c[i]==0xd||c[i]==44||c[i]==10) break;
        if((c[i]>='0')&&(c[i]<='9')||(c[i]=='.'))
        {
             if(flag1) flag1++;
             if(c[i]=='.') flag1++;
             else 
             {
                 t3*=10.0;
                 t3+=(float)(c[i]-0x30);
             }
         }   
         else {return (999);}  /*******含有非法字符**************/
    }       
    for(i=1;i<flag1;i++) t3/=10;
    if(flag) t3=0-t3;
    return t3;
}

void Tdata(char *str)
{
    unsigned int ii;
    for(ii=0;str[ii];ii++)   /*循环发送字符串*/
    {
        SCITXBUF=str[ii];
        while((SCICTL2&0x40)==0);/*发送完成*/          
    }    
}
 
void Tfdata(float t,int num)
{
    int jtemp=0;  
    int itemp=0;
    int ttemp;  
    while(t>=1)
    {jtemp++;t=t/10;}
    for(itemp=0;itemp<jtemp+num+1;itemp++)
    {
       if(itemp==jtemp) {SCITXBUF=46;}
       else
       {
       ttemp=(int)(t*10);
       SCITXBUF=ttemp+48;
       t=t*10-ttemp;   
       } 
       while((SCICTL2&0x40)==0);  
    }      
}  

void PHANTOM(void)
{
        while(1);                                     /* a place to hang if illegal trap */
}


              
void c_int2(void)                                     /*T3周期中断*/
{
        FLAG++;
        if(FLAG==20)                                 /*FLAG对T3PINT进行计数，当FLAG=20时，进行采样*/
        {SAMPLE=1;} 
        EVBIFRA=0xFFFF;
        asm("  CLRC INTM  ");
} 

void c_int4(void)                                   
{
     asm("  CLRC INTM  ");   
} 

void c_int5(void)                                   
{
    if(PIVR&0x0006) 
    {
         if((uartcount==0)&&((SCIRXBUF==' ')||(SCIRXBUF==10)||(SCIRXBUF==13)))
         {                      
            return;
         }
        else  if(SCIRXBUF==8)/*******退格*/
        {
            if(uartcount>0) data_rx[(--uartcount)]=0;
            return;
        }
        else
        {
           data_rx[uartcount]=SCIRXBUF;   /*数据接收，存至Data数组中*/
           if(data_rx[uartcount++]==10)  /*回车符13*/
           {
              uartcount=0;
              uartflag=1;             
           }
        }
    } 
     IFR=0x0010;
     asm("  CLRC INTM  ");                         /*开总中断*/  
} 

void uartchuli(char * str)
{
    float pwm_zkb=0;
    if(str[0]!='L'||str[1]!='E'||str[2]!='G'||str[4]<'1'||str[4]>'4') 
    {
         Tdata("error:LEG");return;
    }
    else if(str[6]!='J'||str[7]!='O'||str[8]!='I'||str[9]!='N'||str[10]!='T'||str[12]<'1'||str[12]>'4')
    {
         Tdata("error:JOINT"); return;
    }
    else 
    {
         if((str[14]='P')&&(str[15]='W')&&(str[16]='M')) 
         {
            pwm_zkb=atoff(str+18); 
            if(pwm_zkb==999){Tdata("error:数字不规范");return;}
            pwm_cmpr=(int)TxPR*(pwm_zkb/(1+pwm_zkb));Tdata((char *)c_prompt);  
            if(str[12]=='1') {CMPR1=pwm_cmpr;Tfdata(1.0,0);}     /*对应PWM1,2*/ 
            if(str[12]=='2') {CMPR2=pwm_cmpr;Tfdata(3.0,0);}    /*对应PWM3,4*/
            if(str[12]=='3') {CMPR3=pwm_cmpr;Tfdata(5.0,0);}    /*对应PWM5,6*/
            if(str[12]=='4') {CMPR4=pwm_cmpr;Tfdata(7.0,0);}     /*对应PWM7,8*/
            Tdata("比较寄存器：" );Tfdata((float)pwm_cmpr,0);Tdata(c_end);
         }
         else
         {
            Tdata("第三个参数错误");Tdata(str);return;
         }
    }   
}


void varinit(void)
{
        FLAG=0;
        SAMPLE=0;  
        ledcount=0;
        i=0;
        j=0;
        t=0;
        m=0;
        n=30; 
        uartflag=0;
        uartcount=0;
        period_us=50; 
        TxPR=CLOCK*period_us/16;
        
}

void cal(void)
{/*i=CAP3_cnt;
 CAP3_cnt=0; */
}

 
void initPID(void)
{
        int j;
        for(j=0;j<2;j++)
        {
        	
                control[j].saturated=0;
                control[j].phase=1;
                control[j].move=1;
                control[j].count=0;
                control[j].overflow=0;
                control[j].Kp=80;
                control[j].Ki=0;
                control[j].Kd=20;
                control[j].PID_MAX=38;
                control[j].PID_MIN=-38;
                control[j].v=0;
                control[j].mv=0;
                control[j].vlimit=4;
                control[j].e0=0;
                control[j].e1=0;
                control[j].de=0;
                control[j].e_sum=0;
                control[j].PID_OUT=0;
                control[j].DUTYCYCLE=50;
                control[j].mposition=0;
                control[j].position=0;
                control[j].pdist=4320;
                control[j].even_cnt=0;