main.c

用电键控制步进电机朝各个方向做曲线

#include "reg52.h"
#include "intrins.h"
#include "math.h"
#include <absacc.h>
#include "VIIC_C51.h"
#include "ZLG7290.h"

//端口定义
sbit KEY_INT=P3^2;
sbit PA1=P2^0;
sbit PB1=P2^1;
sbit PC1=P2^2;
sbit PA2=P2^4;
sbit PB2=P2^5;
sbit PC2=P2^6;

//For Tlc2543
sbit CLK = P2^0;
sbit DIN = P2^1;
sbit DOUT = P2^2;
sbit CS = P2^3;

#define Pi 3.141593
#define step 330./1500				//单步距离,mm
#define pull 0
#define push 1
#define c 1100.					//两滑轮间距，mm
#define dx 150.					//滑轮到计数点原点投影距离,mm
#define dy 150.
#define minline 100.				//最短直线分段
#define motordelay 2			//电机步间延时
#define gate 0x1ff				//传感器门槛
#define searth 35				//寻迹步数，35=1cm
//#define trace_ru 1              //向右上寻迹
//#define trace_rd 2              //向右下寻迹
//#define trace_lu 3              //向左上寻迹
//#define trace_ld 4              //向左下寻迹

extern unsigned char ZLG7290_GetKey();
void Display(unsigned char  *sd);	//显示程序
void DelayX10ms(int count); 		//Delay time=count*10ms
void Delay(unsigned char no);
void RunA(long num,bit dir);		//输入转动的步数、方向，完成电机1转动（相对3相电机） 
void RunB(long num,bit dir);		//输入转动的步数、方向，完成电机1转动（相对3相电机） 
float Pointab_A(float x,float y);	//坐标变换xy-a
float Pointab_B(float x,float y);	//坐标变换xy-b
float Pointxy_X(float a,float b);	//坐标变换xy-a
float Pointxy_Y(float a,float b);	//坐标变换xy-b
void Circle(float x,float y,float r);		//画圆程序
void Stepto(float x, float y);
void Lineto(float x,float y);
unsigned int ad549(void);
unsigned int Trace(void);
void Mypath (void);
void Go_ru(void); //右上
void Go_lu(void); //左上
void Go_rd(void); //右下
void Go_ld(void); //左下

int flag_trace;                  //0:连续； 1：断开

//全局变量
bit dir1,dir2;						//电机方向pull为收，push为放
char sta1,sta2;
unsigned char key;
unsigned char send[8]={00,00,00,00,00,00,00,00};//定义发送缓冲区 0~3--y 4~6--x 7--sign
float alast,blast,xlast,ylast,xkey,ykey;     
unsigned char disp_buf[8]={0,0,0,0,0,0,0,0};
						

void main()
{
	int test;
	//电机a初始化	
	PA1=0; 
	PB1=1;
	PC1=1;
	sta1=0;

	//电机b初始化
	PA2=0; 
	PB2=1; 
	PC2=1;
	sta2=0;
	//RunA(1500,pull);
	//DelayX10ms(1000);
	//RunB(1500,pull);
	//Delay(2);
	//当前点，以la、lb步数为坐标记，拉减放加
/*
	while (1)
	{
		while(KEY_INT);
		key=ZLG7290_GetKey();
		ykey=send[0]+send[1]*10+send[2]*100+send[3]*1000;
		xkey=send[4]+send[5]*10+send[6]*100;
		switch(key)
		{
			case 1 :send[0]++;	//ymm位	
			if(send[0]==10)
			{
		 		send[0]=00;
			}	
			break; 
			case 2 :send[1]++;//ycm位 
			if(send[1]==10)
			{
				send[1]=00;
			}
			break;
			case 3 :send[2]++;//ydm位
			if(send[2]==10)
			{
				send[2]=00;
			}
			break;
			case 4 :send[3]++;//ym位
			if(send[3]==2)
			{
				send[3]=00;
			}break; 
			case 5 :send[4]++;//xmm位
			if(send[4]==10)
			{
				send[4]=00;
			}
			break;
			case 6 :send[5]++;//xcm位	
			if(send[5]==10)
			{
				send[5]=00;
			}
			break;
			case 7 :send[6]++;//xdm位	
			if(send[6]==8)
			{
				send[6]=00;
			}
			break;
			case 8 :send[7]=1;Lineto(xkey,ykey);send[7]=0; break;
			case 9 :           //输入当前坐标     
		 		xlast=xkey;
				ylast=ykey;
			break;
			case 10 :RunB(10,pull);	break;
			case 11 :RunB(10,push);	break;
			case 12 :RunA(10,pull);	break;
			case 13 :RunA(10,push);	break;
			case 14 :send[7]=4;Trace();send[7]=0;break;
			case 15 :send[7]=3;Mypath();send[7]=0;break;
			case 16 :send[7]=2;Circle(xkey,ykey,250);send[7]=0;break;
		}
		Delay(100);
		Display(send);
		//if (key!=9) continue;//等待按键，输入坐标
		//else break;
}
ylast=send[0]+send[1]*10+send[2]*100+send[3]*1000;
xlast=send[4]+send[5]*10+send[6]*100;
//*/

xlast=400;
ylast=500;
alast=Pointab_A(xlast,ylast);
blast=Pointab_B(xlast,ylast);

Stepto(0,0);
Display(send);
DelayX10ms(1000);
//RunB(2500,1);
//RunB(2500,0);
DelayX10ms(1000);

for(test=0;test<100;test++)
{RunA(25,1);Delay(10);}
RunA(2500,0);
Delay(2000);


Circle(400,500,250);
DelayX10ms(1000);
Mypath();
Trace();
while(1);
}

void Display(unsigned char  *sd)//显示程序
{ 
    disp_buf[0] = sd[0];			
    disp_buf[1] = sd[1];		
    disp_buf[2] = sd[2];   
    disp_buf[3] = sd[3];
    disp_buf[4] = sd[4];
    disp_buf[5] = sd[5];
    disp_buf[6] = sd[6];	
    disp_buf[7] = sd[7];
    ZLG7290_SendBuf(disp_buf,8);
}

void DelayX10ms(int count) //Delay time=count*10ms
{
	unsigned int l,m,n;

	for(l=0;l<count;l++)
		for(m=0;m<12;m++)
			for(n=0;n<120;n++);
}

void Delay(unsigned char no)
{ 
	int  i,j;					//延时参数
  	for(; no>0; no--)
  	{ 
		for(i=0; i<10; i++)
     		for(j=0; j<100; j++)
			    _nop_();
  	}
}

void RunA(long num,bit dir)//输入转动的步数、方向，完成电机1转动（相对3相电机） 
{
	long i; 
	if(dir)
	{
		for(i=0;i<=num;i++)
		{ 
		 	sta1++;
		 	sta1=sta1%6;
			Delay(motordelay);
			if (sta1==0)PC1=1;
 			if (sta1==1)PB1=0;
			if (sta1==2)PA1=1;
			if (sta1==3)PC1=0;
			if (sta1==4)PB1=1;
			if (sta1==5)PA1=0;
		}
	}
	else
	  {
	   for(i=0;i<=num;i++)
		{
		sta1--;
		if (sta1<0)sta1=5;
		 Delay(motordelay);
			if (sta1==0)PB1=1;
 			if (sta1==1)PA1=0;
			if (sta1==2)PC1=1;
			if (sta1==3)PB1=0;
			if (sta1==4)PA1=1;
			if (sta1==5)PC1=0;
			_nop_();
		}
	  }
}

