?? cdle-j20_main.c
字號:
#include <AT89X51.h>
#include <math.h>
//#define DX 9
//static unsigned char data CN[4];
static unsigned char data DX=11;
static unsigned char data DELAYTIME=4;
static unsigned char data ii,k; /////////////k
static unsigned int data i;//,j;
static unsigned char data j;
//static unsigned char idata j,k;//,motor;
//static unsigned char idata dl,temp,de;
//static unsigned int Count;
//static unsigned int ComCount[TimerNum]; ////////////計數(shù)終值
//static unsigned int ComCount_[TimerNum]; ////////////與終值的個數(shù)
//static unsigned char idata ComNumber[TimerNum];
//static unsigned char idata Number;
//static unsigned char data CloseCount[TimerNum]; ///////// 關斷計數(shù)器
///////////////////遙控//////////////////////////////////////
static unsigned char data IRCode[3],IRCON,IRCON2;
//static unsigned char data DT;
//static unsigned char data flag; /////////// 0 對應串口 1 遙控
//static unsigned char data GetCode;
unsigned int a_;
static unsigned char data Fasheng_Flag;
static float idata X0,Y0; /////////// chushi X Y
static float data K=0.0;
static float idata X,Y,DL1_,DL2_,tempDL1,tempDL2;
static unsigned int data Step;
//static unsigned char data cof=6;
static float data Coef=0.33;//=3.1415927/cof;
static unsigned char idata KEY=0;
bit keytimes=0;
bit Button=0;
static unsigned char idata X0temp=0,Y0temp=0;
static unsigned char idata Disp[10]={0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f};///0-9
static unsigned char idata Shangshuo=0;
static unsigned char data g,s,b,q;
static unsigned char idata StepMoter[8]={0xF5,0xF1,0xF9,0xF8,0xFA,0xF2,0xF6,0xF4}; ///8 Steps; ---->
static unsigned char idata n1,n2;
static unsigned char idata mm=0;
//bit rightorleft=0; ///////1 right 0 left
//bit WhoDa=0; ///// 1 DL1_ 0 DL2_
static unsigned char idata ThePos1=0,ThePos2=0;
//P0_0---------P0_3 右邊
//P2_0---------P2_3 左
/////////////////////////////////////////////////////////////////////////////
//unsigned char TS[8] = {254,252,248,240,224,192,128,0};
void Delay()
{
for(k=0;k<DELAYTIME;k++)
for(a_=0;a_<400;a_++)
;
}
void GetDL12()
{
DL1_=((X-950)+(Y-1150)*K)/sqrt((950-X)*(950-X)+(1150-Y)*(1150-Y))*DX;///右電機
DL2_=((X+150)+(Y-1150)*K)/sqrt((X+150)*(X+150)+(1150-Y)*(1150-Y))*DX;////左電機
}
void DriveMoter(/*float DL1_,float DL2_*/) ////DL1 是右邊
{
//Coef=3.1415927/cof;
ok:
n1=abs(DL1_)/Coef; ///右電機轉(zhuǎn)動的相數(shù)；
n2=abs(DL2_)/Coef; ////左電機轉(zhuǎn)動的相數(shù)
if(DL1_>0)
tempDL1+=(DL1_-n1*Coef);
else
tempDL1+=DL1_+n1*Coef;
if(DL2_>0)
tempDL2+=(DL2_-n2*Coef);
else
tempDL2+=DL2_+n2*Coef;
/*
if(n1<10) ////////////// 3mm buneng tongguo!
{
//n1=0;
tempDL1+=DL1_;
n1=abs(tempDL1)/Coef;
if(n1>10)
{
DL1_=tempDL1;
tempDL1=0;
}
else
n1=0;
}
if(n2<10)
{
//n1=0;
tempDL2+=DL2_;
n2=abs(tempDL2)/Coef;
if(n2>10)
{
DL2_=tempDL2;
tempDL2=0;
}
else
n2=0;
}
*/
while(mm<n1||mm<n2)
{
mm++;
if(mm<=n1)
{
if(DL1_>0) P1=StepMoter[(mm+ThePos1)%8];
else P1=StepMoter[(8-mm%8+ThePos1)%8]; // mm%4
}
if(mm<=n2)
{
if(DL2_>0) P2=StepMoter[(mm+ThePos2)%8];
else P2=StepMoter[(8-mm%8+ThePos2)%8];
}
Delay();
}
if(n1)
{if(DL1_>0) ThePos1=(n1+ThePos1)%8;else ThePos1=(8-n1%8+ThePos1)%8;}
if(n2)
{if(DL2_>0) ThePos2=(n2+ThePos2)%8;else ThePos2=(8-n2%8+ThePos2)%8;}
mm=0;
if(abs(tempDL1)>3||abs(tempDL2)>3)
{
DL1_=tempDL1;
DL2_=tempDL2;
tempDL1=0;
tempDL2=0;
goto ok;
}
}
void main(void)
{
X0=400;
Y0=400; //////////////初始 cm
X=0;
Y=0;
// DL1_=0.0;
// DL2_=0.0;
Fasheng_Flag=0;
TMOD=0x11; //////都是方式1
EA = 1;
//ES = 1; //開串口中斷
ET0 = 1; /////// T0中斷
TR0 = 0; //啟動0定時器
ET1=1;
TL1 = 0x77; /////// 定時5ms
TH1 = 0xEC;
// EX1=1;
EX0 = 1; ////////////////////////外部0中斷
q=X0/100;
b=X0/10-q*10;
s=Y0/100;
g=Y0/10-s*10;
X0temp=X0/10;
Y0temp=Y0/10;
P0=Disp[0];
P3=0xFF;
// ThePos1=0;ThePos2=0;
// P1=StepMoter[0];////////////初相
// P2=StepMoter[0];
PT0=1; ////// T0 gao you xian ji!
TR1=1;
do
{
// Fasheng();
if(Fasheng_Flag==10)
{
KEY++; //// KEY=1 要求輸入第一組數(shù)據(jù) X0
if(KEY==2) //KEY=2 確認第一組，要求輸入第二組數(shù)據(jù) Y0
{
X0=X0temp*10.0;
q=X0/100;
b=X0/10-q*10;
}
else
if(KEY==3)
{
Y0=Y0temp*10.0;
s=Y0/100;
g=Y0/10-s*10;
KEY=0;
}
///KEY=3 確認第二組Y0
Fasheng_Flag=0;
//KEY=3;
}
if(Fasheng_Flag==1&&!KEY) ///////////走直線
{
ThePos1=0;ThePos2=0;
P1=StepMoter[0];////////////初相
P2=StepMoter[0];
tempDL1=0.0;
tempDL2=0.0;
if(X0) ///////////////X0!=0)
{
K=Y0/X0;
Step=X0/DX;
for(i=0;i<=Step;i++)
{
X=i*DX;
Y=K*X;
if(X>800.0||Y>1000.0)
{
break;
}
GetDL12();
//左右驅(qū)動電機 分別到達 DL1 DL2的距離
DriveMoter();
q=X/100;
b=X/10-q*10; /////////X0
s=Y/100; //cm
g=Y/10-s*10;
// Delay();
}
}
else if(Y0)
{
Step=Y0/DX;
for(i=0;i<=Step;i++)
{
//X=i*DX;
Y=i*DX;
X=0;
DL1_=(Y-1150)/sqrt((950-X)*(950-X)+(1150-Y)*(1150-Y))*DX;///右電機
DL2_=(Y-1150)/sqrt((X+150)*(X+150)+(1150-Y)*(1150-Y))*DX;////左電機
//左右驅(qū)動電機 分別到達 DL1 DL2的距離
DriveMoter();
q=0;
b=0; /////////X0
s=Y/100; //cm
g=Y/10-s*10;
// Delay();
}
}
Fasheng_Flag=0;
}
if(Fasheng_Flag==2&&!KEY) ///////////走圓
{
if(X0<250||X0>800-250||Y0<250||Y0>1000-250)
goto Quit;
ThePos1=0;ThePos2=0;
P1=StepMoter[0];////////////初相
P2=StepMoter[0];
tempDL1=0.0;
tempDL2=0.0;
/*
Step=(X0-250)/DX; ////走初始直線;
K=Y0/(X0-250);
for(i=0;i<=Step;i++)
{
X=0+i*DX;
Y=K*X;
DL1_=((X-950)+(Y-1150)*K)/sqrt((950-X)*(950-X)+(1150-Y)*(1150-Y))*DX;
DL2_=((X+150)+(Y-1150)*K)/sqrt((X+150)*(X+150)+(1150-Y)*(1150-Y))*DX;
//左右驅(qū)動電機 分別到達 DL1 DL2的距離
DriveMoter();
q=X/100;
b=X/10-q*10; /////////X0
s=Y/100; //cm
g=Y/10-s*10; // Delay();
}
*/
Step=500/DX; /////////走上半圓
DL1_=(Y0-1150)/sqrt((950-X0)*(950-X0)+(1150-Y0)*(1150-Y0))*DX;
DL2_=(Y0-1150)/sqrt((X0+150)*(X0+150)+(1150-Y0)*(1150-Y0))*DX;
DriveMoter(); ////////////////// 1 bu!
for(i=1;i<=Step;i++)
{
X=X0-250+i*DX;
Y=Y0+sqrt(250.0*250.0-(X-X0)*(X-X0));
//K=(X0-X)/sqrt(250.0*250.0-(X-X0)*(X-X0));
K=(X0-X)/(Y-Y0);//sqrt(250.0*250.0-(X-X0)*(X-X0));
GetDL12();
//左右驅(qū)動電機 分別到達 DL1 DL2的距離
DriveMoter();
q=X/100;
b=X/10-q*10; /////////X0
s=Y/100; //cm
g=Y/10-s*10; // Delay(); /////////////////////////////
}
//Step=500/DX; /////////////走下半圓
for(i=1;i<=Step;i++)
{
X=X0+250-i*DX;
Y=Y0-sqrt(250.0*250.0-(X-X0)*(X-X0));
K=(-X0+X)/(Y0-Y);//sqrt(250.0*250.0-(X-X0)*(X-X0));
GetDL12();
//左右驅(qū)動電機 分別到達 DL1 DL2的距離
DL1_=-DL1_;
DL2_=-DL2_;
DriveMoter();
q=X/100;
b=X/10-q*10; /////////X0
s=Y/100; //cm
g=Y/10-s*10; //Delay(); /////////////////////////////
}
Quit:;
Fasheng_Flag=0;
if(Fasheng_Flag==3&&!KEY) ///+++++
{ ////////////////////////////// /p1.6
tempDL1=0.0;
tempDL2=0.0;
///p1.5 p1.7////
X=X0;
Y=Y0;
if(P1_5|P1_6|P1_7) /////////// nei wei 1 wai wei 0;
{
K=0.5773; //30
DX=10;
if(P1_6&P1_7)
{
K=0.5773; //30
DX=10;
}
else if(!P1_6&!P1_7)
{
K=0.5773; //30
DX=-10;
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -