📄 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]; ////////////计数终值
//static unsigned int ComCount_[TimerNum]; ////////////与终值的个数
//static unsigned char idata ComNumber[TimerNum];
//static unsigned char idata Number;
//static unsigned char data CloseCount[TimerNum]; ///////// 关断计数器
///////////////////遥控//////////////////////////////////////
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; ///右电机转动的相数；
n2=abs(DL2_)/Coef; ////左电机转动的相数
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 要求输入第一组数据 X0
if(KEY==2) //KEY=2 确认第一组，要求输入第二组数据 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();
//左右驱动电机 分别到达 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;////左电机
//左右驱动电机 分别到达 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;
//左右驱动电机 分别到达 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();
//左右驱动电机 分别到达 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();
//左右驱动电机 分别到达 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 + -