void RunB(long num,bit dir)//输入转动的步数、方向，完成电机1转动（相对3相电机） 
{   
	long i; 
	if(~dir)
	{
		for(i=0;i<=num;i++)
		
		{
		 	sta2++;
		 	sta2=sta2%6;
			Delay(motordelay);
			if (sta2==0)PC2=1;
 			if (sta2==1)PB2=0;
			if (sta2==2)PA2=1;
			if (sta2==3)PC2=0;
			if (sta2==4)PB2=1;
			if (sta2==5)PA2=0;
		}
	}
	else
	  {
	   for(i=0;i<=num;i++)
	   
		{
		sta2--;
		if (sta2<0)sta2=5;
		Delay(motordelay);
			if (sta2==0)PB2=1;
 			if (sta2==1)PA2=0;
			if (sta2==2)PC2=1;
			if (sta2==3)PB2=0;
			if (sta2==4)PA2=1;
			if (sta2==5)PC2=0;
		}
	  }
}

float Pointab_A(float x,float y)//变量转换xy-a
{
	return(sqrt((1150-y)*(1150-y)+(x+150)*(x+150)));
}

float Pointab_B(float x,float y) //变量转换xy-b
{
	return(sqrt((1150-y)*(1150-y)+(950-x)*(950-x)));
}

float Pointxy_X(float a,float b)//变量转换ab-x
{
	return((a*a+c*c-b*b)/2/c-150);
}

float Pointxy_Y(float a,float b) //变量转换ab-y
{
	return(1000-sin(acos((a*a+c*c-b*b)/2/a/c))*a+150);
}

void Lineto(float x, float y)
{
	long i,j;
	float xstep,ystep,deltla,deltlb,lastx,lasty;
	deltla=Pointab_A(x,y)-alast;
	deltlb=Pointab_B(x,y)-blast;
	if(abs(deltla)>abs(deltlb))j=(long)(abs(deltla)/minline+0.5);
	else j=(long)(abs(deltlb)/minline+0.5);
	xstep=(x-xlast)/j;
	ystep=(y-ylast)/j;
	lastx=xlast;
	lasty=ylast;
	for(i=0;i<j;i++)Stepto(lastx+i*xstep,lasty+i*ystep);
}

void Stepto(float x, float y)
{
	float anew,bnew;
	anew=Pointab_A(x,y);
	bnew=Pointab_B(x,y);
	RunA(anew>alast?(long)(anew-alast+0.5):(long)(alast-anew+0.5),anew>alast?pull:push);
 	RunB(bnew>blast?(long)(bnew-blast+0.5):(long)(blast-bnew+0.5),bnew>blast?pull:push);
	alast=alast+(float)((long)(anew-alast+((anew-alast)>0?0.5:-0.5)));
	blast=blast+(float)((long)(bnew-blast+((bnew-blast)>0?0.5:-0.5)));
	xlast=Pointxy_X(alast,blast);
	ylast=Pointxy_Y(alast,blast);
	send[0]=(long)ylast%10;
	send[1]=(long)ylast/10%10;
	send[2]=(long)ylast/100%10;
	send[3]=(long)ylast/1000%10;
	send[4]=(long)xlast%10;
	send[5]=(long)xlast/10%10;
	send[6]=(long)xlast/100%10;
	//Display(send);
}


void Circle (float x,float y,float r)//画圆程序  
{
	long i,j;
	float C;
	C=Pi*2.*r;
	j=(long)(C/minline);
	for(i=0;i<j;i++) Stepto(x+r*cos(2*Pi*(i+1)/j),y+r*sin(2*Pi*(i+1)/j));
}

//*
void Mypath (void) //将物体直线运动 5mm*200
{
	Lineto(0,0);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(800,800);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(600,1000);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(400,0);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(0,400);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(800,400);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(200,1000);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
	Lineto(0,800);
	if(!KEY_INT)
	{
		key=ZLG7290_GetKey();
		return;
	}
}
//*/
//*
unsigned int Trace (void) //将物体直线运动 5mm*200
{
	//flag_trace=trace_ru;
	Go_ru();
	//if(flag_trace=trace_ru)Go_ru();
	//if(flag_trace=trace_lu)Go_lu();
	//if(flag_trace=trace_rd)Go_rd();
	//if(flag_trace=trace_ld)Go_ld();
	return 0;
}

