其他分享
首页 > 其他分享> > 步进电机驱动数控十字滑台直线、圆弧插补(附斜椭圆插补代码实现)

步进电机驱动数控十字滑台直线、圆弧插补(附斜椭圆插补代码实现)

作者:互联网

本人大三,写个博文算是对近期课设的心得体会,供大家学习参考。

效果图:

先介绍一下此次用到的实验设备:

十字滑台,两个步进电机,一个舵机,一块STM32开发板,一支笔。

需要用到的软件是Keil5、CAD。 python和matlab在椭圆插补算法中有用到,只是对算法进行一个验证。

x,y方向的移动通过控制步进电机转向来控制。

我采用的是相对坐标系,个人觉得他更具优势。

具体实验步骤:

1.测定步进电机步距。

 约为800plus/cm,即为80plus/mm。

2.舵机抬起放下角度设定

自行设定,我给的代码中,抬起笔是120°,放下是50°,合适就行。   

3.写移动笔的函数

4.写直线插补函数

5.写圆弧插补函数

6.主程序中加入按键控制入口,各个函数命令如何辨别?

7.轨迹命令区(二维数组比较合适),并定义为全局变量。(写在main里会太小)

 

 具体代码如下:

#include "stdio.h"
#include "stdlib.h" //用到了绝对值
#include "stm32f10x.h"
#include "hal.h"
#include "key.h"
#include "lcd_dis24.h"
#include "delay.h"
#include "motor.h"
#include "pwm.h"
#include "adc.h"
#include "user.h"
#define u16 unsigned int
#define u8  unsigned char
void TIM2_IRQHandler(void);
void MotorXDriver(u16 plus,u8 dir,u16 time);
void MotorYDriver(u16 plus,u8 dir,u16 time);
void Move(int x1,int y1);
void DrawLine(int x1,int y1);
void DrawCricle1(int x0,int y0,int x1,int y1,u16 r,u16 q);//逆圆
void DrawCricle2(int x0,int y0,int x1,int y1,u16 r,u16 q);//顺圆
u8 key_num = 0;
u8 key_last = 0;
u8 time_500us_ok = 0;
u8 time_1s_ok = 0;
u8 time_10ms_ok = 0;
u8 servo = 0,dir = 0;
u16 i=0;
const int fangdayinzi = 8;// 放大因子   80plus/mm
int point[150][7];
//定义要走的路径坐标 100um
	//下标0:1-直线,2逆圆,0-移动笔,3-顺圆
	//下标1:x0,做了修改,默认为0
	//下标2:y0,做了修改,默认为0
	//下标3: x1   //直线插补只需要终点坐标
	//下标4:y1 
	 //下标5:r   //半径 
	 //下标6:q //象限位置
	int point[150][7]={		
	//ze
{1,-22,-4},
{2,-9,7,-11,0,11,2},
{2,-11,0,-7,-8,11,3},
{1,54,-75},
{2,-24,-9,0,-25,25,3},
{2,0,-25,19,-16,25,4},
{2,33,-34,47,0,47,4},
{2,47,0,20,43,47,1},
{1,-63,25},                             //第一点结束
{0,-97,-194},
{1,-10,-3},
{2,-136,-38,-50,-132,141,3},
{2,0,-28,28,0,28,4},
{2,28,0,20,20,28,1},
{2,97,69,0,119,119,1},         // 第二点写完
{0,153,-124},
{1,-106,-201},
{3,129,-82,88,-125,153,4},
{2,-18,21,-27,0,27,2},
{2,-27,0,-25,-11,27,3},
{1,52,-73},
{1,21,-7},
{1,9,32},
{3,-344,0,-336,73,344,2},
{1,65,250},                        //提画完
{0,39,-38},
{2,150,-429,378,-252,454,4},
{1,-131,87},
{3,-36,77,0,85,85,2},
{3,0,85,44,73,85,1},
{1,78,-56},
{1,53,83},
{2,61,-43,75,0,75,4},
{2,75,0,71,23,75,1},
{1,-156,-29},
{3,0,-61,-52,-32,61,3},
{1,55,3},
{1,146,33},
{1,54,21},
{3,0,75,74,13,75,1},
{1,-47,-31},
{1,-93,-141},
{1,159,-89},
{1,52,-17},
{1,133,-31},
{3,133,-355,0,-379,379,4},
{3,0,-379,-82,-370,379,3},
{1,-160,119},
{1,-56,-50},
{3,236,-249,22,-342,343,4},    //又写完
{0,220,87},
{1,5,-120},
{1,-140,-15},
{2,-61,-27,0,-66,66,3},
{1,79,10},
{1,0,-73},
{1,-231,-28},
{1,-12,1},
{2,-54,-16,0,-56,56,3},
{1,197,26},
{1,0,-170},
{2,-277,0,-254,-111,277,3},
{2,290,-133,319,0,319,4},
{1,0,152},
{1,280,13},
{1,9,13},
{2,47,14,0,49,49,1},
{1,-242,-20},
{1,0,78},
{1,121,18},
{2,0,-13,13,0,13,4},
{2,31,0,22,21,31,1},
{1,-18,4},
{1,-108,-16},
{1,0,23},
{3,-89,0,-77,44,89,2},
{2,12,-10,15,0,15,4},
{2,15,0,8,13,15,1},
{1,-40,16},
{1,-28,7},           //共80行
	};

