?? ctr.c
字號:
//**************************************//
// 源 文 件:Ctr.c //
// 描 述:控制物體作多種運動代碼 //
// 主要功能:定點運動 //
// 圓周運動 //
// 沿黑線運動 //
// 編寫作者:by Gholt(林青) //
// 編寫日期:2005-09-07 //
// 整理日期:2005-10-30 //
//**************************************//
//================================
// IO端口分配:
// L1_IO----IOB0 左光感
// L2_IO----IOB1 上光感
// L3_IO----IOB8 右光感
// L4_IO----IOB10 下光感
// L5_IO----IOB2 中光感
//================================
#include "SPCE061V004.H"
#include "math.h"
#define L1_IO 0x0001
#define L2_IO 0x0002
#define L3_IO 0x0100
#define L4_IO 0x0400
#define L5_IO 0x0004
extern unsigned int DeCnt1,DeCnt2; //電機1、2脈沖延遲計數(shù)變量
extern unsigned int M1_Over,M2_Over; //電機1、2停轉(zhuǎn)標(biāo)志變量
extern unsigned N_step1,N_step2; //電機1、2步進數(shù)(1.8度/步進)
int Min_DC=2000; //電機驅(qū)動脈沖延遲計數(shù)2000（轉(zhuǎn)速較慢）
//畫圓、沿黑線運動時:Min_DC=1000（慢速）
//定點運動時:Min_DC=500（快速）
double C1=6.26,C2=6.24; //繞線軸1、2的周長（cm）
double X_Now,Y_Now; //目前物體位置坐標(biāo)
double X_Set,Y_Set; //設(shè)定物體位置坐標(biāo)
double Now_L1,Now_L2; //目前左、右線長（從物體到滑輪cm）
double Set_L1,Set_L2; //設(shè)定左、右線長
double D_L1,D_L2; //左、右線需要改變的長度
//沿黑線移動控制變量
unsigned int Angl_Now=90; //物體以Angl_Now度角移動
unsigned int D_Ang=30; //每次都以+或-30度改變方向
double R_Run_Back=0.3; //步進距離為0.3
double L=10;
int Out=0; //物體偏離黑線標(biāo)志變量(也是移動方向校正次數(shù))
int ActOver = 0; //要求的移動是否完成標(biāo)志
void F_Delay()
{
int i;
for(i=0;i<0xff;i++)
{
*P_Watchdog_Clear=1;
}
}
//===================================================
//函數(shù):void Revise(double x,double y)
//語法:void Revise(double x,double y)
//描述:校正物體移動后，物體的坐標(biāo)、線長
//參數(shù):物體移動后，目前的坐標(biāo)(x,y)
//返回:無
//===================================================
void Revise(double x,double y)
{
X_Now = x;
Y_Now = y;
L12(x,y,&Now_L1,&Now_L2);
X_Set=x;
Y_Set=y;
}
//=======================================================
//函數(shù):void ChgLine()
//語法:void ChgLine()
//描述:根據(jù)設(shè)定的線長和當(dāng)前線長，改變線長，以達到設(shè)定長度
// 主要通過控制電機正、反旋轉(zhuǎn)實現(xiàn)收、放線
//參數(shù):無
//返回:無
//=======================================================
void ChgLine()
{
double dl1,dl2;
unsigned int angle;
dl1=Now_L1-Set_L1;
dl2=Now_L2-Set_L2;
//根據(jù)兩線長匹配兩電機轉(zhuǎn)速
if(fabs(dl1)>fabs(dl2))
{
DeCnt1=Min_DC;
DeCnt2=fabs(dl1/dl2 * DeCnt1);
}
else
{
DeCnt2=Min_DC;
DeCnt1=fabs(dl2/dl1 * DeCnt2);
}
//控制電機1收放線
if(dl1>0) //1收線
{
angle=dl1/C1 * 360.0; //計算旋轉(zhuǎn)角度
AngleRunA(angle,1); //調(diào)用電機角度旋轉(zhuǎn)函數(shù)
}
else if(dl1<0) //1放線
{
angle=-dl1/C1 * 360.0;
AngleRunA(angle,0);
}
else M1_Over=1; //dl1==0時，電機1停機
//控制電機2收放線
if(dl2>0) //2收線
{
angle=dl2/C2 * 360.0;
AngleRunB(angle,1);
}
else if(dl2<0) //2放線
{
angle=-dl2/C2 * 360.0;
AngleRunB(angle,0);
}
else M2_Over=1;
}
//=======================================================
//函數(shù):void PointGo(double x,double y)
//語法:void PointGo(double x,double y)
//描述:定點運動，移動到(x,y)點
//參數(shù):目標(biāo)點(x,y)
//返回:無
//=======================================================
void PointGo(double x,double y)
{
X_Set=x;
Y_Set=y;
*P_Watchdog_Clear = 1;
L12(x,y,&Set_L1,&Set_L2); //計算目標(biāo)點到兩滑輪的距離
ChgLine();
}
//=======================================================
//函數(shù):void CirculGo(double x,double y,double R)
//語法:void CirculGo(double x,double y,double R)
//描述:以(x,y)為圓心，作半徑為R的圓周運動，從(x+R,y)點開始移動
// 如物體不在(x+R,y)點上，則會先移動到該點，再開始作圓周運動
// [注:由于計算的控制坐標(biāo)和實際物體運動坐標(biāo)存在偏差，
// 所以在函數(shù)中加了偏差校正，簡單說:
//
// 設(shè)定軌跡 實際軌跡
// 校正前: 圓 橢圓
// 校正后: 橢圓 圓
// ]
//參數(shù):圓心(x,y)和半徑R
//返回:無
//=======================================================
void CirculGo(double x,double y,double R) //B_3
{
int i=0,j;
double nextX,nextY,radian,R_revise,L_revise=5.0,cosQ,sinQ;
Min_DC=1000;
PointGo(x+R,y); //先移動到(x+R,y)點
//把圓等分成360幾點，連續(xù)走完各點
while(i<=360)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1) //兩電機都停機，即到了某點
{ //才走下一點
i+=1; //角度加一
//偏差校正
if(i<=180)
{
R_revise=L_revise * i / 180.0;
}
else
{
R_revise=L_revise*(360.0-i)/180.0;
}
//計算下一點坐標(biāo)
Revise(X_Set,Y_Set);
radian=i/180.0 * 3.14159;
sinQ=sin(radian);
cosQ=cos(radian);
nextY=(R+R_revise) * sinQ;
nextX=(R+R_revise) * cosQ;
//調(diào)用坐標(biāo)顯示函數(shù)(實現(xiàn)實時顯示)
DisplayCorr((unsigned int)(R*cosQ+x+0.3),(unsigned int)(R*sinQ+y+0.7));
PointGo(nextX+x,nextY+y); //向目標(biāo)點移動
}
}
ActOver=1; //圓周運動完成
}
//=======================================================
//函數(shù):void Rodam()
//語法:void Rodam()
//描述:作事先設(shè)定好的任意軌跡運動
//參數(shù):無
//返回:無
//=======================================================
void Rodam() //B_2
{
int i=0;
//事先設(shè)定好的坐標(biāo)點:
//1(40.0,30.0) 2(00.0,00.0) 3(40.0,70.0) 4(20.0,70.0)
double Xarr[4]={40.0,0.0,40.0,20.0};
double Yarr[4]={30.0,0.0,70.0,70.0};
Min_DC=500;
while(i<4)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1) //到了某點,才走下一點,分別走完4點
{
Revise(X_Set,Y_Set); //到了某點,要校正新坐標(biāo)
//調(diào)用坐標(biāo)顯示函數(shù)
DisplayCorr((unsigned int)X_Now,(unsigned int)Y_Now);
PointGo(Xarr[i],Yarr[i]);
i++;
}
}
while(M1_Over==0|M2_Over==0)
{
*P_Watchdog_Clear=1;
}
ActOver=1;
}
//=======================================================
//函數(shù):void XoYoStar(double x,double y)
//語法:void XoYoStar(double x,double y)
//描述:從原點(0,0)移動到目標(biāo)點(x,y)
// 如物體不在原點上，則會先移動到原點
//參數(shù):目標(biāo)點(x,y)
//返回:無
//=======================================================
void XoYoStar(double x,double y) //B_4
{
int i=0;
Min_DC=500;
PointGo(0.0,0.0);
while(i<1)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
PointGo(x,y);
i++;
}
}
while(M1_Over==0|M2_Over==0)
{
*P_Watchdog_Clear=1;
}
ActOver=1;
}
//=======================================================
//函數(shù):void AngR(unsigned int angle,double R)
//語法:void AngR(unsigned int angle,double R)
//描述:以angle度為方向，從當(dāng)前位置移動R厘米
//參數(shù):移動方向angle，移動距離R
//返回:無
//=======================================================
void AngR(unsigned int angle,double R)
{
double dx,dy,radian;
radian=angle/180.0 * 3.14159;
dx=R*cos(radian);
dy=R*sin(radian);
PointGo(X_Now+dx,Y_Now+dy);
Angl_Now=angle;
}
//=======================================================
//函數(shù):void L_IO_init()
//語法:void L_IO_init()
//描述:光電傳感器使用端口的初始化(輸入)
// L1_IO----IOB0 左光感
// L2_IO----IOB1 上光感
// L3_IO----IOB8 右光感
// L4_IO----IOB10 下光感
// L5_IO----IOB2 中光感
//參數(shù):無
//返回:無
//=======================================================
void L_IO_init()
{
unsigned temp;
temp=L1_IO | L2_IO | L3_IO | L4_IO | L5_IO;
*P_IOB_Dir &= ~temp;
*P_IOB_Attrib &= ~temp;
*P_IOB_Data &= ~temp;
}
//=======================================================
//函數(shù):int TestL(int Li)
//語法:int TestL(int Li)
//描述:測試光電傳感器Li(上、下、左、右或中)的信號，高有效
//參數(shù):光電傳感器編號Li(1:左 2:上 3:右 4:下 5:中)
//返回:光電傳感器信號(1:有效 2:無效)
//=======================================================
int TestL(int Li)
{
switch(Li)
{
case 1: //左
if(*P_IOB_Data&L1_IO)
return 1;
else
return 0;
case 2: //上
if(*P_IOB_Data&L2_IO)
return 1;
else
return 0;
case 3: //右
if(*P_IOB_Data&L3_IO)
return 1;
else
return 0;
case 4: //下
if(*P_IOB_Data&L4_IO)
return 1;
else
return 0;
case 5: //中
if(*P_IOB_Data&L5_IO)
return 1;
else
return 0;
default:
return 0;
}
}
//==================================================================
//函數(shù):void ForExInt()
//語法:void ForExInt()
//描述:對移出黑線物體的處理，使物體沿黑線移動
// 當(dāng)物體移出黑線時(既中間光感移出黑線)，激發(fā)外部中斷
// 由中斷程序間接調(diào)用ForExInt()，只要out!=0就會調(diào)用該函數(shù)
// [注:原理
// 當(dāng)前移動方向 上 下 左 右
// 感應(yīng)到黑線的光感
// 1左 向左調(diào)整 向左調(diào)整 \ \
// 2上 \ \ 向上調(diào)整 向上調(diào)整
// 3右 向右調(diào)整 向右調(diào)整 \ \
// 4下 \ \ 向下調(diào)整 向下調(diào)整
// ]
//參數(shù):無
//返回:無
//==================================================================
void ForExInt()
{
int AtL,AtR;
if(Angl_Now>=45&&Angl_Now<135) //向上運動時的處理
{
do //如果左右光感信號無效(沒感應(yīng)到黑線)，則以原方向繼續(xù)移動
{ //直到光感信號有效，從而得知偏離方向，為校正方向提供依據(jù)
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
//偏離方向未確定，以原方向繼續(xù)移動
//角度為Angl_Now，距離R_Run_Back
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(1); //檢測左光感信號
AtR=TestL(3); //檢測右光感信號
if(AtL==1) break; //左光感感應(yīng)到黑線(得知右偏)
if(AtR==1) break; //右光感感應(yīng)到黑線(得知左偏)
}while(1);
}
else if(Angl_Now>=135&&Angl_Now<225) //向左運動時的處理(同上)
{
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(4); //檢測下光感信號
AtR=TestL(2); //檢測上光感信號
if(AtL==1) break;
if(AtR==1) break;
}while(1);
}
else if(Angl_Now>=225&&Angl_Now<315) //向下運動
{
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(3); //檢測右光感信號
AtR=TestL(1); //檢測左光感信號
if(AtL==1) break;
if(AtR==1) break;
}while(1);
}
else //向右運動
{
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(2); //檢測上光感信號
AtR=TestL(4); //檢測下光感信號
if(AtL==1) break;
if(AtR==1) break;
}while(1);
}
//移動角度校正
if(AtL==1) //增加30度角
Angl_Now=(Angl_Now+D_Ang)%360;
else //減少30度角
Angl_Now=(360+Angl_Now-D_Ang)%360;
// if(*P_IOB_Data&0x0004)
Out--; //校正控制次數(shù)減一
}
//=======================================================
//函數(shù):void BlkLinR()
//語法:void BlkLinR()
//描述:沿黑線移動入口函數(shù)，主要實現(xiàn)物體的移動(調(diào)用AngR()函數(shù))
// 當(dāng)有外部中斷時(既物體偏離黑線)，負(fù)責(zé)調(diào)用ForExInt()校正
//參數(shù):無
//返回:無
//=======================================================
void BlkLinR()
{
unsigned int temp;
Min_DC=1000;
Init_IRQ3_EXT1();
L_IO_init();
while(1)
{
*P_Watchdog_Clear=1;
if(Out!=0) ForExInt(); //校正
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
// temp=KeyProess();
// if(temp!=0) Ask();
// {
// AngR(Angl_Now,0.0);
// ActOver=1;
// }
}
}
/*
void LineGo(double x,double y)
{
int i=0;
double nextx=X_Now,nexty,k,dx=0.7,DX,DY,temp=L-0.5;
if(x!=X_Now)
k=(Y_Now-y)/(X_Now-x);
if(x<X_Now) dx=-dx;
Min_DC=1500;
while(L>2.0)
{
F_Delay();
if(M1_Over==1&&M2_Over==1)
{
nextx+=dx;
nexty=k*(nextx-x)+y;
DX=nextx-x;
DY=nexty-y;
DX*=DX;
DY*=DY;
L=sqrt(DX+DY);
Revise(X_Set,Y_Set);
PointGo(nextx,nexty);
i++;
}
}
ActOver=1;
}
void LineGo_R(double x,double y)
{
int i=0,n,angl;
double DX,DY,L,DR=0.5;
DX=x-X_Now;
DY=y-Y_Now;
angl=atan2(DX,DY) / 3.141592 * 180;
DX*=DX;
DY*=DY;
L=sqrt(DX+DY);
n=(int)L/DR;
X_Set=X_Now;
Y_Set=Y_Now;
while(1)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(angl,DR);
i++;
}
if(i>n) break;
}
ActOver=1;
}
void BacKLine()
{
int Ang=90,R=0.3;
L_IO_init();
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
if(TestL(5)==0)
{
if(TestL(1)==1&&TestL(2)==0&&TestL(3)==0&&TestL(4)==0)
Ang=180;
if(TestL(1)==0&&TestL(2)==1&&TestL(3)==0&&TestL(4)==0)
Ang=90;
if(TestL(1)==0&&TestL(2)==0&&TestL(3)==1&&TestL(4)==0)
Ang=0;
if(TestL(1)==0&&TestL(2)==0&&TestL(3)==0&&TestL(4)==1)
Ang=270;
}
Revise(X_Set,Y_Set);
AngR(Ang,R);
}
}while(1);
}
*/
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -