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

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

效果图：

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

十字滑台，两个步进电机，一个舵机，一块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 120LCD_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); //移动x1elseMotorXDriver(-x1,1,500);//向y方向移动if(y1>=0)MotorYDriver(y1,0,500);elseMotorYDriver(-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为当前坐标，初始化为0u16 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为当前坐标，初始化为0u16 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); //leftnumber_xy --;}if(Fm <0 && number_xy>0){Fm  = Fm + 2*abs(y)+1;x = x;y = abs(y)+1;MotorYDriver(1,0,500); //upnumber_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); //downnumber_xy --;}if(Fm <0 &&number_xy>0){Fm  = Fm + 2*abs(x)+1;x = abs(x)+1;y = y;MotorXDriver(1,1,500); //leftnumber_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); //upnumber_xy --;}if(Fm <0 && number_xy>0){Fm  = Fm + 2*abs(x)+1;y = y;x = abs(x)+1;MotorXDriver(1,0,500); //rightnumber_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为当前坐标，初始化为0u16 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); //downnumber_xy --;}if(Fm <0 && number_xy>0){Fm  = Fm + 2*abs(x)+1;y = y;x = abs(x)+1;MotorXDriver(1,0,500); //rightnumber_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); //rightnumber_xy --;}if(Fm <0 &&number_xy>0){Fm  = Fm + 2*abs(y)+1;y = abs(y)+1;x = x;MotorYDriver(1,0,500); //upnumber_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); //upnumber_xy --;}if(Fm <0 && number_xy>0){Fm  = Fm + 2*abs(x)+1;y = y;x = abs(x)+1;MotorXDriver(1,1,500); //leftnumber_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); //leftnumber_xy --;}if(Fm <0 && number_xy>0){Fm  = Fm + 2*abs(y)+1;y = abs(y)+1;x = x;MotorYDriver(1,1,500); //downnumber_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为当前坐标，初始化为0u16 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); //leftnumber_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); //upnumber_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); //downnumber_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); //leftnumber_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); //upnumber_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); //rightnumber_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为当前坐标，初始化为0u16 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); //downnumber_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); //rightnumber_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); //rightnumber_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); //upnumber_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); //leftnumber_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); //downnumber_xy --;}     if(number_xy <= 0)   //写完跳出break;   }}
}

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

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

python验证斜椭圆算法

import mathxlist = []
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 * yx = x-1y = ynumber_xy -=1xlist.append(x)ylist.append(y)if(Fm<0 and number_xy>0):Fm += 2 * B * y + B + C * xx = xy = y + 1number_xy -=1xlist.append(x)ylist.append(y)if(x == xf and y == yf):breakif(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：算法退出那里最好用步进电机步数走完作为退出条件，它最可靠。

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

ps：我蛮无语的，本文其实是在2022-01月份写的，后面觉得把课程设计答案（自己做的）发在博客不太好，于是在3月份的时候就将博文删除了，可是没想到在我发文期间被有心人利用，原封不动复制我博客内容做成pdf文档发到某度文库等牟取私利，也不注明内容出处，有两个字叫“侵权”你懂吗？？？

所以今天干脆把之前写的博客恢复了（回收站里捡来的）。

本科某西南211，其实写的时候是大四上。。。。

步进电机驱动数控十字滑台直线、圆弧插补（附斜椭圆插补代码实现）相关推荐

提升机，升降机，绞车，气缸驱动机械手爪，隧道式清洗干燥机，十字滑台机械手，小型饲料粉碎机……3D模型
提升机,升降机,绞车分页机机器人抓手sw 岸桥起重机(岸边集装箱起重机可调角度的输送机气缸驱动机械手爪继电器电性能测试机隧道式清洗干燥机凸轮式包装机的往复机构弯管机附件(附动态图) 龙 ...
单片机四线步进电机驱动 PROTEUS 和51单片机教程（附仿真文件+源代码）
单片机四线步进电机驱动程序功能: 当按下SW1时步进电机正转,按下SW2后步进电机反转,并可以根据源码任意调节旋转速度和每一个脉冲旋转角度程序参考源代码如下: /C语言程序:/ #include ...
matlab六轴直线插补代码
针对已有点位,通过matlab编程进行直线插补,插补逻辑是将X.Y.Z.A.B.C先分成XY,ZA和BC三部分,给定直线轴插补步长LStep和旋转轴插补步长RStep.先计算要插补的两行代码各个轴所要 ...
【电机应用控制】——步进电机控制原理（四相五线/两相四线/细分驱动）驱动器梯形/S形加减速算法直线/圆弧插补
目录一.步进电机简介二.步进电机控制原理 1.四相五线 2.两相四线 3.细分驱动三.步进电机驱动器四.梯形加减速算法五.S形加减速算法六.直线插补七.圆弧插补八.步进电机闭环系统(位 ...
【STM32H7的DSP教程】第50章 STM32H7的样条插补实现，波形拟合丝滑顺畅
完整版教程下载地址:http://www.armbbs.cn/forum.php?mod=viewthread&tid=94547 第50章 STM32H7的样条插补实现,波形拟合 ...
六轴机器人轨迹规划之空间直线插补
1.原理简述直线插补时,指定起止坐标与速度. 确定要插直线的周期增量,分解到xyz轴. 2.matlab代码 clear; clc; p0=[1,2,3]; pf=[2,4,5]; %指定起止位置 ...
plc 滑台流程图_PLC控制直线滑台运动概述
设备中直线模组滑台常用的可编程操控器(简称 PLC )以其通用性强.可靠性高.指令体系简单.编程简便易学.易于掌握.体积小.修理作业少.现场接口装置方便等一系列长处,被广泛应用于工业自动操控中 ...
plc 滑台流程图_直线滑台模组PLC控制说明
直线滑台只能传动部分的主体,需要驱动电机的驱动,以及控制部分的程序控制,直线滑台的控制部分可以用PLC.控制卡.总线等,下面介绍如何使用PLC控制直线滑台. 1.直线滑台标配有三个感应开关,根据开关方 ...
直线滑台模组与电机控制
1直线滑台模组的基本分类 1.1按工作原理分类按工作原理对直线滑台模组进行分类,可分为两大类:丝杆传动和同步带传动. 丝杆传动通过联轴器将伺服电机和滚珠丝杆连接在一起,进而将伺服器的旋转运动转化为 ...

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

python验证斜椭圆算法

步进电机驱动数控十字滑台直线、圆弧插补（附斜椭圆插补代码实现）相关推荐

最新文章

热门文章