步进电机实时多轴联动插补梯型加减速算法-STM32单片机实现方法

dbxzjq

已于 2025-03-17 23:02:05 修改

阅读量2.5k

点赞数 14

文章标签：单片机嵌入式硬件 stm32

于 2024-04-02 16:37:41 首次发布

本文链接：https://blog.youkuaiyun.com/dbxzjq/article/details/137266511

版权

关于步进电机的实时加减速算法，注意是实时，运动数据是实时产生的，不是缓存后像播放电影一样的的方式，而且加减速度+多轴联动插补脉冲的例子基本不可能找得到，在国内可能这一类的东西都是专业技术，都以保密的形式不可能有任何细节可以分享出来，也只能这样才能保证产品能卖更高的价格！独家才能干大嘛！翻来找去也就只能找到外国人开源的GRBL代码，通过网络搜索，大多都是翻译过来的AVR446 PDF档的内容，通过对SQRT 泰勒级数展开得到一个近似值的算法求得每一步的脉冲时间来实现，算法转换整型类型进行运算，可以在低端单片机实现快速运算，从而实现实时加减速处理

运动控核心中，实时脉冲，多轴联动插补，G代码解释，移动路径的预判（前瞻），前瞻说白了就是空间矢量路径的所有转变弯部分的预先判断，因为不可能在过弯时把速度减到停止状态，这样一来加工时间会加工，而且会影响切削表面的效果，所以在当矢量路径在拐弯时要减速到适合的速度才能拐这一个弯，所以前瞻的目的就是为了处理矢量路径的各种拐弯。

真要学习这些运动控制的也就只有开源的GRBL和linuxCNC,但是对于初学者来说，直接看如此大量和复杂的代码去理解相关的核心技术是非常不容易的，这文章也是自学路上的一点理解和记录下来，留给自己回头看看的作用，希望也对初学者能有微薄的帮助，但愿国人人人如虎，有能人写出国内最好的CNC控制而且是白菜价的系统。

速度，加速度，时间，三者的关系 t=(v0+v)/a

t 时间

v 速度

v0 初速度

a 加速度

如果v0=0,去掉v0,可以得到三者的关系为 t=v/a

哪如何理解这三者的关系呢，按相应的加速度a进行加速，直到达到速度v对应的速度，要花的时间t

在这个加速的时间 t 里，步进电机要移动多少步才能达到速度 v

s=v*t

s 说白了就是速度与时间的乘积，也就是对应的面积，s=v*t只能对应着匀速度面积，也就是说按这个时间和速度来移动得了多少步s

对于梯型加减速度，先看一下图形

图上标记着S1，S2，S3，分别为三个面积，对应着加速度部分的面积，匀速度面积，减速度面积，先按加速度=减速度，哪就先只分析加速度和匀速度部分

S2=v*t 匀速度部分也就是一个长方形的面积，

S1=v*t/2 加速度的面积也就是长方形的一半，就是对应的面积

所以得到加速度的公式也就是