int main(void)
{	
	ChipHalInit();			//片内硬件初始化
	ChipOutHalInit();		//片外硬件初始化
	//Tim8通道1、2为
	SetTim8Pwm(1,0);
	SetTim8Pwm(2,0);
	SetTim8Pwm(3,500);      //TIM8	
	SetTim4Pwm(1,0);
	SetTim4Pwm(2,0);
	SetTim4Pwm(3,0);
	SetTim4Pwm(4,0);
//duoji taiqi 50
	//duoji fangxia 120
	LCD_Display("ADC1:     ADC2:     ADC3:     ADC4:     ADC5:     ADC6:     ADC7:     ADC8:     ADC9:     ADC10:     ",0,16,0,0xffff);
	LCD_Display("DIN1:     DIN2:     DIN3:     DIN4:     DIN5:     DIN6:     DIN7:     DIN8:     DIN9:     DIN10:    DIN11:    DIN12:     ",0,80,0,0xffff);
	LCD_Display("按键:     ",50,176,0,0xffff);
	LCD_Display("电机:  ",50,160,0,0xffff);
	LCD_Display("关",100,160,0xf800,0xffff);
//	LCD_Display("123",100,160,0xf800,0xffff);

	while (1)	  
	{ 
		 time_10ms_ok = 1;
		 if(time_10ms_ok == 1)//10ms执行一次的函数,例如键盘扫描
		{
			time_10ms_ok = 0;
			key_num = key();
			if(key_num != key_last)
			 {
				key_last = key_num;
				if((key_num >= '0' && key_num <= '9') || key_num == '*' || key_num == '#')  //按下0-9或* #
				{
					LCD_write_EN(key_num,100,176,0xf800,0xffff);//直接显示 0-9或者* #键值						
					}
				  if(key_num == '4')
					{
            MotorXDriver(4000,1,100);
					}
					else if(key_num == '2')
          {
						 MotorYDriver(2000,0,100);
					}
					else if(key_num =='6')
          {
						MotorXDriver(4000,0,100);
					}
					else if(key_num == '8')
          {
						MotorYDriver(1500,1,100);
					}
					else if(key_num == '1')
          {
						SetTim4Pwm(1,50);
					}
					else if(key_num == '3')
          {
						SetTim4Pwm(1,120);
					}
				  else if(key_num == '0')
					{
						SetOutPut(11,1);
						SetTim8Pwm(1,120);//抬起笔
						key_num = key();//读取按键
						if(key_num == '0')//按下数字0
						{
							//绘图
							for(i=0;i<80;i++)
							{
								//开始绘制
								if(point[i][0]==1)//1代表直线
								{
			DrawLine((fangdayinzi*point[i][3]),(fangdayinzi*point[i][4]));
								}
								else if(point[i][0]==2)//2代表圆弧(逆圆)
								{
	DrawCricle1((fangdayinzi*point[i][1]),(fangdayinzi*point[i][2]),(fangdayinzi*point[i][3]), (fangdayinzi*point[i][4]), (fangdayinzi*point[i][5]), point[i][6]);
								}
								else if(point[i][0]==3)//3代表圆弧(顺圆)
								{						DrawCricle2((fangdayinzi*point[i][1]),(fangdayinzi*point[i][2]),(fangdayinzi*point[i][3]), (fangdayinzi*point[i][4]), (fangdayinzi*point[i][5]), point[i][6]);
								}
								else if(point[i][0]==0)//0-移动笔
								{		Move((fangdayinzi*point[i][3]),(fangdayinzi*point[i][4]));
								}
							}
							SetTim4Pwm(1,120);
							break;
		        }
					}
					else if(key_num == '*')
					{
						SetOutPut(10,1);
					}
					else if(key_num == '#')
					{
						SetOutPut(12,1);
					}
					servo = 0;
					LCD_Display("关",100,160,0xf800,0xffff);
			 }
				else if(key_num >= 'A' && key_num <= 'D')//按下F1-F4
				{
					//键值为‘A’-‘D’为F1-F4的键值,转换后显示
					switch(key_num)
					{
						case 'A':LCD_Display("F1",100,176,0xf800,0xffff);\
								SetTim4Pwm(1,45);SetTim4Pwm(2,90);\
								SetTim4Pwm(3,135);SetTim4Pwm(4,180);\
								LCD_Display("开ª",100,160,0xf800,0xffff);\
								servo = 1;break;
						case 'B':LCD_Display("F2",100,176,0xf800,0xffff);break;
						case 'C':LCD_Display("F3",100,176,0xf800,0xffff);break;
						case 'D':LCD_Display("F4",100,176,0xf800,0xffff);break;
					}					
				}
				else
				{
					LCD_Display("  ",100,176,0,0xffff);//松开按键后清除键值显示
					if(servo == 0)
						SetOutPut(0,0);
				}				 
			}
	}
  return 0;
}


