contmotor.c

全国电子设计大赛获奖作品 悬挂运动控制系统

#include "spce061v004.h"
#include "main.h"
#include "math.h"

extern int PositionX,PositionY;


int NFStat = 0;                         // 当前传感器状态       
int NFDir = 0;                          // 上次移动的方向 

float  GetL(float, float);                     // 根据(x,y)坐标, 计算出左线长度 L
float  GetR(float, float);                     // 根据(x,y)坐标, 计算出右线长度 R

extern void ControlL(int);                     // 控制A电机
extern void ControlR(int);                     // 控制B电机
extern void MoveTo(float, float);              // 移动到指定 坐标 x,y 单位 厘米
extern void Delay(unsigned int);               // 延时

extern void TBCircle(int, int, int);           // 移动到指定 圆心 x,y,R 单位 厘米

extern void TBFollow(void);                    // 跟踪粗1.5厘米到1.8厘米的黑色曲线

extern void Follow(void);                      // 寻迹算法 
extern void GetPoint(void);                    // 得到最新的检测值

unsigned int ctrlL[4] = {0x03,0x06,0x0c,0x09}; //左电机正转控制时序
unsigned int ctrlR[4] = {0x90,0xc0,0x60,0x30}; //右电机正转控制时序

float PI = 3.1416;                             //  
float Motor_R = 0.54;                          // 电机的绕线轴半径, 单位厘米

unsigned int DelayTime=0;                      // 延时程序的计数器
unsigned int MotorStepDelay = 5800;            // 延时时间常数   

unsigned StepL=0, StepR=0;                     // 电机步进排计数器 0/1/2/3

float AL = 116;                                // 画笔位于原点的左线长度, 单位厘米
float AR = 150;                                // 画笔位于原点的右线长度

float NewL = 115;                              // 画笔当前位置的左线长度, 厘米   
float NewR = 151;                              // 画笔当前位置的右线长度, 厘米   

extern int SysStatus;                          // 系统运行状态字
                                               // = 1 运行到指定位置 X,Y
                                               // = 2 画圆
                                               // = 3 跟踪  
                                                                       
extern int OFStat;                             // 前次传感器状态
extern int NFStat;                             // 当前传感器状态       
extern int OFDir;                              // 上次移动的方向 
extern int NFDir;                              // 新的移动方向

float NewX = 0;                                // 画笔当前的相对坐标, 单位 厘米 
float NewY = 0; 

float OldX = 0;                                // 画笔前次的相对坐标, 单位 厘米 
float OldY = 0;  

//=========================================================================================
//
//=========================================================================================
extern void TBFollow(void)                     // 跟踪粗1.5厘米到1.8厘米的黑色曲线
{ 
  NFStat = 0x01;                               // 低四位是检测信号, 先确定上压线
  NFDir  = 0x0ff;                              // 方向全 1
  while (NFDir)                                // 方向没有全测试, 就要继续  
   { GetPoint();                               // 取最新的坐标点信息
     //NFStat = 0;
     if (NFStat)                               // 回归前次坐标点
      { MoveTo(OldX,OldY);                 
        //NewX = OldX;
        //NewY = OldY;
        continue;  
      }
     else 
      { OldX = NewX;
        OldY = NewY;
        NFDir = 0xFF;    // 向其他8个方向测试
      }
      
     if (NFDir = 0xFF)   // 向右上移动
      {  NFDir = NFDir & 0xFE;  MoveTo(NewX+1, NewY+1);
      }
     if (NFDir = 0xFE)   // 向上移动
      {  NFDir = NFDir & 0xFC;  MoveTo(NewX+0, NewY+1);
      }
     if (NFDir = 0xFC)   // 向左上移动
      {  NFDir = NFDir & 0xFB;  MoveTo(NewX-1, NewY+1);
      }
     if (NFDir = 0xF8)   // 向左移动
      {  NFDir = NFDir & 0xF0;  MoveTo(NewX-1, NewY+0);
      }
     if (NFDir = 0xF0)   // 向左下移动
      {  NFDir = NFDir & 0xE0;  MoveTo(NewX-1, NewY-1);
      }
     if (NFDir = 0xE0)   // 向下移动
      {  NFDir = NFDir & 0xC0;  MoveTo(NewX+0, NewY-1);
      }
     if (NFDir = 0xC0)   // 向右下移动
      {  NFDir = NFDir & 0x80;  MoveTo(NewX+1,NewY+1);
      }
     if (NFDir = 0x80)   // 向右移动
      {  NFDir = NFDir & 0x80;  MoveTo(NewX+0,NewY+1);
      }
   }
  //OFStat = NFStat;                             // 存储前次检测及运行结果
  //OFDir = NFDir;
}

//=========================================================================================
//
//=========================================================================================
extern void GetPoint(void)
{
}

//=========================================================================================
// 根据(x,y)坐标, 计算出左线长度 L
//=========================================================================================
float GetL(float a, float b)          
{  float x;
   x = sqrtf( (a+15) * (a+15) + (115-b) * (115-b));
   *P_Watchdog_Clear=1;
   return x;
}

//=========================================================================================
// 根据(x,y)坐标, 计算出右线长度 R
//=========================================================================================
float GetR(float a,float b)           
{   float y;
    y = sqrtf((95-a) * (95-a) + (115-b) * (115-b));
    *P_Watchdog_Clear=1;
    return y;
}   

//=========================================================================================
// TB Write 控制左边电机转动 1 步
// Step > 0 正转， Step < 0 反转
//=========================================================================================
extern void ControlL(int Step)      
{ if(Step >= 0)                     //电机正传
   { StepL = StepL % 4;
     *P_IOA_Data = *P_IOA_Buffer & 0xfff0 | ctrlL[StepL];//*P_IOA_Data & 0xfff0 | ctrlL[StepL];
     StepL ++;
     *P_Watchdog_Clear=1;
   }
  else 
   { if (StepL < 0) StepL = 3;
     else StepL = StepL % 4;
     *P_IOA_Data = *P_IOA_Buffer & 0xfff0 | ctrlL[StepL];//*P_IOA_Data & 0xfff0 | ctrlL[StepL];
     StepL --;
     *P_Watchdog_Clear=1;
   }
}          

//=========================================================================================
// TB Write 控制右边电机转动 1 步
// Step > 0 正转， Step < 0 反转
//=========================================================================================
extern void ControlR(int Step) 
{ if(Step >= 0)                     //电机正传
   { if (StepR < 0) StepR = 3;
     else StepR = StepR % 4;
     *P_IOA_Data = *P_IOA_Buffer & 0xff0f | ctrlR[StepR];
//     *P_IOA_Data = *P_IOA_Data & 0xff0f | ctrlR[StepR];
     *P_Watchdog_Clear=1;
     StepR --;
   }
  else
   { StepR = StepR % 4; 
     *P_IOA_Data = *P_IOA_Buffer & 0xff0f | ctrlR[StepR];
//     *P_IOA_Data = *P_IOA_Data & 0xff0f | ctrlR[StepR];
     *P_Watchdog_Clear=1;
     StepR ++;
   }           
}          