s=v*v/a/2 这样得就求得对应加速度要加速到速度v要移动多少个步(即脉冲数量），哪反过来按不同的面积s求出对应的速度v 得到如下式

v=sqrt(s*a*2)

速度说白了就是步进电机的频率，也就是脉冲频率，哪么对应的频率的周期时间，是不是就是速度的倒数啦，t=1/v,得到对应的周期时间后，如果使用STM32F1的定时器，是不是可以这样得到定时器的计数值，假设定时器时钟频率为72M，PSC=0，也就是只设置ARR的值，哪么

ARR=72000000/sqrt(s*a*2)，把S面积换为步数n

ARR=72000000/sqrt(n*a*2) n++,得到每一步的定时器计数值来，就能控制对应加速度的每一步的脉冲周期定时器计数值来了，定义一个速度变量

speed=72000000/v 得到相应的目标速度的定时器计数值

n=0;

void TIM2_IRQHandler(void){

TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

if(speed<ARR)

{

ARR=72000000/sqrt(n++*a*2);

TIM2->ARR=ARR；

}

if(speed>ARR)

{

ARR=72000000/sqrt(n--*a*2);

TIM2->ARR=ARR；

}

这样就按照目标速度进行加速或者减速度了,只要目标速度speed控制为相应的目标速度的定时器计数值，进行比较就能控制它工作在加速，匀速，减速

原理就是这样，但是sqrt在F103这样的M3核心的MCU里，它执行一个运算过程要花费大量的时间，要十多微秒的时间，这样就无法做到更高的脉冲频率输出，所以AVR446 PDF档里讲解的就是为了找到一个快速的算法来计算每一步的周期时间，另外还有一个问题，16位的定时器，在65535的ARR值时，得到低的频率为1098HZ，无法更低的频率，电机启动时远低于这个频率，所以在不改变PSC条件下，难以实现低频率直接计算，还要浪费时间去设置PSC，等处理，或者通过微步进行处理，相对较麻烦，但是可以使用SysTick定时器，这是24位的，在2^24 个计数可以得到4.29HZ这样低的频率，实际上最好是可以使用32位定时器

ARR=72000000/sqrt(n*a*2) 这样的式子，使用sqrt是不现实的，因为sqrt在F103无法快速运算，占用计算时间太长了，哪么还有什么办法呢？可以使用哪个网络上流传的神开方函数

float fastInvSqrt(const float x)
{
const float xhalf = x * 0.5f;
union {
float x;
int32_t i;
} u;
u.x=x;
u.i = 0x5f3759df - (u.i >> 1); //0x5f375a86
return u.x * (1.5f - xhalf * u.x * u.x);
}

这个函数返回的是SQRT的倒数，通过测试在F103上，执行一次运算大概在4微秒以内，哪么在F103上，弄最高50KHZ，或者45KHZ应该能勉强可以的，也就是说一个周期时间为22.22微秒的时间，半周期有11.11微秒，脉冲做成对称方波，分为上半周期和下半周期，在半个周期里进行定时器计数值的运算和XYZ，三个轴的插补运算，代码的总执行时间要求在半个周期内完成，也就是11.11微秒内完成所有这些代码执行，另外半个周期的时间里也就只做IO的复位操作，其它什么也不干，这是把这半个周期的时间留给MCU的主循环里处理一些其它的工作

如下是一个简单加速度过程的脉冲输出测试代码

SysTick_Config(72000); SysTick初始化一下随便给一个值

uint32_t a=2*32000;

uint32_t phase=0；n=1;

在main 的主循环里让run=1,就是启动脉冲输出，phase代表当前相位，让它一直自加1就好了，反正加到溢出，&1后它依然是0和1两种状态，>>1 也就是计数值的一半1/2，用移位比除法更快，得到快一倍的定时计数值，对应着前半周还是后半周

void SysTick_Handler(void)
{

if(run){
if(++phase&1){

GPIOA->BSRR= GPIO_Pin_1;
SysTick->LOAD=(uint32_t)(72000000.0f*InvSqrt(a*n))>>1;
//SysTick->LOAD=(uint32_t)(72000000/sqrtf((float)64000.0*gg))>>1;

SysTick->VAL = 0;
   if(n++>28444)n=28444;

   }

else    GPIOA->BRR= GPIO_Pin_1;
}
}

这个只是简单的测试计算不同的定时计数值输出加速度脉冲，实际上还要处理加速度，匀速度，减速判断，还有多轴插补部分的代码，这样一来，你要测量出所有的代码能否在半个周期的时间内执行完成所有代码，如果不行，哪么你的最高频率就得降低下来，直到适合为止，另半个周期也就是让IO复位，什么也不再处理了，因为要保留半个周期的时间给MCU主循环处理其它相应的事情，如果定时器的两个相位时间全被代码占用了，主循环里也就什么都无法干了，全被定时器的中断服务函数占用了所有的MCU时间片了，不管你用什么算法进行计算者要满足这个条件，当然算法执行时间更少，也就意味着你可以做到更高的脉冲输出频率了

如果使用像STM32F407，M4内核的MCU，主频为168MHZ，即定时器时钟可以为168M，因为时钟频率更高它有更好的高频分辨率，在高频时，即ARR=较小的计数值时，比如ARR=300，和ARR=301时，分别两个频率差非常小，这样改变频率时的跳动更小，达到更平滑的频率过渡，也有32位定时器，可以直接操作ARR得到不同速度的计数值，直接给PSC=0即可，同时MCU每周期指令时间更短得多，执行对应的运算时间会更短，可以做更高的脉冲频率输出，而且M4内核支持FPU，可以直接使用__sqrtf()函数进行平方根计算，几百纳秒完成一次运算，即不会超出1微秒就能计算一次平方根结果对比测试比这个倒数平方根fastInvSqrt()执行的速度要快得多，经过测量相应的代码可以6轴插补下勉强做到200KHZ的频率，是6个轴都同时运动下，如果3轴肯定能更快，要是使用AT32F435，主频更高，运行速度也就能更快了，像ESP32也是可以，而且是双核心的芯片，一个核心处理事情，一个核心作为加减速度和插补脉冲处理也是非常不错，只是这个芯片IO较少，要多轴得进行IO扩展，可以通过SPI接口，HC595进行扩展，把脉冲IO对应的脉冲变化量作集中到一个16位置变量或者32位变量输出2片或者4片HC595也是可以的，16IO和32IO扩展输出

其实速度加速度的公式来来去去也就是这些，能否更高脉冲频率说白了也就是运算能力的问题，对于使用时钟频率低的定时器，它的每一个计数值的频率跨度就越大，这样频率阶梯也就越大，线性平滑就越差，时钟频率越高，阶梯就越小，线性平滑性越理想，可以通过定时器的不同计数值计算出对应的频率，看计数值每步进1，之间的频率跨度多大，即这个频率跨度已经限制死了脉冲跳变跨度的大小了，时钟频率越低的定时器，肯定越差的了，可能1000HZ一个梯度，这样的步进驱动要不得，运行时只要受点阻力就会卡死不转动了，所以往往梯型加减速度都没弄明白过来，就搞什么S形的，对于单片机的性能来说要真做理想，是比较难度高的，因为条件已经限制死，不管你是否不使用中断产生脉冲，你都必须用定时器来产生脉冲，定时器已经给限制死了，实际上最适合的还是FPGA，它可以工作于非常高的频率几百MHZ的频率进行积分就能产生相应的脉冲了，哪么有几百MHZ的采样频率，微分出来的每一份就是非常非常小的时间啦，积分出来的脉冲频率的分辨率就可以非常的高，能达到非常理想的线性平滑度，这是硬件逻辑电路的处理速度，单片机的软件是无法跟得上的，比如说500MHZ的采样频率，输出最高脉冲频率为1M，哪它也能有500份的分辨率了，单片机如何做到？单片机里的定时器时钟，说白了对应数字来说也就是采样频率，按F1定时器的计算公式来说明一下

F=72000000/((psc+1)*(arr+1))

200KHZ

72000000/((0+1)*(359+1))=200000HZ

72000000/((0+1)*(360+1))=199445hz

两个频率差是多少？200000-199445=555HZ，哪么频率跳变的最小分辨率就是555HZ，如果是F407的168M呢？

168000000/((0+1)*(839+1))=200000hz

168000000/((0+1)*(840+1))=199762hz

200000-199762=238HZ，频率的阶梯就少了许多了，这就与采样定理一个意思了，线性平滑越理想，当然频率越往高，这个阶梯也相应的加大，越往低，阶梯就越小了，所以实际上能做多高频率，也同时保证这个更高的频率能平滑的过渡，就看定时器的时钟频率有多高，也就是采样频率能有多高，因为当电机在高脉冲频率时，如果频率阶梯太大时，会容易出现问题，电机会否加大抖动，或者直接卡死也有可能，即工作于这个极限频率下的时候，最大速度的时候，定时器的采样率对应的频率分辨率已经给你限制得死死的了，这与你是否S形加减速，还是梯形加减速两种类型的区别也被定时器的时钟频率给限制死了最高频，S形加减速只是为了解决电机的惯性而来的，但它无法解决定时器自身的限制，所以即使有更强大的运算能力，定时器时钟无法提供更高频率分辨能力，把脉冲频率能弄得更高，比如在在临近最高频率时，比如当前频率为190K，频率阶梯一级跳变是1000HZ，或者2000HZ或者更大时，也就是说电机从190K的频率一下跳变到191K，或者192K时，电机能否稳定跳变到这个频率？像168MHZ时钟的定时器下，它的每步频率变化也就是238HZ，哪从190K，增一个量程，也就是190.238K，当然步进量越是微小，会越稳定，1/sqrt(n2a),移动的步数越多的时候计算出来的频率隔离也会是越小的，所以定时器的高频分辨率能越细，越能配合到1/sqrt(n2a)的计算结果，当然如果定时器在较高主频的MCU下，如果定时器时钟源无法等于主频率，只能在2分频下，哪么也相当于给限制死了，只有一半的时钟频率提供定时器了，再高的主频也等于失去意义了，相当于运算能力再强，主频再高的MCU，最高脉冲频率只能受限于你的定时器，这也是无法改变的事实，定时器的时钟源必须可以支持主频不分频的频率才有意义，当然也看电机的驱动器细分而定，比如说4细分时，可以支持最大的频率跨度是多少HZ，8细分时的频率跨度，16细分，32细分，64细分，128细分下，能支持的频率跨度都不同的，因为电机的步进角被电细分了，即使更大的频率跨度都可以稳定支持，这个度能达到多少，只能是实际测试出来才能清楚，因为这样的限制用软件难以解决，这部分也就是说要充分的弄明白所使用的MUC，它能给你提供了什么，能支持极限运用到什么程度还能稳定，越往更高的脉冲频率输出，难度也是越高的，基本上所有的代码运行都是分秒必争的，当然能用硬件外设同时解决是最理想的，相当于多了一个线程处理，关于定时器的这些部分对应细节处理是影响较大的，往往刚接触步进电机控制的都没去关注到这一点，所以这里也提出来讲了一下，接着的部分也就是如何在定时器中断里实现加速度，匀速度，减速度，和多轴联动插补的实现部分，实际上也是一些对于每个步进电机如何初始化一些参数，如何比较出当前是加速度，匀速，还是减速的逻辑关系，也是与目标速度，加速度所要的步数，减速度所要的步数，移动距离是否达到目标速度的一些参数计算，然后就是利用Bresenham直线插补算法进行多轴插补输出脉冲

步进电机的一些参数，步毫米，或者毫米步，即每毫米要多少步，比如1.8度步进角的步进电机，转一周360度，360/1.8=200步，如果所使用的是丝杆，比如常规使用的1605，也就是丝杆转动一周移动5毫米，哪么200/5=40步每毫米，通过电机的驱动，可以设置电细分，比如说16细分，即16*40=640步每毫米，或者是32细分，32*40=1280步每毫米，如此类推，然后是最大速度，最大速度可以是按每分钟移动多少毫米，或者每秒移动多少毫米，比如说按每速度/分钟1000毫米，计算的步数就是1000*640=640000步/分钟，转换为每秒，640000/60=10666.66......步每秒，也就是这个就是对应1000毫米/分钟的速度下的脉冲频率10666.66.......hz,接着是加速度，通是按每秒平方作为单位，比如常规CNC设置是加速度为100mm/秒平方，也就是它的单位是秒的，在实际参与相关运算时，都把它们转换为步秒作为单位，100*640=64000步秒，对于步作为单位，步没有半步，没有0.xxxx浮点步，即都只能是整数步，所以不足的浮点步，只能是先存起来，放到后面累积为一个整数步才给输出一步，还有目标速度，它与最大速度不一样，目标速度是在执行移动时所设定的速度，这个目标速度是被最大速度所限制的，如果超过，就只能是使用最大速度，小于最大速度才按目标速度，CNC上的G代码的F值，有的叫进给速度，也就是目标速度了，最大速度是按照设备所能支持的最大速度，也就是运动控制器所支持的最高脉冲输出频率，即上面所说的计算出对应的脉冲频率的方式，比如你的控制器性能只能支持最高100KHZ的脉冲频率，你的最大速度计算出对应的频率不能超过100KHZ，然后是对应每个轴的当前坐标参数，移动距离等，这是一个步进电机最基本的参数，另外Bresenham插补也要定义几个插补用的参数，还有就是对应轴的IO引脚，是否可以支持选择对应引脚号等

另外就是在定时器中断服务函数里所有代码，包括定时器计算计数值，插补，逻辑处理等待都要优化执行效率，尽量可以更少的时间执行所有代码，这样才能提高最高输出脉冲频率，即尽可能减少指令的数量达到目的，才能减少执行的时间，除了调试时使用O1编译，实际生成的代码要O3优化，这是非常关键的

这里还是先补充一下关于AVR446 PDF档所使用的算法是什么

先看一下 t=v/a, v=t*a, s=v*t ,加速度部分的s=v*t/2,哪如何不用v，只使用t 和 a 求出对应的面积S来呢

t*a得到的是v,哪么t*v/2=s,s=t*t*a/2 ,得到加速度对应时间和步数S(面积）哪反过来直接求出每s所对应的时间为t=sqrt(s*2/a)

当然在文档里有个角速度参数，实际上可以省略掉角速度，用步来进行计算，出来结果是完全一样的，AVR446 PDF里是通过当前面积减上一个面积的时间求得当前的周期时间来进行计算也就是sqrt((n+1)*2/a)-sqrt(n*2/a)=Ct

所以在AVR446 PDF档里的式子就是这个样子

C0=sqrt(2/a)

Ct=C0*(sqrt(n+1)-sqrt(n))

这很容易就理解过来了，来来去去，也就是t,v,a三者的关系式，还有一个面积公式s，比如说2步的面积，与1步的面积，哪么2步的面积减去1步的面积就等于第2步的实际面积，这里2步的面积并不是指第2步的面积，是第1步+第2步的面积总和，即2步面积，所以2步面积减1步面积，得到实际的第2步面积，这样求出对应第2步面积的时间来，所以不管你是求对应面积的时间还是求对应面积的速度，它必须都经过求平方根这个过程，SQRT是必须的，然后AVR446 PDF后面的就是根据SQRT的泰勒级数展开得到一个简化不用求平方根的近似快速计算式出来，泰勒级数展开我就不会指导了，没上过高中，没学过高数，不会这些啊！也就只会加减乘除，然后把所有浮点数按倍数放大为整数再进行整数运算，这样即使低端的单片机也能快速运算得到近似的时间值，哪么时间乘定时器时钟频率就是定时器的计数值了，sqrt(s*a*2)求的是速度值，sqrt((n+1)*2/a)-sqrt(n*2/a)求出的是时间值，两者有什么区别呢，区别在于它们的初始频率是不一样的，但到达目标频率时是基本上一样的，在到达目标速度整个加速过程的总时间对于两次平方根求时间是最准确，求速度一次平方根可能会有轻微的出入，但实际上不影响使用，至于用哪一种更好，自行拿主意就好了，M4内核，支持FPU，使用这个函数__sqrtf()，C0*(__sqrtf(n+1)-__sqrtf(n))求时间也是非常非常快的，也就是两次平方根，如果__sqrtf(s*a*2) 求速度只是一次平方根，或者可以完全按AVR446 PDF的展开简化近似式也是可以，只是存在误差较大一些，反正要想精确，要想直接求平方根实现，还是支持FPU硬件浮点运算的MCU吧！

代码以最简单的方式用STM32F103实现了三个轴的加减速和联动插补，使用的是Bresenham插补算法，也就是GRBL所使用的的插补算法，Bresenham其实也是一个数字积分法，通过一个共用参考计数值，各个轴通过积分器积分，达到这个参考计数值后相当于溢出(算是软件处理溢出）并非常是数据类型溢出，也就是积分已满，就输出一个脉冲，计算最大速度和加速度以最简单的方式，没有计算出XYZ的矢量进行求得，每个轴，实际上是可以不同的最大速度，不同的最大加速度，和不同的每毫米步数，所以在求出实际的目标速度时，要利用XYZ三个轴以三角形的方式求出矢量和对应的加速度，最大速度，和目标速度，这里都是随便max,和min求得，实际上不准确的，只是为了能够快速完成代码能演示如何实现加减速度和插补联动，具体的计算方式可以看我另一文章多轴步进电机的加速度，速度原理分析和如何实现-优快云博客，或者参考GRBL的实现代码

代码只实现了执行一次矢量移动，如果想要实现连续多条路径矢量移动，要进行修改，增加运动数据的缓存，把不同移动的路径矢量添加进去，也可以简单的在主循环里执行

if(motion.movestep==0) move(10,5,-3,2000);

即每移动一次矢量，都要比较这个motion.movestep=0后，代码之前的矢量移动已经完成，才能继续下一条移动，这只是一个最简单的框架，对于加减速度判断也是以最简单的逻辑进行比较的，要真正实用还有许多东西要处理的，代码原理就不讲解了，本页的代码都添加了注释，很容易理解，代码量非常少的，自行分析一下就能弄明白了，后面提供程序工程和Proteus仿真工程下载连接

#include "stm32f10x.h"

#include  "OLED.h"

#include <stdlib.h>


#define xstep GPIO_Pin_0
#define ystep GPIO_Pin_2
#define zstep GPIO_Pin_4
#define xdir GPIO_Pin_1
#define ydir GPIO_Pin_3
#define zdir GPIO_Pin_5
typedef struct {	
	double speed; //速度 mm/min
	double accel;	//加速度 mm/s^2
	uint32_t step_mm;	//每毫米步数
	uint32_t counter;  //累加器
	uint32_t steps;  //移动的步数
	uint8_t dir; //方向
}parameter;
typedef struct {
	uint32_t maxspeed; //最大速度 步/s
	uint32_t maxaccel; //最大加速度 步/s^2
	uint32_t step_event_count; //插补计数参考
	uint32_t target_speed;  //目标速度
	uint32_t target_count;  //目标速度定时器计数值
	uint32_t acc2;  //2*acc
	uint32_t movestep;  //移动计数器
	uint32_t decstep;  //减速步数
	uint32_t load;    //定时器计数值临时记录器
	uint16_t step_mask; //步IO的掩码
	uint16_t step_io;   //步 IO 脉冲输出记录器
	uint16_t dir_mask; //方向IO的掩码
	uint16_t dir_io;   //方向IO输出记录器
	int32_t x;  //x坐标记录器
	int32_t y;  //y坐标记录器 
	int32_t z;  //z坐标记录器
	parameter axles[3];	 //三轴基本参数
}motion_;
motion_ motion;
uint32_t phase=0 ,stepAdd=0;
#define max(a, b) (a>b?a: b)
#define min(a, b) (a<b?a: b)
#define ABS(x) (x>0?x: -x) 
void IO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
	
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//GPIOA
	
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	//GPIO_SetBits(GPIOA,GPIO_Pin_0|GPIO_Pin_1);
}
void setAxles(uint8_t  axle,double speed,double accel,uint32_t step_mm)
{
	motion.axles[axle].speed=speed;
	motion.axles[axle].accel=accel;
	motion.axles[axle].step_mm=step_mm;
}
void AxleInit() //轴参数初始化函数
{
	IO_Init();
	setAxles(0,2000,100,320);
	setAxles(1,2000,100,320);
	setAxles(2,2000,100,320);
	motion.step_mask=0;
	motion.dir_mask =0;
	motion.step_mask =motion.step_mask |xstep|ystep|zstep;
    motion.dir_mask =motion.dir_mask | xdir|ydir|zdir;
	motion.dir_io=0;
	motion.step_io=0;
	motion.x=0;
	motion.y=0;
	motion.z=0;
}
//快速倒数平方根
float fastInvSqrt(const float x)
{
    const float xhalf = x * 0.5f;
    union {
        float x;
        int32_t i;
    } u;
		u.x=x;
    u.i = 0x5f3759df - (u.i >> 1); //0x5f375a86
    return u.x * (1.5f - xhalf * u.x * u.x);
} 
//三轴矢量移动函数，x,y,z单位：毫米(mm),speed实际移动的目标速度，单位：毫米/每分钟
void move(double x,double y,double z,double speed)
{
    uint32_t maxstepmm;
		if(x<0.0f){  //X轴移动的方向处理
			motion.axles[0].dir =0;
			motion.dir_io &= ~xdir;
		}
		if(x>0.0f){
			motion.axles[0].dir =1;
			motion.dir_io |= xdir;
		}
        //通过移动的距离(毫米)*电机每毫米步数，计算X轴实际移动的步数
		motion.axles[0].steps=(uint32_t)ABS(x*(double)motion.axles[0].step_mm);
	
		if(y<0.0f){
			motion.axles[1].dir =0;
			motion.dir_io &= ~ydir;
		}
		if(y>0.0f){
			motion.axles[1].dir =1;
			motion.dir_io |= ydir;
		}
		motion.axles[1].steps=(uint32_t)ABS(y*(double)motion.axles[1].step_mm);
	
			if(z<0.0f)
			{
				motion.axles[2].dir =0;
				motion.dir_io &= ~zdir;
			}
		if(z>0.0f){
			motion.axles[2].dir =1;
			motion.dir_io |= zdir;
		}
		motion.axles[2].steps=(uint32_t)ABS(z*(double)motion.axles[2].step_mm);
	  GPIOA->ODR =  (GPIOA->ODR & ~motion.dir_mask)| motion.dir_io; //方向IO输出
		
//求出最大的哪个轴的每毫米步数作为加速度和速度的步计算参考，这方式是不正确的，可以参考GRBL代码是如何计算，只是为了最简单的演示，所以才这样写  maxstepmm=max(max(motion.axles[0].step_mm,motion.axles[1].step_mm),motion.axles[2].step_mm);

	motion.maxspeed= max(max(motion.axles[0].speed,motion.axles[1].speed),motion.axles[2].speed)/60.0f * (double)maxstepmm;
	motion.maxaccel= min(min(motion.axles[0].accel,motion.axles[1].accel),motion.axles[2].accel)* (double)maxstepmm;
//计算出插补参考计数  
	motion.step_event_count =max(max(motion.axles[0].steps,motion.axles[1].steps),motion.axles[2].steps);
		
//初始化各轴插补累加器	motion.axles[0].counter=motion.axles[1].counter=motion.axles[2].counter=motion.step_event_count>>1;
	//目标速度，从每分钟毫米转换为每秒步数，即实际频率值	
	motion.target_speed= speed/60.0f * (double)maxstepmm;
    //如果目标速度大于最大速度，按最大速度运行
	if(motion.target_speed>motion.maxspeed)motion.target_speed=motion.maxspeed;
    //加速度计算值，2*acc,每一步时间计算用
	motion.acc2=2*motion.maxaccel;
    //目标速度转换为定时器的计数值，1/2周期时间，因为上下半周期分别处理
	motion.target_count=(uint32_t)(72000000.0f/(double)motion.target_speed)>>1;
    
    //计算出到达要减速的步数
	motion.decstep= ((motion.target_speed*motion.target_speed/motion.maxaccel)>>1);
    //如果总步数的一半小于减速的步数，使用总步数的一半作为减速步数，也就是没有匀速度，只有加速和减速
	if(motion.step_event_count>>1 < motion.decstep)motion.decstep=(motion.step_event_count>>1);
	//移动步数计数器，以递减方式
	motion.movestep=motion.step_event_count;
    //初始化定时器和加速度计数参考
	stepAdd=0; 
	motion.load =(uint32_t)(72000000.0f*fastInvSqrt(motion.acc2*1))>>1;
	SysTick_Config(motion.load);
	
}

//加减速，联动插补，都在定时器中断里处理
void SysTick_Handler(void)
{
	if(motion.movestep>0) //是否有运动数据
	{
		if(++phase&1) //相位判断，上半周期，下半周期
		{
			 GPIOA->ODR =  (GPIOA->ODR & ~motion.step_mask); //io 复位，默认为低电平
			motion.movestep--;
			if(motion.load > motion.target_count && motion.movestep >	motion.decstep) //加速
			{
				stepAdd++;
				motion.load=(uint32_t)(72000000.0f*fastInvSqrt(motion.acc2*stepAdd))>>1;
				
			}
			 if(motion.target_count>=motion.load && motion.movestep >	motion.decstep) //匀速
			{
				
				motion.load=motion.target_count;
			}
			 if(motion.movestep <	motion.decstep) //减速
			{
			  stepAdd--;
				if(stepAdd == 0)stepAdd=1;
				motion.load=(uint32_t)(72000000.0f*fastInvSqrt(motion.acc2*stepAdd))>>1;

			}				
//X,Y,Z三轴插补-------------------------------------------------------
            //x轴DDA插补计算
			motion.axles[0].counter+=motion.axles[0].steps; //累加器进行累加，也就是积分中
			if(motion.axles[0].counter>motion.step_event_count){ //积分已经满(溢出，非数据类型溢出）
				motion.axles[0].counter-=motion.step_event_count; //软件处理溢出
				motion.step_io |=xstep;              //输出脉冲，先记录到一个变量里，最后才统一输出
				if(motion.axles[0].dir)motion.x++;  //记录坐标值
				else motion.x--;
			}
            //Y轴DDA插补计算
			motion.axles[1].counter+=motion.axles[1].steps;
			if(motion.axles[1].counter>motion.step_event_count){
				motion.axles[1].counter-=motion.step_event_count;
				motion.step_io |=ystep;				
				if(motion.axles[1].dir)motion.y++;
				else motion.y--;
			}
            //Z轴DDA插补计算
			motion.axles[2].counter+=motion.axles[2].steps;
			if(motion.axles[2].counter>motion.step_event_count){
				motion.axles[2].counter-=motion.step_event_count;
				motion.step_io |=zstep;
				if(motion.axles[2].dir)motion.z++;
				else motion.z--;
			}
//可以添加A，B，C.....轴，只要MCU的运算速度够快，可以更多的轴进行插补，达到更多的轴也可以更高的脉冲频率输出
//---------------------------------------------------------------------		
				SysTick->LOAD=motion.load;
				//SysTick->VAL=0;			
		}
			
      else 	{
                //哪个轴输出高电平
				GPIOA->ODR =  (GPIOA->ODR & ~motion.step_mask)| motion.step_io;
				motion.step_io=0; //清零输出脉冲记录
			}
  }
			
}


float fastSqrt(const float x)
{
    const float xhalf = x * 0.5f;
    union {
        float x;
        int32_t i;
    } u;
		u.x=x;
    u.i = 0x5f3759df - (u.i >> 1); //0x5f375a86
    return x * u.x * (1.5f - xhalf * u.x * u.x);
} 


int main(void)
{ 
	double temp,temp1,temp2;
	

	OLED_Init();
	

	OLED_ShowString(1,1,"X:",0);
	OLED_ShowString(2,1,"Y:",0);
	OLED_ShowString(3,1,"Z:",0);
	
	if(motion.axles[0].dir)		OLED_ShowString(1,3,"+",0);
	else OLED_ShowString(1,3,"-",0);
	temp = ABS(motion.x)/(double)motion.axles[0].step_mm;
    OLED_ShowNum(1,4,(uint32_t)temp,4,0);
	OLED_ShowString(1,8,".",0);
	OLED_ShowNum(1,9,(uint32_t)(temp*1000)%1000,3,0);

	if(motion.axles[1].dir)		OLED_ShowString(2,3,"+",0);
	else OLED_ShowString(2,3,"-",0);
	temp1 = ABS(motion.y)/(double)motion.axles[1].step_mm;
    OLED_ShowNum(2,4,(uint32_t)temp1,4,0);
	OLED_ShowString(2,8,".",0);
	OLED_ShowNum(2,9,(uint32_t)(temp1*1000)%1000,3,0);

	if(motion.axles[2].dir)		OLED_ShowString(3,3,"+",0);
	else OLED_ShowString(3,3,"-",0);
	temp2 = ABS(motion.z)/(double)motion.axles[2].step_mm;
    OLED_ShowNum(3,4,(uint32_t)temp2,4,0);
	OLED_ShowString(3,8,".",0);
	OLED_ShowNum(3,9,(uint32_t)(temp2*1000)%1000,3,0);

		AxleInit(); //轴初始化
		move(10,5,-3,2000); //执行一次移动
	while(1){ 
    //OLED显示三轴坐标值
   	if(motion.axles[0].dir)		OLED_ShowString(1,3,"+",0);
		else OLED_ShowString(1,3,"-",0);
		temp = ABS(motion.x)/(double)motion.axles[0].step_mm;
		OLED_ShowNum(1,4,(uint32_t)temp,4,0);
		OLED_ShowString(1,8,".",0);
		OLED_ShowNum(1,9,(uint32_t)(temp*1000)%1000,3,0); 
		
		if(motion.axles[1].dir)		OLED_ShowString(2,3,"+",0);
		else OLED_ShowString(2,3,"-",0);
		temp1 = ABS(motion.y)/(double)motion.axles[1].step_mm;
		OLED_ShowNum(2,4,(uint32_t)temp1,4,0);
		OLED_ShowString(2,8,".",0);
		OLED_ShowNum(2,9,(uint32_t)(temp1*1000)%1000,3,0);

		if(motion.axles[2].dir)		OLED_ShowString(3,3,"+",0);
		else OLED_ShowString(3,3,"-",0);
		temp2 = ABS(motion.z)/(double)motion.axles[2].step_mm;
		OLED_ShowNum(3,4,(uint32_t)temp2,4,0);
		OLED_ShowString(3,8,".",0);
		OLED_ShowNum(3,9,(uint32_t)(temp2*1000)%1000,3,0);		
}
}

同时也使用Proteus进行了仿真，使用的是Proteus PRO 8.17版本的，自行百度搜索一下就有下载了，安装好后就是和谐好的了，也非常稳定，看一下运行的结果图吧！对应三个轴的步和方向引脚输出的波形，加速度，匀速，减速

放大加速度部分：

能够看到脉冲周期时间慢慢减少，三个轴插补着输出，只执行一次move(10,5,-3,2000) ，前三个参数是X，Y，Z移动的距离，单位mm，最后一个是目标速度，毫米/每分钟作为单位

再看实时运行的波形图，移动完成后，看到的减速对应的波形

利用定时中断现实现，毕竟占用MCU时间片是比较多的，所以不是最理想的办法，但这种方式也是最容易实现，插补，坐标计数都最容易实现，写这个原因也是自己刚接触步进电机控制的时候，到处搜索相关的，大多也就是一些加减速原理算法，插补算法什么的，但实际上如何整合到一起呢！都没有很好的入门资料，要么看GRBL整个代码，GRBL也不是实时性的，它是把计算出来的运动数据进行缓存，一个个要移动的矢量数据进行处理产生脉冲数据，估计许多刚接触也是着实头痛，当然没有用到DMA，定时器PWM等硬件产生脉冲的方式，到达最高频率时留给主循环的MCU时间片非常的少，也就是最高频率周期的一半时间，但这种方式使用更高端的MCU(更高的主频，支持FPU浮点运算）可以做到更高的脉冲频率的，定时中断函数里的所有代码，可以进行一个执行完所有代码的时间测量，具体代码执行时间测量百度去搜吧！得到这个时间值，脉冲频率就不能大于这个时间值了，要对最高脉冲频率进行限制，才能保证稳定运行。

这样一个框架代码对于入门参考已经非常足够了，因为非常少的代码量就实现到了，可以更轻松的理解过来，对于CNC的运动控制还有许多的部分要理解才行，这只是最基本的产生加减速度和如何联动插补的部分而已

下一步看能否实现实时S型加减速，研究一下能否实现出快速的S型运算方式

STM32F103程序代码，包括Proteus仿真工程 ,使用proteus pro 8.17版运行步进电机实时多轴插补联动梯型加减速算法单片机实现方法资源-优快云文库