space.c

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

/*2,3,01位置控制，工作空间展示，正反转只需改动pid->position-=pid->v，pid->mposition=(pid->count+65536*(pid->overflow)-1000)的65536的加号*/

#include"regs2407.h"
#include"math.h"
/*#include"stdio.h"*/
#define accel 1 
#define CLOCK 24
#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 pwm_cmpr,period_us; 
int PREGIST;
const char c_prompt[]="已设定PWM"; 
int ledcount; 
int CAN_FLAG; 
int cantemp; 
int ultrasonicflag; 
int distance[2];    
int distcount; 
int distflag;


long CAP3_cnt,CAP6_cnt;
int FLAG,SAMPLE,motor_num,j,m,n,t,b,c,d,e,f,g;
unsigned int a01[8]={25,30,35,40,50,58,68,75};
unsigned int a23[8]={6,7,8,10,12,14,17,20};


typedef struct
{
	 unsigned int saturated;
         unsigned int phase;
         unsigned int move;  
         unsigned int dir;
         unsigned int count;
         unsigned int overflow;
       /*unsigned int Kp;
         unsigned int Ki;
         unsigned int Kd; */
         int MAX;
         int MIN;
         int DUTYCYCLE;
         int v;
         int vlimit;
         long e0;
         long e1;
         long de; 
         /*long e_sum;*/
         long OUT;
         long mposition;
         long position;
         long pdist;
         long even_cnt;
}PID;
PID control[4];
            

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 UpdatePosition0(PID*pid);
void UpdatePosition1(PID*pid);
void UpdateTrajectory0(PID*pid);
void UpdateTrajectory1(PID*pid);
void calcPID0(PID*pid,int k);
void calcPID1(PID*pid,int k);
void delay(unsigned int t);

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周期中断50us*/
{
        FLAG++;
        if(FLAG==4)                                 /*FLAG对T3PINT进行计数，当FLAG=20时1ms，进行采样*/
        {
        	SAMPLE=1;
        } 
        EVBIFRA=0xFFFF;
        asm("  CLRC INTM  ");
} 

void c_int4(void)                 
{
        if((PIVR&0x0035)==PIVR)                       /*CAP3中断*/
        {
                EVAIFRC=0xFFFF;                       /*清CAP3中断标志位*/
                if((PCDATDIR&0x0004)==0)              /*如果IOPC2输入为低，表明电机正转*/
                {
                	CAP3_cnt=CAP3_cnt+4;                   /*正转，CAP3_cnt加1*/                     
                }           
                else                                  /*如果IOPC2输入为高，表明电机反转*/
                {
                	CAP3_cnt=CAP3_cnt-4;                   /*反转，CAP3_cnt减1*/
                }    
        }
        
        else if((PIVR&0x0038)==PIVR)                  /*CAP6中断*/
        {
                EVBIFRC=0xFFFF;                       /*清CAP6中断标志位*/
  	        if((PCDATDIR&0x0008)==0)              /*如果IOPC3输入为低，表明电机正转*/
                {
                	CAP6_cnt=CAP6_cnt+4;                   /*正转，CAP6_cnt加1*/
                }            
                else                                  /*如果IOPC3输入为高，表明电机反转*/
                {
                	CAP6_cnt=CAP6_cnt-4;                   /*反转，CAP6_cnt减1*/
                }
        }	
 
        asm("  CLRC INTM  ");                         /*开总中断*/
 
} 
   
void c_int5(void)                                   
{
    switch(PIVR) 
    { 
      case 6 :     /*串口中断*/
      {
         if(ultrasonicflag==0)
         {
             if(SCIRXBUF==0xaa)
             { 
                ultrasonicflag=1;
             }
             else 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;             
                }
             }
         }
         else
         {
             distance[distcount]=SCIRXBUF;
             distcount++;
             if(distcount==2) 
             {
                distcount=0;
                distflag=1;
             }
         }
         break; 
      }   
      case 64:      /*can中断*/
      { 
          CANRCR=0X040;/*复位RMP2:接收信息标志,MIF2*/ 
          CAN_FLAG=1;/*置用户接收标志*/
          break;
      }
    } 
     IFR=0x0010;
     asm("  CLRC INTM  ");                         /*开总中断*/  
}