步进电机驱动数控十字滑台直线、圆弧插补（附斜椭圆插补代码实现）
作者：互联网
本人大三，写个博文算是对近期课设的心得体会，供大家学习参考。
效果图：
先介绍一下此次用到的实验设备：
十字滑台，两个步进电机，一个舵机，一块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