//TIM2中断函数,500us中断一次,这个函数不能删除,否则会导致烧录失败
void TIM2_IRQHandler(void)
{
	u8 time_counter_1s = 0;
  u8 time_counter_10ms = 0;
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
		time_500us_ok = 1;
		if(++ time_counter_10ms >= 20)
		{
				time_counter_10ms = 0;
				time_10ms_ok = 1;
			if(++ time_counter_1s >= 50)
			{
					time_counter_1s = 0;
					time_1s_ok = 1;
			}
		}
	}
	if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
	{
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
	}
}



//X电机驱动函数,两个电机同向转动
void MotorXDriver(u16 plus,u8 dir,u16 time)
{
	u16 i;
	//方向控制
	if(dir == 1)    //left,1向左
	{
	  DIR1_L();
	  DIR2_L();
	}
	else           
  {
		DIR1_H();
		DIR2_H();
	}
	//脉冲控制
	for(i =0;i<plus;i++)
  {
		PUL1_H();
		PUL2_H();
		delay_us(time);
		PUL1_L();
		PUL2_L();
		delay_us(time);
	}
}
//Y电机驱动函数,两个电机反向转动
void MotorYDriver(u16 plus,u8 dir,u16 time)
{
	u16 i;
	//方向控制
	if(dir == 1)      //down,1代表向下
	{
	  DIR1_L();
		DIR2_H();
	}
	else
  {
		DIR1_H();
		DIR2_L();
	}
	//脉冲控制
	for(i =0;i<plus;i++)
  {
		PUL1_H();
		PUL2_H();
		delay_us(time);
		PUL1_L();
		PUL2_L();
		delay_us(time);
	}
}