void Go_ru(void) //右上
{
	int i,i1;
	if(flag_trace<97)return ;
	while(ad549()<gate)RunB(10,pull); //右
	for(i=0;i<searth;i++)while(ad549()>gate)RunA(1,pull);//上
	if(i>=searth) //没找到
	{
		for(i=0;i<searth*2;i++)while(ad549()>gate)RunA(1,push); //下
		if(i>=searth*2) //没找到
		{
			i1=i;
			for(i=0;i<searth*2;i++)RunA(1,pull);	//退回去,上
			flag_trace--;
			Go_lu(); //走左上
		}
		else Go_rd(); //找到，走右下
	}
	else 
	{
		Go_ru();//找到，走右上
		flag_trace=99;
	}
}



void Go_rd(void) //右下
{
	int i,i1;
	//flag_trace=trace_rd;
	if(flag_trace<97)return ;
	while(ad549()<gate)RunB(10,pull); //右
	for(i=0;i<searth;i++)while(ad549()>gate)RunA(1,push);//下
	if(i>=searth) //没找到
	{
		for(i=0;i<searth*2;i++)while(ad549()>gate)RunA(1,pull); //上
		if(i>=searth*2) //没找到
		{
			i1=i;
			for(i=0;i<searth*2;i++)RunA(1,push);	//退回去,下
	        flag_trace--;		
			Go_ld(); //走左下
		}
		else Go_ru(); //找到，走右上
	}
	else 
	{
		Go_rd();//找到，走继续右下
		flag_trace=99;
	}
}

void Go_ld(void) //左下
{
	int i,i1;
	//flag_trace=trace_ld;
	if(flag_trace<97)return ;
	while(ad549()<gate)RunB(10,push); //右左
	for(i=0;i<searth;i++)while(ad549()>gate)RunA(1,push);//下
	if(i>=searth) //没找到
	{
		for(i=0;i<searth*2;i++)while(ad549()>gate)RunA(1,push); //上
		if(i>=searth*2) //没找到
		{
			i1=i;
			for(i=0;i<searth*2;i++)RunA(1,pull);	//退回去,下
			flag_trace--;
			Go_rd(); //走右下
		}
		else Go_ru(); //找到，走右上
	}
	else 
	{
		Go_ld();//找到，走左下
		flag_trace=99;
	}
}

void Go_lu(void) //左上
{
	int i,i1;
	//flag_trace=trace_lu;
	if(flag_trace<97)return ;
	while(ad549()<gate)RunB(10,push); //左
	for(i=0;i<searth;i++)while(ad549()>gate)RunA(1,pull);//上
	if(i>=searth) //没找到
	{
		for(i=0;i<searth*2;i++)while(ad549()>gate)RunA(1,push); //下
		if(i>=searth*2) //没找到
		{
			i1=i;
			for(i=0;i<searth*2;i++)RunA(1,pull);	//退回去,上
			flag_trace--;
			Go_ru(); //走右上
		}
		else Go_rd(); //找到，走
	}
	else 
	{
		Go_lu();//找到，走左上
		flag_trace=99;
	}
}
//*/

unsigned int ad549(void)
{
	unsigned int AD1=0;
	unsigned char i;
	unsigned int AD2=0;
	CS = 1;
	CLK = 0;
	DOUT= 1; // Initialize for Read 
	CS = 0;  // Enable and Drive out Bit9
	for(i=0;i<12;i++)
	{
		AD1 = AD1<<1;
		if( DOUT ) AD1 = AD1|0x0001; 
		DIN=(bit)(AD2&0x8000);
		CLK=1;
		AD2<<=1;
		_nop_();
		_nop_();
		CLK=0; // 
		_nop_();
		_nop_();
	} 
	AD1 = AD1>>1;
	CS = 1;
	for(i=0;i<20;i++); // delay >21us
	return AD1;
}



//*/