//=========================================================================================
// 控制画笔从当前位置, 移动的指定坐标点
// 坐标以浮点形式给出, 可以提高精度, 单位厘米
//=========================================================================================
extern void MoveTo(float x1,float y1)    // h = x1, c = y1
{  float  LLength,RLength;               // 左右线的长度
   float  MoveL,MoveR;                   // A/B电机移动量
   float  SL,SR;                         // A/B电机步进 
   float  sRunL,sRunR;                   // A/B电机的实际运行步数 
   //int    RunL,RunR;                     // A/B电机的步数  
   float  StopL=0.0, StopR=0.0;          // A/B电机的暂停步计数
   int    DirL=0, DirR =0;               // A/B电机的方向 >=0 正向
   
   *P_Watchdog_Clear=1;
  
   LLength = GetL(x1,y1);                // 计算出左右线的长度
   RLength = GetR(x1,y1);
  
   MoveL = NewL - LLength;               // 计算出左右电机应当转动长度       
   MoveR = NewR - RLength;
     
   SL = MoveL / (2 * PI * Motor_R / 200);// 计算出左右电机应当转动的步数      
   SR = MoveR / (2 * PI * Motor_R / 200);// 电机转动1周, 200步
   
   if (SL >=0) DirL = 1;                 // 判断左右电机移动的方向 
   else DirL = -1;
   if (SR >=0) DirR = 1; 
   else DirR = -1;
   
   SL = fabsf((SL));                     // 左右电机移动的总步数
   SR = fabsf((SR));
           
   sRunL = 0;                            // 左右电机的运行步计数器  
   sRunR = 0;
     
   if(SL >= SR)                          // 左电机的步进长
    { while ( sRunL <= SL)               // 以左电机为准移动 
      { ControlL(DirL);                  // 先动左电机
        StopR = StopR + fabsf(SR/SL);    // 计算并驱动右电机
        if (StopR > 1.0)                 // 为了平滑运动, 累加运行步少的电机
         { ControlR(DirR); 
           StopR = StopR - 1;
         }
        sRunL ++; 
        Delay(MotorStepDelay);           
      }
     if (StopR > 0.0)  ControlR(DirR);   // 若右电机还有运行步, 则再走一步
    }  
   else
    { while ( sRunR <= SR)               // 以右电机为准移动 
       { ControlR(DirR);                 // 先驱动右电机
         StopL = StopL + fabsf(SL/SR);   // 计算并驱动左电机  
         if (StopL > 1.0) 
           { ControlL(DirL); 
             StopL = StopL - 1;
           }
         sRunR ++; 
         Delay(MotorStepDelay);  
       }
      if (StopL > 0.0)  ControlL(DirL);  // 若左电机还有运行步, 则再走一步       
     }  
    
   // 在此加上纠正偏差的算法 
   if (SysStatus = 2)                       // 画圆时纠正偏差 
   //while ( sRunR <= SR)              
   //   ControlR(DirR);                    // 右电机多走一步
     
   NewL = LLength;                       // 修改左右线的最新长度
   NewR = RLength;  
   
   NewX = x1;                            // 修改画笔的相对坐标值
   NewY = y1;      
} 

//=========================================================================================
// 以x,y为圆心, R为半径画圆
//=========================================================================================
extern void TBCircle(int x,int y, int R)
{  int Angle=0;                          // 360度可以细分为MaxNum段
   int MaxNum = 360;                     // 细分段数
   float Mx,My;                          // 目标坐标
     
   *P_Watchdog_Clear=1;
 
   MoveTo(x * 1.0,y * 1.0);              // 移动到圆心
   
   SysStatus = 2;
//   while ( Angle <= MaxNum)              // 360度   
//    { 
//      Mx = x + (cosf(2 * Angle * PI  / MaxNum) * R);
//      My = y + (sinf(2 * Angle * PI  / MaxNum) * R);
      
//      MoveTo(Mx,My);
//      Angle ++;
//    }
   while ( Angle <= MaxNum)              // 360度   
    { 
      //if (Angle <= (int)(MaxNum/8))      // 0 - 45 度 
       { Mx = x + (cosf(2 * Angle * PI  / MaxNum) * R) ;
         My = y + (sinf(2 * Angle * PI  / MaxNum) * R);
       }      

      MoveTo(Mx,My);
      Angle ++;
//***************      
      PositionX=Mx;PositionY=My;
      PositionDisp();
//****************      
    }
}      

//extern void Delay(unsigned int Times) 
//{  while (Times > 0) Times--;
//}