//移动笔函数
void Move(int x1,int y1)
{
	//抬起笔
	SetTim4Pwm(1,120);
	delay_ms(500);
	//向x方向移动
	if(x1>=0)
		MotorXDriver(x1,0,500); //移动x1
	else
		MotorXDriver(-x1,1,500);
	//向y方向移动
	if(y1>=0)
		MotorYDriver(y1,0,500);
	else
		MotorYDriver(-y1,1,500);	
      SetTim4Pwm(1,50);
     	delay_s(1);//确保真正放下
}
//画直线函数(可以画水平线和铅垂线)
void DrawLine(int x1,int y1)   //给终点坐标
{
	//定义局部变量
	long Fm = 0;  //定义每走一步的偏差判断
	int x0 = 0, y0= 0; //x0,y0为当前坐标,初始化为0
	u16 number_xy; //定义步数

		//直线插补处理
	if(x1>0&&y1>=0) //终点坐标在第一象限(含x正半轴,不含y正半轴)
	{
		number_xy = (x1+y1);  
		while(1)
		{    //向右走
			if(Fm>=0 && number_xy>0)  //Fm>=0 且没画完, 以步数为跳出条件,是点睛之笔
			{
				Fm -= y1;
				x0 += 1;
				y0 = y0;
				MotorXDriver(1,0,500); //调用电机驱动程序,正转,步进电机走一步,所以我的精度是蛮高的
				number_xy --;
			}	
					// 向上走
			if(Fm<0 && number_xy>0)  //Fm<0且没画完
			{
			   Fm += x1;
				 x0 = x0;
				 y0 += 1;
				 MotorYDriver(1,0,500); //调用电机驱动程序,正转
				 number_xy --;
			}	
			if(number_xy <= 0)   //写完跳出
				break;
			if(x0 == x1 && y0 == y1)//写到了也跳出 
				break;	
		}
	}
				
	if(x1<=0&&y1>0) //终点坐标在第二象限(含y正半轴,不含x负半轴)
	{ 
		x1 = abs(x1);
		number_xy = (x1+y1);  
		while(1)
		{   //向左走
			if(Fm>=0 && number_xy>0)  //Fm>=0且没画完
			{
				Fm -= y1;
				x0 += 1;
				y0 = y0;
			  MotorXDriver(1,1,500); //调用电机驱动程序,反转
				number_xy --;
			}
				// 向上走
			if(Fm<0 && number_xy>0)  //Fm<0且没画完
			{
				 Fm += x1;
				 x0 = x0;
				 y0 += 1;
				 MotorYDriver(1,0,500); //调用电机驱动程序,正转
			   number_xy --;
			}	
			if(number_xy <=0)   //写完跳出
				 break;	
			if(x0 == x1 && y0 == y1) 
					break;		
  	    }
	}
	if(x1<0&&y1<=0) //终点坐标在第三象限(含x负半轴,不含y负半轴)
	{ 
		x1 = abs(x1);
		y1 = abs(y1);
		number_xy = (x1+y1);  
		while(1)
		{   //向左走
			if(Fm>=0 && number_xy>0)  //Fm>=0且没画完
			{
				 Fm -= y1;
				 x0 += 1;
				 y0 = y0;
				 MotorXDriver(1,1,500); //调用电机驱动程序,反转
				 number_xy --;
			}
					//  向下
			if(Fm<0 && number_xy>0)  //Fm<0
			{
				Fm += x1;
				x0 = x0;
				y0 += 1;
			  MotorYDriver(1,1,500); //
				number_xy --;
			}
				if(number_xy <= 0)   //
						break;
				if(x0 == x1 && y0 == y1) 
						break;	
		}
	}
	if(x1>=0&&y1<0) //终点坐标在第四象限(含y负半轴,不含x正半轴)
	{ 
		y1= abs(y1);
		number_xy = (x1+y1);  
		while(1)
		{   //向右走
			if(Fm>=0 && number_xy>0)  //Fm>=0
			{
				Fm -= y1;
				x0 += 1;
				y0 = y0;
			  MotorXDriver(1,0,500); 
				number_xy --;
			}
				// 向上
			if(Fm<0 && number_xy>0)  //Fm<0
			{
				Fm += x1;
				x0 = x0;
				y0 += 1;
				MotorYDriver(1,1,500); 
				number_xy --;
			}
			if(number_xy <=0)   //
						break;
			if(x0 == x1 && y0 == y1) 
						break;
		}
	}
}
//画圆弧函数(逆圆)
void DrawCricle1(int x0,int y0,int x1,int y1,u16 r ,u16 q) // 给起点、终点坐标和半径、象限位置
{
	//定义局部变量
	int x = 0,y = 0; //x,y为当前坐标,初始化为0
	u16 number_xy; //定义步数
	number_xy = (abs(x1-x0)+abs(y1-y0));
	x = abs(x0);     //记录当前坐标的绝对值
	y = abs(y0); 
  long Fm = 0;	
  Fm = x*x+y*y-r*r;  //定义偏差
	if(q == 1)
	{
		while(1) //第一象限
		{    
			//圆弧插补处理,使用逆圆插补
			if(Fm >=0 && number_xy>0)
			{
				 Fm  = Fm - 2*abs(x)+1;
				 x = abs(x)- 1;
				 y = y;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  = Fm + 2*abs(y)+1;
				 x = x;
				 y = abs(y)+1;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	if(q == 2)//第二象限
	{
		while(1) 
		{  
			//圆弧插补处理,使用逆圆插补
			if(Fm >=0 &&number_xy>0)
			{
				 Fm  = Fm - 2*abs(y)+1;
				 x = x;
				 y = abs(y)-1;
				 MotorYDriver(1,1,500); //down
				 number_xy --;
			}
			if(Fm <0 &&number_xy>0)
			{
				 Fm  = Fm + 2*abs(x)+1;
				 x = abs(x)+1;
				 y = y;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	if(q == 3)
	{
		while(1) //第三象限
		{	
			if(Fm >=0 && number_xy>0)
			{
				 Fm  = Fm - 2*abs(x)+1;
				 x = abs(x)-1;
				 y = y;
				 MotorXDriver(1,0,500); 
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  = Fm + 2*abs(y)+1;
				 x = x;
				 y = abs(y)+1;
				 MotorYDriver(1,1,500); 
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	if(q == 4)
	{
		while(1) //第四象限
		{   
			if(Fm >=0 && number_xy>0)
			{
				 Fm  = Fm - 2*abs(y)+1;
				 y = abs(y)-1;
				 x = x;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  = Fm + 2*abs(x)+1;
				 y = y;
				 x = abs(x)+1;
				 MotorXDriver(1,0,500); //right
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
}



//画圆弧函数(顺圆)
void DrawCricle2(int x0,int y0,int x1,int y1,u16 r ,u16 q) // 给起点、终点坐标和半径
{
	//定义局部变量
	int x = 0,y = 0; //x,y为当前坐标,初始化为0
	u16 number_xy; //定义步数
	number_xy = (abs(x1-x0)+abs(y1-y0));
	x = abs(x0);     //记录当前坐标的绝对值
	y = abs(y0); 
  long Fm = 0;	
  Fm = x*x+y*y-r*r;  //定义偏差;
	if(q == 1)
	{
		while(1) //第一象限
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm  = Fm - 2*abs(y)+1;
				 y = abs(y)- 1;
				 x = x;
				 MotorYDriver(1,1,500); //down
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  = Fm + 2*abs(x)+1;
				 y = y;
				 x = abs(x)+1;
				 MotorXDriver(1,0,500); //right
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	if(q == 2)//第二象限
	{
		while(1) 
		{  
			if(Fm >=0 &&number_xy>0)
			{
				 Fm  = Fm - 2*abs(x)+1;
				 y = y;
				 x = abs(x)-1;
				 MotorXDriver(1,0,500); //right
				 number_xy --;
			}
			if(Fm <0 &&number_xy>0)
			{
				 Fm  = Fm + 2*abs(y)+1;
				 y = abs(y)+1;
				 x = x;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	if(q == 3)
	{
		while(1) //第三象限
		{
			if(Fm >=0 && number_xy>0)
			{
				Fm  = Fm - 2*abs(y)+1;
				 y = abs(y)- 1;
				 x = x;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  = Fm + 2*abs(x)+1;
				 y = y;
				 x = abs(x)+1;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	if(q == 4)
	{
		while(1) //第四象限
		{   
			if(Fm >=0 && number_xy>0)
			{
				 Fm  = Fm - 2*abs(x)+1;
				 y = y;
				 x = abs(x)-1;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  = Fm + 2*abs(y)+1;
				 y = abs(y)+1;
				 x = x;
				 MotorYDriver(1,1,500); //down
				 number_xy --;
			}
			if(x == abs(x1) &&y == abs(y1)) 
				break;
			if(number_xy <= 0)   //跳出
				break;
		}
	}
}

椭圆插补找了一篇文献,具体可以参考知网逐点比较法斜椭圆弧插补 - 中国知网

具体代码如下:

椭圆插补
    斜椭圆算法代码:
#define suojian 1000000.0
#define pi 3.14159
//画斜椭圆函数(含标准)逆椭圆
void DrawEllipse(int a,int b,int dushu,u16 q) //给长、短半轴,偏移角度,和part
{ 
    int xe=0;
	int ye=0,xf=0,yf=0,xg=0,yg=0,xh=0,yh=0;	
	int x = 0,y = 0; //x,y为当前坐标,初始化为0
	u16 number_xy = 0; //定义步数
	float sinr = sin((dushu*pi/180));
	float cosr = cos((dushu*pi/180));
	float A= (a*a/suojian*sinr*sinr+b*b/suojian*cosr*cosr);
	float B= (a*a/suojian*cosr*cosr+b*b/suojian*sinr*sinr);
	float C= (2*cosr*sinr*(b*b-a*a))/suojian;
	long D = -a*a/suojian*b*b;   
	float E = C*C-4*A*B;
	float Fm = 0;

	xe =(int)(sqrt((4*B*D/suojian)/(E/suojian)));          
	ye =(int)((-C/(2*B))*sqrt((4*B*D/suojian)/(E/suojian)));   //E点坐标
	
	xf =(int)((-C/(2*A))*sqrt((4*A*D/suojian)/(E/suojian)));  
	yf =(int)(sqrt((4*A*D/suojian)/(E/suojian)));            //F点坐标
	
	xg =(int)(-sqrt((4*B*D/suojian)/(E/suojian)));
	yg =(int)((C/(2*B))*sqrt((4*B*D/suojian)/(E/suojian)));  // G点坐标
	
	xh =(int)((C/(2*A))*sqrt((4*A*D/suojian)/(E/suojian)));  
	yh =(int)(-sqrt((4*A*D/suojian)/(E/suojian)));           //H点坐标
	if(q == 1)
	{ 
		number_xy = (abs(xe-xf)+abs(ye-yf));
		x = xe; y = ye;
		while(1) //first part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm += (-2*A/suojian*x+A/suojian-C*y/suojian);
				 x = x-1;
				 y = y;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}
			if(Fm <0&& number_xy>0)
			{
				 Fm += (2*B/suojian*y+B/suojian+C*x/suojian);
				 x = x;
				 y = y+1;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}			
			if(number_xy <= 0)   //跳出
				break;		
		}
	}
	
	if(q == 2)
	{
		number_xy = (abs(xf-xg)+abs(yg-yf));
		x = xf; y = yf;
		while(1) // second part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm += (-2*B/suojian*y+B/suojian-C*x/suojian);
				 x = x;
				 y = y-1;
				 MotorYDriver(1,1,500); //down
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  +=  (-2*A/suojian*x+A/suojian-C*y/suojian);
				 x = x-1;
				 y = y;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}			

			if(number_xy <= 0)   //跳出
				break;
		}
	}
	
	if(q == 3)
	{
		number_xy = (abs(xh-xg)+abs(yg-yh));
		x = xg; y = yg;
		while(1) //third part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm +=  (2*A/suojian*x+A/suojian+C*y/suojian);
				 x = x+1;
				 y = y;
				 MotorXDriver(1,0,500); 
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm += (-2*B/suojian*y+B/suojian-C*x/suojian);
				 x = x;
				 y = y-1;
				 MotorYDriver(1,1,500); 
				 number_xy --;
			}
	
			if(number_xy <= 0)   //跳出
				break;
		}
	}
	
	if(q == 4)
	{
		number_xy = (abs(xh-xe)+abs(ye-yh));
		x = xh; y = yh;
		while(1) //fourth part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm += (2*B/suojian*y+B/suojian+C*x/suojian);
				 x = x;
				 y = y+1;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  +=  (2*A/suojian*x+A/suojian+C*y/suojian);
				 x = x+1;
				 y = y;
				 MotorXDriver(1,0,500); //right
				 number_xy --;
			}		
			if(number_xy <= 0)   //跳出
				break;	
		}
	}
}



#define suojian 1000000.0
#define pi 3.14159
//画斜椭圆函数(含标准)顺圆
void DrawEllipse1(int a,int b,int dushu,u16 q)//给长、短半轴,偏移角度,和part
{
    int xe=0;
	int ye=0,xf=0,yf=0,xg=0,yg=0,xh=0,yh=0;	
	int x = 0,y = 0; //x,y为当前坐标,初始化为0
	u16 number_xy = 0; //定义步数
	float sinr = sin((dushu*pi/180));
	float cosr = cos((dushu*pi/180));
	float A= (a*a/suojian*sinr*sinr+b*b/suojian*cosr*cosr);
	float B= (a*a/suojian*cosr*cosr+b*b/suojian*sinr*sinr);
	float C= (2*cosr*sinr*(b*b-a*a))/suojian;
	long D = -a*a/suojian*b*b;   
	float E = C*C-4*A*B;
	float Fm = 0;

	xe =(int)(sqrt((4*B*D/suojian)/(E/suojian)));          
	ye =(int)((-C/(2*B))*sqrt((4*B*D/suojian)/(E/suojian)));   //E点坐标
	
	xf =(int)((-C/(2*A))*sqrt((4*A*D/suojian)/(E/suojian)));  
	yf =(int)(sqrt((4*A*D/suojian)/(E/suojian)));            //F点坐标
	
	xg =(int)(-sqrt((4*B*D/suojian)/(E/suojian)));
	yg =(int)((C/(2*B))*sqrt((4*B*D/suojian)/(E/suojian)));  // G点坐标
	
	xh =(int)((C/(2*A))*sqrt((4*A*D/suojian)/(E/suojian)));  
	yh =(int)(-sqrt((4*A*D/suojian)/(E/suojian)));           //H点坐标

	if(q == 1)
	{ 
		number_xy = (abs(xe-xf)+abs(ye-yf));
		x = xf; y = yf;
		while(1) //first part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm += (-2*B/suojian*y+B/suojian-C*x/suojian);
				 x = x;
				 y = y-1;
				 MotorYDriver(1,1,500); //down
				 number_xy --;
			}
			if(Fm <0&& number_xy>0)
			{
				 Fm += (2*A/suojian*x+A/suojian+C*y/suojian);
				 x = x+1;
				 y = y;
				 MotorXDriver(1,0,500); //right
				 number_xy --;
			}			
			if(number_xy <= 0)   //写完跳出
				break;		
		}
	}
	
	if(q == 2)
	{
		number_xy = (abs(xf-xg)+abs(yg-yf));
		x = xg; y = yg;
		while(1) // second part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm += (2*A/suojian*x+A/suojian+C*y/suojian);
				 x = x+1;
				 y = y;
				 MotorXDriver(1,0,500); //right
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  +=  (2*B/suojian*y+B/suojian+C*x/suojian);
				 x = x;
				 y = y+1;
				 MotorYDriver(1,0,500); //up
				 number_xy --;
			}			

			if(number_xy <= 0)   //写完跳出
				break;

		}
	}
	
	if(q == 3)
	{
		number_xy = (abs(xh-xg)+abs(yg-yh));
		x = xh; y = yh;
		while(1) //third part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm +=  (2*B/suojian*y+B/suojian+C*x/suojian);
				 x = x;
				 y = y+1;
				 MotorYDriver(1,0,500); 
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm += (-2*A/suojian*x+A/suojian-C*y/suojian);
				 x = x-1;
				 y = y;
				 MotorXDriver(1,1,500); 
				 number_xy --;
			}
	
			if(number_xy <= 0)   //写完跳出
				break;
		}
	}
	
	if(q == 4)
	{
		number_xy = (abs(xh-xe)+abs(ye-yh));
		x = xe; y = ye;
		while(1) //fourth part
		{    
			if(Fm >=0 && number_xy>0)
			{
				 Fm += (-2*A/suojian*x+A/suojian-C*y/suojian);
				 x = x-1;
				 y = y;
				 MotorXDriver(1,1,500); //left
				 number_xy --;
			}
			if(Fm <0 && number_xy>0)
			{
				 Fm  +=  (-2*B/suojian*y+B/suojian-C*x/suojian);
				 x = x;
				 y = y-1;
				 MotorYDriver(1,1,500); //down
				 number_xy --;
			}		
			if(number_xy <= 0)   //写完跳出
				break;	
		}
	}
}

引入常量"suojian"是为了不让数据过大而导致计算错误。

由于步进电机走的步数必须是整数,因此切点坐标采用了强制转换(int)。

python验证斜椭圆算法

import math

xlist = []
ylist = []
a, b, r, x, y, s, c = 4000, 3200, 45, 0, 0, 0, 0
number_xy = 0
hudu = r * math.pi / 180.0
s = math.sin(hudu)
c = math.cos(hudu)
A = (a ** 2) * (s ** 2) + (b ** 2) * (c ** 2)
B = (a ** 2) * (c ** 2) + (b ** 2) * (s ** 2)
C = 2 * c * s * (b ** 2 - a ** 2)
D = -a * a * b * b
E = C * C - 4 * A * B
Fm = 0
xe = (int)(math.sqrt((4 * B * D) / E))
ye = (int)((-C / (2 * B)) * math.sqrt((4 * B * D) / E))
xf = (int)((-C / (2 * A)) * math.sqrt((4 * A * D) / E))
yf = (int)(math.sqrt((4 * A * D) / E))
xg = (int)(-math.sqrt((4 * B * D) / E))
yg = (int)((C / (2 * B)) * math.sqrt((4 * B * D) / E))
xh = (int)((C / (2 * A)) * math.sqrt((4 * A * D) / E))
yh = (int)(-math.sqrt((4 * A * D) / E))
number_xy = math.fabs(xf - xe) + math.fabs(yf - ye)
x, y = xe, ye
Fm = A * x * x + B * y * y + C * x * y + D
xlist.append(xe)
ylist.append(ye)

while(True):
    if(Fm>=0 and number_xy>0):
        Fm += -2 * A * x + A - C * y
        x = x-1
        y = y
        number_xy -=1
        xlist.append(x)
        ylist.append(y)
    if(Fm<0 and number_xy>0):
        Fm += 2 * B * y + B + C * x
        x = x
        y = y + 1
        number_xy -=1
        xlist.append(x)
        ylist.append(y)
    if(x == xf and y == yf):
        break
    if(number_xy <=0):
        break
print(xlist)
print(ylist)
str ='\n'
f=open('list2.txt','w')
f.write(str.join('%s' %id for id in xlist))
f=open('list3.txt','w')
f.write(str.join('%s' %id for id in ylist))

Matlab散点图分析

正椭圆

 45度斜椭圆

tips:算法退出那里最好用步进电机步数走完作为退出条件,它最可靠。

代码还有很多可以优化的地方,但是效果已经挺不错了,供大家学习参考。

标签:滑台,suojian,abs,int,number,插补,附斜,xy,Fm
来源: https://blog.csdn.net/weixin_48220838/article/details/122619673