?? contmotor.c
字號:
#include "spce061v004.h"
#include "main.h"
#include "math.h"
extern int PositionX,PositionY;
int NFStat = 0; // 當前傳感器狀態(tài)
int NFDir = 0; // 上次移動的方向
float GetL(float, float); // 根據(jù)(x,y)坐標, 計算出左線長度 L
float GetR(float, float); // 根據(jù)(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}; //左電機正轉(zhuǎn)控制時序
unsigned int ctrlR[4] = {0x90,0xc0,0x60,0x30}; //右電機正轉(zhuǎn)控制時序
float PI = 3.1416; //
float Motor_R = 0.54; // 電機的繞線軸半徑, 單位厘米
unsigned int DelayTime=0; // 延時程序的計數(shù)器
unsigned int MotorStepDelay = 5800; // 延時時間常數(shù)
unsigned StepL=0, StepR=0; // 電機步進排計數(shù)器 0/1/2/3
float AL = 116; // 畫筆位于原點的左線長度, 單位厘米
float AR = 150; // 畫筆位于原點的右線長度
float NewL = 115; // 畫筆當前位置的左線長度, 厘米
float NewR = 151; // 畫筆當前位置的右線長度, 厘米
extern int SysStatus; // 系統(tǒng)運行狀態(tài)字
// = 1 運行到指定位置 X,Y
// = 2 畫圓
// = 3 跟蹤
extern int OFStat; // 前次傳感器狀態(tài)
extern int NFStat; // 當前傳感器狀態(tài)
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) // 方向沒有全測試, 就要繼續(xù)
{ 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; // 存儲前次檢測及運行結(jié)果
//OFDir = NFDir;
}
//=========================================================================================
//
//=========================================================================================
extern void GetPoint(void)
{
}
//=========================================================================================
// 根據(jù)(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;
}
//=========================================================================================
// 根據(jù)(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 控制左邊電機轉(zhuǎn)動 1 步
// Step > 0 正轉(zhuǎn)， Step < 0 反轉(zhuǎn)
//=========================================================================================
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 控制右邊電機轉(zhuǎn)動 1 步
// Step > 0 正轉(zhuǎn)， Step < 0 反轉(zhuǎn)
//=========================================================================================
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電機的實際運行步數(shù)
//int RunL,RunR; // A/B電機的步數(shù)
float StopL=0.0, StopR=0.0; // A/B電機的暫停步計數(shù)
int DirL=0, DirR =0; // A/B電機的方向 >=0 正向
*P_Watchdog_Clear=1;
LLength = GetL(x1,y1); // 計算出左右線的長度
RLength = GetR(x1,y1);
MoveL = NewL - LLength; // 計算出左右電機應(yīng)當轉(zhuǎn)動長度
MoveR = NewR - RLength;
SL = MoveL / (2 * PI * Motor_R / 200);// 計算出左右電機應(yīng)當轉(zhuǎn)動的步數(shù)
SR = MoveR / (2 * PI * Motor_R / 200);// 電機轉(zhuǎn)動1周, 200步
if (SL >=0) DirL = 1; // 判斷左右電機移動的方向
else DirL = -1;
if (SR >=0) DirR = 1;
else DirR = -1;
SL = fabsf((SL)); // 左右電機移動的總步數(shù)
SR = fabsf((SR));
sRunL = 0; // 左右電機的運行步計數(shù)器
sRunR = 0;
if(SL >= SR) // 左電機的步進長
{ while ( sRunL <= SL) // 以左電機為準移動
{ ControlL(DirL); // 先動左電機
StopR = StopR + fabsf(SR/SL); // 計算并驅(qū)動右電機
if (StopR > 1.0) // 為了平滑運動, 累加運行步少的電機
{ ControlR(DirR);
StopR = StopR - 1;
}
sRunL ++;
Delay(MotorStepDelay);
}
if (StopR > 0.0) ControlR(DirR); // 若右電機還有運行步, 則再走一步
}
else
{ while ( sRunR <= SR) // 以右電機為準移動
{ ControlR(DirR); // 先驅(qū)動右電機
StopL = StopL + fabsf(SL/SR); // 計算并驅(qū)動左電機
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; // 細分段數(shù)
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--;
//}?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -