can.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 ledcount; 
int CAN_FLAG; 
int cantemp;

int FLAG,SAMPLE,i,j,m,t;
 //*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)                                   
{
    switch(PIVR) 
    { 
      case 6 :     /*串口中断*/
      {
         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;             
           }
         }
         break; 
      }   
      case 64:      /*can中断*/
      { 
          CANRCR=0X040;/*复位RMP2:接收信息标志,MIF2*/ 
          CAN_FLAG=1;/*置用户接收标志*/
          break;
      }
    } 
     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') 
    {
        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(c_end);return;
           }
        } 
    }
    else if(str[0]=='c'&&str[1]=='a'&&str[2]=='n'&&str[3]=='o'&&str[4]=='u'&&str[5]=='t') 
    {
        CANMBX3A=0x0201;  /*邮箱3信息初始化*/
        CANMBX3B=0x0403;
        CANMBX3C=0x0605;
        CANMBX3D=0x0807;  
        CANTCR=0x20;/*TRS3=1邮箱3请求发送*/  
        while(CANTCR&0x2000==0) continue; /*等待发送应答TA3=1则发送成功 */   
        CANTCR=0x2000;      /*清TA3\MIF3标志位*/ 
        Tdata("0102030405060708 发送成功");Tdata(c_end);              
    } 
}


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;
        CAN_FLAG=0;
}

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

void caninit()
{
   MCRB=MCRB|0x0C0;  /*设置IOPC6IOPC7为CANRXCANTX*/
   CANIFR=0xFFFF;    /*清所有CAN 中断标志*/
   CANLAM1H=0x7000;   /*设置邮箱2\3的屏蔽寄存器,=1则相应的位不相符也能接受*/
   CANLAM1L=0x0000;    /*0则id必须匹配*/
   CANMCR=0x1000;      /*ccr=1改变配置请求,数据按0-1-2-3-4-5-6-7-8顺序，数据域改变请求禁止*/
   while(CANGSR&0x0010==0)continue;  /*CCE=1即可配置波特率*/
   CANBCR2=0x0003;   /*系统时钟24M,CAN波特率500K*/
   CANBCR1=0x0033;
   CANMCR=CANMCR&0xEFFF;  /*ccr=0改变配置请求结束*/
   while(CANGSR&0x0010!=0)continue;  /*CCE=0改变配置波特率成功*/  
   CANMDER=0x040; /*不使能邮箱,邮箱2配置成接收,3发送*/   
   CANMCR=0x0102;      /*cDr=1数据域改变请求,自测模式，选择邮箱3*/ 
   CANMSGID2H=0x2000; /*设置邮箱2的控制字及ID,IDE=0标准标示符,AME=0禁止屏蔽,标准方式为MSGID2H[12-2]*/
   CANMSGID2L=0x0000;   /*AME=0禁止屏蔽即接收邮箱的标识符必须与被接收邮箱的标识符相符才能接收信息   */
   CANMSGCTRL2=0x08; /*数据长度DCL=8,RTR=0数据帧*/