步进电机梯形加减速
什么是梯形加减速
假设该装置使用步进电机实现物体X的移动，系统要求从A点出发，到B点停止，移动的时间越短越好且系统稳定。
根据步进电机的特性，最大程度加大电机转速（提高脉冲频率），则到达B点的时间就越短，但是如果施加脉冲频率过高，超过了步进电机的最小启动频率，则会造成电机内部的反向电动势的阻尼作用，转子与定子之间的磁反应将跟随不上电信号的变化，导致堵转或者丢步，滑块连动都没动。
所以要求在电机启动时需要一个低速，但为了实现系统要求，在启动之后慢慢的升高速度，实现一个加速的过程，那如果在达到终点前一直在加速，就又有另外的一个问题，到达终点时速度太快就会导致刹不住，出现过冲现象，为了防止这个现象我们需要在达到终点前的一段距离提前减速，使得滑块到达终点时速度刚好为0，即从最高速度一直减至停止。
可以将这个过程描述为：梯形加减速。
1、在OA加速过程中，由低于步进电机的启动频率开始启动（模型中由0启动），以固定的加速度增加速度到目标值。
2、在AB匀速过程中，以最大速度匀速运动。
3、在BC减速过程中，以固定的加速度减小速度到0。
这种算法是一种在加速过程和减速过程中加速度不变的匀变速控制算法。
梯形加减速的原理分析
梯形加减速的原理：控制脉冲频率来控制速度。控制脉冲个数来控制位移。
梯形加减速的算法实现
推导过程复杂，可自行百度。
梯形加减速整体流程
定时器中断处理流程：
定时器中断产生脉冲并且只有在步进电机移动时进入，四个不同运动的状态，分别是stop--accel--run--decel--stop。
每一个速度变化的阶段的划分是前面计算得出的对应脉冲个数，当脉冲个数到达限制则启动切换即可，这种操作可以通过状态机在定时器中断中实现。
在定时器中断实现状态机功能：
当应用程序启动或者步进电机停止状态机处于STOP状态。当输入移动步数建立计算完成，一个新状态被置位同时定时器的中断被使能。
当运行的步数超过1步状态机进入ACCEL状态，如果只移动1步，那么状态直接变为DECEL，因为只移动一步是不需要加速的。
当状态变为ACCEL，应用程序一直加速步进电机达到期望的最大速度，这是状态变为RUN，或者减速必须开始，状态变为DECEL。
当状态变为RUN时，步进电机会持续保持最大速度speed旋转，直到必须开始减速然后把状态改变为DECEL。它会一直保持DECEL状态并一直减速到期望的步数并且速度为0。然后状态变为STOP。
中断流程图：
步进电机梯形加减速实验
硬件资源：TIM8_CH3（PI7）、DIR（PB2）、EN（PF11）。
涉及梯形加减速实现部分：
#define TIM_FREQ 168000000U /* 定时器主频 */ #define MAX_STEP_ANGLE 0.225 /* 最小步距(1.8/MICRO_STEP) */ #define PAI 3.1415926 /* 圆周率*/ #define FSPR 200 /* 步进电机单圈步数 */ #define MICRO_STEP 8 /* 步进电机驱动器细分数 */ #define T1_FREQ (TIM_FREQ/84) /* 频率ft值 */ #define SPR (FSPR*MICRO_STEP) /* 旋转一圈需要的脉冲数 */ /* 数学常数 */ #define ALPHA ((float)(2*PAI/SPR)) /* α= 2*pi/spr */ #define A_T_x10 ((float)(10*ALPHA*T1_FREQ)) #define T1_FREQ_148 ((float)((T1_FREQ*0.676)/10)) /* 0.676为误差修正值 */ #define A_SQ ((float)(2*100000*ALPHA)) #define A_x200 ((float)(200*ALPHA)) /* 2*10*10*a/10 */ typedef struct { __IO uint8_t run_state; /* 电机旋转状态 */ __IO uint8_t dir; /* 电机旋转方向 */ __IO int32_t step_delay; /* 下个脉冲周期（时间间隔），启动时为加速度 */ __IO uint32_t decel_start; /* 开始减速位置 */ __IO int32_t decel_val; /* 减速阶段步数 */ __IO int32_t min_delay; /* 速度最快，计数值最小的值(最大速度，即匀速段速度) */ __IO int32_t accel_count; /* 加减速阶段计数值 */ } speedRampData; enum STA { STOP = 0, /* 加减速曲线状态：停止*/ ACCEL, /* 加减速曲线状态：加速阶段*/ DECEL, /* 加减速曲线状态：减速阶段*/ RUN /* 加减速曲线状态：匀速阶段*/ }; enum DIR { CCW = 0, /* 逆时针 */ CW /* 顺时针 */ }; enum EN { EN_ON = 0, /* 失能脱机引脚 */ EN_OFF /* 使能脱机引脚 使能后电机停止旋转 */ }; /********************************************梯形加减速***********************************************/ speedRampData g_srd = {STOP,CW,0,0,0,0,0}; /* 加减速变量 */ __IO int32_t g_step_position = 0; /* 当前位置 */ __IO uint8_t g_motion_sta = 0; /* 是否在运动？0：停止，1：运动 */ __IO uint32_t g_add_pulse_count = 0; /* 脉冲个数累计 */ /* * @brief 生成梯形运动控制参数 * @param step：移动的步数 (正数为顺时针，负数为逆时针). * @param accel 加速度,实际值为accel*0.1*rad/sec^2 10倍并且2个脉冲算一个完整的周期 * @param decel 减速度,实际值为decel*0.1*rad/sec^2 * @param speed 最大速度,实际值为speed*0.1*rad/sec * @retval 无 */ void create_t_ctrl_param(int32_t step, uint32_t accel, uint32_t decel, uint32_t speed) { __IO uint16_t tim_count; /* 达到最大速度时的步数*/ __IO uint32_t max_s_lim; /* 必须要开始减速的步数（如果加速没有达到最大速度）*/ __IO uint32_t accel_lim; if(g_motion_sta != STOP) /* 只允许步进电机在停止的时候才继续*/ return; if(step < 0) /* 步数为负数 */ { g_srd.dir = CCW; /* 逆时针方向旋转 */ ST3_DIR(CCW); step =-step; /* 获取步数绝对值 */ } else { g_srd.dir = CW; /* 顺时针方向旋转 */ ST3_DIR(CW); } if(step == 1) /* 步数为1 */ { g_srd.accel_count = -1; /* 只移动一步 */ g_srd.run_state = DECEL; /* 减速状态. */ g_srd.step_delay = 1000; /* 默认速度 */ } else if(step != 0) /* 如果目标运动步数不为0*/ { /*设置最大速度极限, 计算得到min_delay用于定时器的计数器的值 min_delay = (alpha / t)/ w*/ g_srd.min_delay = (int32_t)(A_T_x10 /speed); //匀速运行时的计数值 /* 通过计算第一个(c0) 的步进延时来设定加速度，其中accel单位为0.1rad/sec^2 step_delay = 1/tt * sqrt(2*alpha/accel) step_delay = ( tfreq*0.676/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100 */ g_srd.step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10); /* c0 */ max_s_lim = (uint32_t)(speed * speed/ (A_x200 * accel / 10)); /* 计算多少步之后达到最大速度的限制 max_s_lim = speed^2 / (2*alpha*accel) */ if(max_s_lim == 0) /* 如果达到最大速度小于0.5步，我们将四舍五入为0,但实际我们必须移动至少一步才能达到想要的速度 */ { max_s_lim = 1; } accel_lim = (uint32_t)(step*decel/(accel+decel)); /* 这里不限制最大速度 计算多少步之后我们必须开始减速 n1 = (n1+n2)decel / (accel + decel) */ if(accel_lim == 0) /* 不足一步 按一步处理*/ { accel_lim = 1; } if(accel_lim <= max_s_lim) /* 加速阶段到不了最大速度就得减速。。。使用限制条件我们可以计算出减速阶段步数 */ { g_srd.decel_val = accel_lim - step; /* 减速段的步数 */ } else { g_srd.decel_val = -(max_s_lim*accel/decel); /* 减速段的步数 */ } if(g_srd.decel_val == 0) /* 不足一步 按一步处理 */ { g_srd.decel_val = -1; } g_srd.decel_start = step + g_srd.decel_val; /* 计算开始减速时的步数 */ if(g_srd.step_delay <= g_srd.min_delay) /* 如果一开始c0的速度比匀速段速度还大，就不需要进行加速运动，直接进入匀速 */ { g_srd.step_delay = g_srd.min_delay; g_srd.run_state = RUN; } else { g_srd.run_state = ACCEL; } g_srd.accel_count = 0; /* 复位加减速计数值 */ } g_motion_sta = 1; /* 电机为运动状态 */ ST3_EN(EN_ON); tim_count=__HAL_TIM_GET_COUNTER(&g_atimx_handle); __HAL_TIM_SET_COMPARE(&g_atimx_handle, ATIM_TIMX_PWM_CH3, tim_count + g_srd.step_delay / 2); /* 设置定时器比较值 */ HAL_TIM_OC_Start_IT(&g_atimx_handle, ATIM_TIMX_PWM_CH3); /* 使能定时器通道 */ }
状态机实现：
/** * @brief 定时器比较中断 * @param htim：定时器句柄指针 * @note 无 * @retval 无 */ void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim) { __IO uint32_t tim_count = 0; __IO uint32_t tmp = 0; uint16_t new_step_delay = 0; /* 保存新（下）一个延时周期 */ __IO static uint16_t last_accel_delay = 0; /* 加速过程中最后一次延时（脉冲周期） */ __IO static uint32_t step_count = 0; /* 总移动步数计数器*/ __IO static int32_t rest = 0; /* 记录new_step_delay中的余数，提高下一步计算的精度 */ __IO static uint8_t i = 0; /* 定时器使用翻转模式，需要进入两次中断才输出一个完整脉冲 */ if (htim->Instance == TIM8) { tim_count = __HAL_TIM_GET_COUNTER(&g_atimx_handle); tmp = tim_count + g_srd.step_delay / 2; /* 整个C值里边是需要翻转两次的所以需要除以2 */ __HAL_TIM_SET_COMPARE(&g_atimx_handle, ATIM_TIMX_PWM_CH3, tmp); i++; /* 定时器中断次数计数值 */ if (i == 2) /* 2次，说明已经输出一个完整脉冲 */ { i = 0; /* 清零定时器中断次数计数值 */ switch (g_srd.run_state) /* 加减速曲线阶段 */ { case STOP: step_count = 0; /* 清零步数计数器 */ rest = 0; /* 清零余值 */ /* 关闭通道*/ HAL_TIM_OC_Stop_IT(&g_atimx_handle, ATIM_TIMX_PWM_CH3); ST3_EN(EN_OFF); g_motion_sta = 0; /* 电机为停止状态 */ break; case ACCEL: g_add_pulse_count++; /* 只用于记录相对位置转动了多少度 */ step_count++; /* 步数加1*/ if (g_srd.dir == CW) { g_step_position++; /* 绝对位置加1 记录绝对位置转动多少度*/ } else { g_step_position--; /* 绝对位置减1*/ } g_srd.accel_count++; /* 加速计数值加1*/ /* 计算新(下)一步脉冲周期(时间间隔) */ new_step_delay = g_srd.step_delay - (((2 * g_srd.step_delay) + rest) / (4 * g_srd.accel_count + 1)); /* 计算余数，下次计算补上余数，减少误差 */ rest = ((2 * g_srd.step_delay) + rest) % (4 * g_srd.accel_count + 1); if (step_count >= g_srd.decel_start) /* 检查是否到了需要减速的步数 */ { g_srd.accel_count = g_srd.decel_val; /* 加速计数值为减速阶段计数值的初始值 */ g_srd.run_state = DECEL; /* 下个脉冲进入减速阶段 */ } else if (new_step_delay <= g_srd.min_delay) { /* 检查是否到达期望的最大速度 计数值越小速度越快，当你的速度和最大速度相等或更快就进入匀速*/ last_accel_delay = new_step_delay; /* 保存加速过程中最后一次延时（脉冲周期）*/ new_step_delay = g_srd.min_delay; /* 使用min_delay（对应最大速度speed）*/ rest = 0; /* 清零余值 */ g_srd.run_state = RUN; /* 设置为匀速运行状态 */ } break; case RUN: g_add_pulse_count++; step_count++; /* 步数加1 */ if (g_srd.dir == CW) { g_step_position++; /* 绝对位置加1 */ } else { g_step_position--; /* 绝对位置减1*/ } new_step_delay = g_srd.min_delay; /* 使用min_delay（对应最大速度speed）*/ if (step_count >= g_srd.decel_start) /* 需要开始减速 */ { g_srd.accel_count = g_srd.decel_val; /* 减速步数做为加速计数值 */ new_step_delay = last_accel_delay; /* 加阶段最后的延时做为减速阶段的起始延时(脉冲周期) */ g_srd.run_state = DECEL; /* 状态改变为减速 */ } break; case DECEL: step_count++; /* 步数加1 */ g_add_pulse_count++; if (g_srd.dir == CW) { g_step_position++; /* 绝对位置加1 */ } else { g_step_position--; /* 绝对位置减1 */ } g_srd.accel_count++; /* 计算新(下)一步脉冲周期(时间间隔) */ new_step_delay = g_srd.step_delay - (((2 * g_srd.step_delay) + rest) / (4 * g_srd.accel_count + 1)); /* 计算余数，下次计算补上余数，减少误差 */ rest = ((2 * g_srd.step_delay) + rest) % (4 * g_srd.accel_count + 1); /* 检查是否为最后一步 */ if (g_srd.accel_count >= 0) /* 判断减速步数是否从负值加到0是的话 减速完成 */ { g_srd.run_state = STOP; } break; } g_srd.step_delay = new_step_delay; /* 为下个(新的)延时(脉冲周期)赋值 */ } } }
步进电机S形加减速
梯形加减速特点：
算法简便，占用时少、响应快、效率高，实现方便。
但匀加速和匀减速不符合步进电机速度变化规律，在变速和匀速转折点不能平滑过渡。启动、停止、高速运动段会产生较大的冲击和振动及噪音。
所以这种算法主要应用在对升降速过程要求不高的场合，如简单的定长送料（3D打印机等）。即精密场合下，梯形加减速算法并不适用。
什么是S形加减速
在加减速的变化过程中速度曲线呈现一个英文字母“S”形的，我们称之为S形加减速算法。
S形加减速在启动停止以及高速运动时的速度变化的比较慢，导致冲击力噪音就很小。所以更适用于精密的工件搬运与建造。
S形加减速模型分析
实现S形的方法有很多，在传统的S形曲线加减速算法中，它包括七个运动阶段：
加加速阶段，恒加速阶段，减加速阶段，恒速阶段，加减速阶段，恒定减速阶段和减减速阶段。
即为S行曲线加减速七段式模型，虽然该算法具有平稳、精度高的特点，但是也可以感受到该算法的参数复杂，大大降低到了工作效率，而且对硬件的要求也比较高。
因此将介绍的是五段S曲线加减速算法并通过查表的方式进行实现，这是一种简单但具有实时性和高精度的加减速控制算法，非常适合资源紧凑的小型嵌入式系统。
五段S曲线加减速算法中T1、T2、T3、T4、T5分别代表加加速阶段，减加速阶段、恒速阶段、加减速阶段和减减速阶段的时间。
加加速度的开始点和结束点为零，加速度的斜率是相同的，所以T1 = T2。T4 = T5 = T。
加速度的变化率（J）相等，只是方向相反。
加速段位移和减速段位移算法一样，只是速度排序是倒序。
S形加减速的算法实现
推导过程复杂，可自行百度。
S形加减速整体流程
用户设置参数：设置总步数Step、加速段总时间以及减速段总时间、最大速度
求解未知量：根据已知量，求解中点速度、加速度、加加速段步数以及减加速段步数等
生成速度表：将设置S = 1，计算第一步的时间值以及速度值，并计算出速度表
实现第一步：通过速度表，求第一步的C值，设置定时器比较值以及开启脉冲输出，中断使能
中断服务函数：通过速度表得到每一步的定时器计数值，判断当前运动状态，分别是加速段、匀速段、减速段和停止状态。
步进电机S形加减速实验
硬件资源：TIM8_CH3（PI7）、DIR（PB2）、EN（PF11）。
控制相关：
/****************************************S型加减速运动*****************************************************/ volatile int32_t g_step_pos = 0; /* 当前位置 */ volatile uint16_t g_toggle_pulse = 0; /* 脉冲频率控制 */ motor_state_typedef g_motor_sta = STATE_IDLE; /* 电机状态 */ speed_calc_t g_calc_t = {0} ; __IO uint32_t g_add_pulse_count = 0; /* 脉冲个数累计 */ /** * @brief 速度表计算函数 * @param vo,初速度;vt,末速度;time,加速时间 * @retval TRUE：成功；FALSE：失败 */ uint8_t calc_speed(int32_t vo, int32_t vt, float time) { uint8_t is_dec = FALSE; int32_t i = 0; int32_t vm = 0; /* 中间点速度 */ int32_t inc_acc_stp = 0; /* 加加速所需的步数 */ int32_t dec_acc_stp = 0; /* 减加速所需的步数 */ int32_t accel_step = 0; /* 加速或减速需要的步数 */ float jerk = 0; /* 加加速度 */ float ti = 0; /* 时间间隔 dt */ float sum_t = 0; /* 时间累加量 */ float delta_v = 0; /* 速度的增量dv */ float ti_cube = 0; /* 时间间隔的立方 */ float *velocity_tab = NULL; /* 速度表格指针 */ if (vo > vt) /* 初速度比末速度大,做减速运动,数值变化跟加速运动相同 */ { /* 只是建表的时候注意将速度倒序 */ is_dec = TRUE; /* 减速段 */ g_calc_t.vo = ROUNDPS_2_STEPPS(vt); /* 转换单位 起速:step/s */ g_calc_t.vt = ROUNDPS_2_STEPPS(vo); /* 转换单位 末速:step/s */ } else { is_dec = FALSE; /* 加速段 */ g_calc_t.vo = ROUNDPS_2_STEPPS(vo); g_calc_t.vt = ROUNDPS_2_STEPPS(vt); } time = ACCEL_TIME(time); /* 得到加加速段的时间 */ printf("time=%f\r\n", time); vm = (g_calc_t.vo + g_calc_t.vt) / 2 ; /* 计算中点速度 */ jerk = fabs(2.0f * (vm - g_calc_t.vo) / (time * time)); /* 根据中点速度计算加加速度 */ inc_acc_stp = (int32_t)(g_calc_t.vo * time + INCACCELSTEP(jerk, time)); /* 加加速需要的步数 */ dec_acc_stp = (int32_t)((g_calc_t.vt + g_calc_t.vo) * time - inc_acc_stp); /* 减加速需要的步数 S = vt * time - S1 */ /* 申请内存空间存放速度表 */ accel_step = dec_acc_stp + inc_acc_stp; /* 加速需要的步数 */ if (accel_step % 2 != 0) /* 由于浮点型数据转换成整形数据带来了误差,所以这里加1 */ { accel_step += 1; } /* mallo申请内存空间,记得释放 */ velocity_tab = (float *)(mymalloc(SRAMIN, ((accel_step + 1) * sizeof(float)))); if (velocity_tab == NULL) { printf("内存不足!请修改参数\r\n"); return FALSE; } /* * 目标的S型速度曲线是对时间的方程,但是在控制电机的时候则是以步进的方式控制,所以这里对V-t曲线做转换 * 得到V-S曲线,计算得到的速度表是关于步数的速度值.使得步进电机每一步都在控制当中 */ /* 计算第一步速度,根据第一步的速度值达到下一步的时间 */ ti_cube = 6.0f * 1.0f / jerk; /* 根据位移和时间的公式S = 1/6 * J * ti^3 第1步的时间:ti^3 = 6 * 1 / jerk */ ti = pow(ti_cube, (1 / 3.0f)); /* ti */ sum_t = ti; delta_v = 0.5f * jerk * pow(sum_t, 2); /* 第一步的速度 */ velocity_tab[0] = g_calc_t.vo + delta_v; /*****************************************************/ if (velocity_tab[0] <= SPEED_MIN) /* 以当前定时器频率所能达到的最低速度 */ { velocity_tab[0] = SPEED_MIN; } /*****************************************************/ for (i = 1; i < accel_step; i++) { /* 步进电机的速度就是定时器脉冲输出频率,可以计算出每一步的时间 */ /* 得到第i-1步的时间 */ ti = 1.0f / velocity_tab[i - 1]; /* 电机每走一步的时间 ti = 1 / Vn-1 */ /* 加加速段速度计算 */ if (i < inc_acc_stp) { sum_t += ti; /* 从0开始到i的时间累积 */ delta_v = 0.5f * jerk * pow(sum_t, 2); /* 速度的变化量: dV = 1/2 * jerk * ti^2 */ velocity_tab[i] = g_calc_t.vo + delta_v;/* 得到加加速段每一步对应的速度 */ /* 当最后一步的时候,时间并不严格等于time,所以这里要稍作处理,作为减加速段的时间 */ if (i == inc_acc_stp - 1) { sum_t = fabs(sum_t - time); } } /* 减加速段速度计算 */ else { sum_t += ti; /* 时间累计 */ delta_v = 0.5f * jerk * pow(fabs(time - sum_t), 2); /* dV = 1/2 * jerk *(T-t)^2 看这个逆向看减加速的图 */ velocity_tab[i] = g_calc_t.vt - delta_v; /* V = vt - delta_v */ if (velocity_tab[i] >= g_calc_t.vt) { accel_step = i; break; } } } if (is_dec == TRUE) /* 减速 */ { float tmp_Speed = 0; /* 倒序排序 */ for (i = 0; i < (accel_step / 2); i++) { tmp_Speed = velocity_tab[i]; velocity_tab[i] = velocity_tab[accel_step - 1 - i]; /* 头尾速度对换 */ velocity_tab[accel_step - 1 - i] = tmp_Speed; } g_calc_t.decel_tab = velocity_tab; /* 减速段速度表 */ g_calc_t.decel_step = accel_step; /* 减速段的总步数 */ } else /* 加速 */ { g_calc_t.accel_tab = velocity_tab; /* 加速段速度表 */ g_calc_t.accel_step = accel_step; /* 加速段的总步数 */ } return TRUE; } /** * @brief S型加减速运动 * @param vo:初速度;vt:末速度;AcTime:加速时间;DeTime:减速时间;step:步数; * @retval 无 */ void stepmotor_move_rel(int32_t vo, int32_t vt, float AcTime, float DeTime, int32_t step) { if (calc_speed(vo, vt, AcTime) == FALSE) /* 计算出加速段的速度和步数 */ { return; } if (calc_speed(vt, vo, DeTime) == FALSE) /* 计算出减速段的速度和步数 */ { return; } if (step < 0) { step = -step; ST3_DIR(CCW); } else { ST3_DIR(CW); } if (step >= (g_calc_t.decel_step + g_calc_t.accel_step)) /* 当总步数大于等于加减速度步数相加时，才可以实现完整的S形加减速 */ { g_calc_t.step = step; g_calc_t.dec_point = g_calc_t.step - g_calc_t.decel_step; /* 开始减速的步数 */ } else /* 步数不足以进行足够的加减速 */ { /* 步数不足不足以运动，要把前面申请的速度表所占内存释放，以便后续可重复申请 */ myfree(SRAMIN, g_calc_t.accel_tab); /* 释放加速段速度表 */ myfree(SRAMIN, g_calc_t.decel_tab); /* 释放减速段速度表 */ printf("步数不足，参数设置错误!\r\n"); return; } g_calc_t.step_pos = 0; g_motor_sta = STATE_ACCEL; /* 电机为加速状态 */ g_calc_t.ptr = g_calc_t.accel_tab; /* 把加速段的速度表存储到ptr里边 */ g_toggle_pulse = (uint32_t)(T1_FREQ / (*g_calc_t.ptr)); g_calc_t.ptr++; __HAL_TIM_SET_COUNTER(&g_atimx_handle, 0); __HAL_TIM_SET_COMPARE(&g_atimx_handle, TIM_CHANNEL_3, (uint16_t)(g_toggle_pulse / 2)); /* 设置定时器比较值 */ HAL_TIM_OC_Start_IT(&g_atimx_handle, ATIM_TIMX_PWM_CH3); /* 使能定时器通道 */ ST3_EN(EN_ON); }
定时器中断状态机：
/** * @brief 定时器比较中断 * @param htim：定时器句柄指针 * @note 无 * @retval 无 */ void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim) { volatile uint32_t Tim_Count = 0; volatile uint32_t tmp = 0; volatile float Tim_Pulse = 0; volatile static uint8_t i = 0; if (htim->Instance == TIM8) { i++; /* 定时器中断次数计数值 */ if (i == 2) /* 2次，说明已经输出一个完整脉冲 */ { i = 0; /* 清零定时器中断次数计数值 */ g_step_pos ++; /* 当前位置 */ if ((g_motor_sta != STATE_IDLE) && (g_motor_sta != STATE_STOP)) { g_calc_t.step_pos ++; } switch (g_motor_sta) { case STATE_ACCEL: g_add_pulse_count++; Tim_Pulse = T1_FREQ / (*g_calc_t.ptr); /* 由速度表得到每一步的定时器计数值 */ g_calc_t.ptr++; /* 取速度表的下一位 */ g_toggle_pulse = (uint16_t)(Tim_Pulse / 2); /* 翻转模式C需要除以2 */ if (g_calc_t.step_pos >= g_calc_t.accel_step) /* 当大于加速段步数就进入匀速 */ { myfree(SRAMIN, g_calc_t.accel_tab); /* 运动完要释放内存 */ g_motor_sta = STATE_AVESPEED; } break; case STATE_DECEL: g_add_pulse_count++; Tim_Pulse = T1_FREQ / (*g_calc_t.ptr); /* 由速度表得到每一步的定时器计数值 */ g_calc_t.ptr++; g_toggle_pulse = (uint16_t)(Tim_Pulse / 2); if (g_calc_t.step_pos >= g_calc_t.step) { myfree(SRAMIN, g_calc_t.decel_tab); /* 运动完要释放内存 */ g_motor_sta = STATE_STOP; } break; case STATE_AVESPEED: g_add_pulse_count++; Tim_Pulse = T1_FREQ / g_calc_t.vt; g_toggle_pulse = (uint16_t)(Tim_Pulse / 2); if (g_calc_t.step_pos >= g_calc_t.dec_point) { g_calc_t.ptr = g_calc_t.decel_tab; /* 将减速段的速度表赋值给ptr */ g_motor_sta = STATE_DECEL; } break; case STATE_STOP: HAL_TIM_OC_Stop_IT(&g_atimx_handle, ATIM_TIMX_PWM_CH3); /* 开启对应PWM通道 */ g_motor_sta = STATE_IDLE; break; case STATE_IDLE: break; } } /* 设置比较值 */ Tim_Count = __HAL_TIM_GET_COUNTER(&g_atimx_handle); tmp = 0xFFFF & (Tim_Count + g_toggle_pulse); __HAL_TIM_SET_COMPARE(&g_atimx_handle, TIM_CHANNEL_3, tmp); } }
操作：
#define V_END 300 /* 末速度 */ #define V_START 0 /* 初速度 */ #define ACCELTIME 3.5f /* 加速时间 (s) */ #define DECEELTIME 1.5f /* 减速时间 (s) */ __IO uint16_t g_step_angle = 15; /* 设置的步进步数*/ if(KEY0) { if(g_motor_sta == STATE_IDLE) { g_add_pulse_count=0; stepmotor_move_rel(V_START,V_END,ACCELTIME,DECEELTIME,g_step_angle*SPR );/* 一次加减速运动 */ } }else if(KEY1) { g_step_angle = g_step_angle + 1; if(g_step_angle>=50) g_step_angle = 1; }else if(KEY2) { g_step_angle = g_step_angle - 1; if(g_step_angle<=1) g_step_angle = 50; }
步进电机直线插补
前面都是针对单一步进电机，只能完成和结构相关的简单直线运动（例如丝杆），也称为“一维运动”。
但在实际应用中往往需要工件做斜线甚至圆弧运动，这时就需要通过两个步进电机连轴做同步运动才能实现，称为“多轴联动”。
插补简介
沿规定轮廓，计算起点和终点之间的若干个坐标点，以微小的直线段运动逼近。
插补实质就是进行数据点的密化工作。
常见的插补方式：直线插补、圆弧插补、抛物线插补、样条线插补等。
直线插补：通常用于走斜线或直线轨迹，在此方式中，两点间的插补沿着直线的点群来逼近。
插补算法
脉冲增量插补法：该插补为行程标量插补，常用于开环系统，每次插补结束产生一个行程增量，以脉冲的方式输出，一个脉冲所产生的坐标轴移动量叫做脉冲当量，即通过向各个运动轴分配驱动脉冲来控制机床坐标轴协调运动，从而加工出一定轮廓形状的算法。
数字增量插补法：该插补为事件标量插补，分两步进行。首先计算出插补周期内各坐标轴的增量值，称为粗插补；然后再根据采样得到的实际位置增量计算跟随误差，得到速度指令输出给伺服驱动系统，称为精插补。适用于闭环或半闭环系统。
逐点比较法：每走一步都要将加工点的瞬时坐标同规定的图形轨迹相比较，判断其偏差，然后决定下一步的走向，如果加工点走到图形外面去了，那么下一步就要向图形里面走，如果加工点在图形里，那下一步就往图形外面走，以缩小偏差，这样就能得到一个非常接近规定图形的轨迹。
逐点比较法直线插补原理
逐点比较法不管是直线插补还是圆弧插补，步骤都分为以下四步：
1、偏差判别：根据偏差函数值判别加工点的相对直线位置。
2、坐标进给：沿减小误差的方向进给一步。
3、偏差计算：进给后，计算新加工点相对直线的位置。
4、终点判别：判别是否到达终点，未到达则继续第一步步骤，继续插补，到终点则停止。
偏差判别：
坐标判别：
当我们通过偏差判别知道动点与直线的相对位置后，接着就是坐标进给，需往误差减小的方向进给：
当F = 0时，在直线上，需判断终点坐标x、y的值。当x ≥ y时，动点P往+X方向进给一步，否则往+Y方向进给一步。
当F > 0时，在直线上方，应该向+X方向进给一步。
当F < 0时，在直线下方，应该向+Y方向进给一步。
按照上述方法进行步进，就可以走出逼近目标直线的轮廓出来。
偏差计算：
接着就是偏差判别式的化简：
由上面可知，偏差判别式为：F（Xi，Yi） = Xe * Yi - Xi * Ye;
假设下一步向+X步进，那么此时：F（Xi+1，Yi） = Xe * Yi - （Xi+1）* Ye = Xe * Yi - Xi * Ye - Ye = F（Xi，Yi）- Ye。
假设下一步向+Y步进，那么此时：F（Xi，Yi+1） = Xe * （Yi+1） - Xi * Ye = Xe * Yi - Xi * Ye + Xe = F（Xi，Yi）+ Xe。
经过上述化简后，判别式简单很多，只需要知道终点坐标及上一步的偏差值，便可求出下一步的偏差值。
终点判别：
插补到何时停止呢？常用的终点判别法有三种：总步长法、终点坐标法、投影法。
总步长法：插补前，将终点位置的x、y坐标绝对值相加求和，得出总步数，开始插补时，无论是往哪个轴给进一步，总步数都减一，直到为0时，代表已到终点，就停止插补。
终点坐标法：插补前，先定义x、y轴两个方向的步长计数器，分别存放两个坐标轴方向上应该走的总步数。开始插补后当x、y方向每进给一步，就在相应的计数器中减1，直到两个计数器的值都减为0时，刀具抵达终点，停止插补。
投影法：在插补处理开始之前，先确定所走步数较大的那根轴，并求出该轴运动的总步数，然后存放在总步长计数器中，当对应的轴有进给时，计数器减1，直到计数器为0。相当于终点坐标向值较大的轴做投影，所以叫投影法。
由于总步长法实现简单且易于理解，所以本实验的终点判别使用的是总步长法。
任意象限直线插补的原理
前面都是以第一象限为例，实际上为了完成任意直线插补还需要考虑运动方向在4个象限内的情况，原理同理。
步进电机直线插补实验
编写步骤：
1、初始化定时器：初始化IO，设置ARR、PSC，计数方式以及脉冲输出模式等。
2、电机旋转方向判定：实现任意象限直线插补，需提前判断所属象限，设置电机旋转方向。
3、编写直线插补算法：实现插补第一步，偏差判别，坐标进给，偏差计算并开始脉冲输出与中断使能。
4、编写中断服务函数：实现除第一步以外的其他步数的偏差判别、坐标进给、偏差计算、终点判别。
硬件资源：
步进电机接口三（X轴）：
PI7：TIM8_CH3
PB2：DIR3
PF11：EN3
步进电机接口四（Y轴）：
PC9：TIM8_CH4
PH2：DIR4
PH3：EN4
直线插补实验：
/*****************************************************直线插补实验*****************************************************/ typedef struct { uint16_t pulse_pin; /* 定时器脉冲输出引脚 */ uint32_t pulse_channel; /* 定时器脉冲输出通道 */ uint16_t en_pin; /* 电机使能引脚编号 */ uint16_t dir_pin; /* 电机方向引脚编号 */ GPIO_TypeDef *dir_port; /* 电机方向引脚端口 */ GPIO_TypeDef *en_port; /* 电机使能引脚端口 */ } st_motor_ctr_def; /* 插补算法类型定义 */ typedef struct { __IO uint8_t moving_mode; /* 运动模式 */ __IO uint8_t inter_dir; /* 插补方向 */ __IO uint8_t qua_points; /* 象限点 */ __IO uint8_t x_dir; /* X轴方向 */ __IO uint8_t y_dir; /* Y轴方向 */ __IO int32_t end_x; /* 终点坐标X */ __IO int32_t end_y; /* 终点坐标Y */ __IO uint32_t end_pulse; /* 终点位置总的脉冲数 */ __IO uint32_t act_axis; /* 活动轴 */ __IO int32_t f_e; /* 函数方程 */ } inter_pol_def; inter_pol_def g_pol_par = {0}; /* 直线插补参数值 */ const st_motor_ctr_def st_motor[2] = \ { { STEPMOTOR_TIM_PULSE_PIN_X, STEPMOTOR_TIM_CHANNEL1, STEPMOTOR_X_ENA_PIN, STEPMOTOR_X_DIR_PIN, STEPMOTOR_X_DIR_PORT, STEPMOTOR_X_ENA_PORT }, { STEPMOTOR_TIM_PULSE_PIN_Y, STEPMOTOR_TIM_CHANNEL2, STEPMOTOR_Y_ENA_PIN, STEPMOTOR_Y_DIR_PIN, STEPMOTOR_Y_DIR_PORT, STEPMOTOR_Y_ENA_PORT }, }; __IO st_motor_status_def g_motor_sta = STATE_STOP; /* 步进电机运动状态 */ /** * @brief 步进电机驱动器定时器初始化 * @param 无 * @retval 无 */ void stepmotor_init(void) { __HAL_TIM_MOE_ENABLE(&g_atimx_handle); /* 主输出使能 */ __HAL_TIM_CLEAR_IT(&g_atimx_handle, TIM_IT_UPDATE); /* 清除更新中断标志位 */ } /** * @brief 直线增量插补函数实现直线插补功能,两个步进电机分别向X轴和Y轴步进IncX,IncY步 * @param IncX ：终点X轴坐标 * @param IncY ：终点Y轴坐标 * @param Speed ：进给速度 * @retval 无 */ void line_incmove(uint32_t IncX, uint32_t IncY, uint32_t Speed) { /* 偏差方程置零 */ g_pol_par.f_e = 0; /* 计算起点到终点坐标对应的脉冲数位置*/ g_pol_par.end_x = IncX; g_pol_par.end_y = IncY; g_pol_par.end_pulse = g_pol_par.end_y + g_pol_par.end_x; /* 根据终点判断在直线上的进给方向,减少偏差 */ if (g_pol_par.end_y > g_pol_par.end_x) { g_pol_par.act_axis = AXIS_Y; /* 第一步进给Y轴 */ g_pol_par.f_e = g_pol_par.f_e + g_pol_par.end_x; } else { g_pol_par.act_axis = AXIS_X; /* 第一步进给X轴 */ g_pol_par.f_e = g_pol_par.f_e - g_pol_par.end_y; } /* 设置通道的比较值 */ __HAL_TIM_SET_COMPARE(&g_atimx_handle, st_motor[AXIS_X].pulse_channel, Speed); __HAL_TIM_SET_COMPARE(&g_atimx_handle, st_motor[AXIS_Y].pulse_channel, Speed); __HAL_TIM_SET_AUTORELOAD(&g_atimx_handle, Speed * 2); /* ARR设置为比较值2倍，这样输出的波形就是50%的占空比 */ TIM_CCxChannelCmd(TIM8, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_ENABLE); /* 使能通道输出 */ HAL_TIM_Base_Start_IT(&g_atimx_handle); /* 使能定时器以及开启更新中断 */ g_motor_sta = STATE_RUN; /* 标记电机正在运动 */ } /** * @brief 实现任意象限直线插补 * @param coordsX ：终点X轴坐标 * @param coordsY ：终点Y轴坐标 * @param Speed ：进给速度 * @retval 无 */ void line_inpolation(int32_t coordsX, int32_t coordsY, int32_t Speed) { if (g_motor_sta != STATE_STOP) /* 当前电机正在运转 */ { return ; } /* 其他象限的直线跟第一象限是一样,只是电机运动方向不一样 */ g_pol_par.moving_mode = LINE; if (coordsX < 0) /* 当x轴小于0时，电机方向设为反向*/ { g_pol_par.x_dir = CCW; coordsX = -coordsX; /* 取绝对值 */ ST_LINE_DIR(CCW, AXIS_X); } else { g_pol_par.x_dir = CW; ST_LINE_DIR(CW, AXIS_X); } if (coordsY < 0) /* 当y轴小于0时，电机方向设为反向*/ { g_pol_par.y_dir = CCW; coordsY = -coordsY; /* 取绝对值 */ ST_LINE_DIR(CCW, AXIS_Y); } else { g_pol_par.y_dir = CW; ST_LINE_DIR(CW, AXIS_Y); } line_incmove(coordsX, coordsY, Speed); }
定时器中断回调函数：
/** * @brief 定时器中断回调函数 * @param htim ： 定时器句柄 * @retval 无 */ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { __IO uint32_t axis = 0; /* 进给轴 */ axis = g_pol_par.act_axis; /* 当前进给轴 */ /* 判断是否到达终点或者还没开始运动 */ if (g_pol_par.end_pulse == 0) { return; } /* 根据进给方向 更新坐标值 */ if (g_pol_par.moving_mode == LINE) { if (g_pol_par.f_e > 0) /* 偏差方程 > 0 ,说明当前位置位于直线上方,应向X轴进给 */ { g_pol_par.act_axis = AXIS_X; g_pol_par.f_e = g_pol_par.f_e - g_pol_par.end_y; /* 第一象限的X轴进给时,偏差计算 */ } else if (g_pol_par.f_e < 0) /* 偏差方程 < 0 ,说明当前位置位于直线下方,应向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; g_pol_par.f_e = g_pol_par.f_e + g_pol_par.end_x; /* 第一象限的Y轴进给时,偏差计算 */ } /* 偏差为0的时候,判断x,y轴终点的大小决定进给方向 */ else if (g_pol_par.f_e == 0) /* 偏差方程 = 0 ,说明当前位置位于直线,应判断终点坐标再进给 */ { if (g_pol_par.end_y > g_pol_par.end_x) /* 当Y轴更长的话，应向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; g_pol_par.f_e = g_pol_par.f_e + g_pol_par.end_x; /* 第一象限的Y轴进给时,偏差计算 */ } else { g_pol_par.act_axis = AXIS_X; g_pol_par.f_e = g_pol_par.f_e - g_pol_par.end_y; } } } /* 判断是否需要跟换进给轴 */ if (axis != g_pol_par.act_axis) { TIM_CCxChannelCmd(TIM8, st_motor[axis].pulse_channel, TIM_CCx_DISABLE); TIM_CCxChannelCmd(TIM8, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_ENABLE); } /* 终点判别:总步长 */ g_pol_par.end_pulse--; if (g_pol_par.end_pulse == 0) { g_motor_sta = STATE_STOP; /* 到达终点 */ TIM_CCxChannelCmd(TIM8, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_DISABLE);/* 关闭当前轴输出 */ HAL_TIM_Base_Stop_IT(&g_atimx_handle); /* 停止定时器 */ } }
实验部分：
if(key == KEY0_PRES) /* 按下KEY0开始插补 */ { line_inpolation(5*1600,5*1600, 2000); /* 第一象限直线类似图形'/' */ while(g_motor_sta); line_inpolation(5*1600, -5*1600, 2000); /* 第四象限直线类似图形'\' */ while(g_motor_sta); line_inpolation(-5*1600,-5*1600, 2000); /* 第三象限直线类似图形'/' */ while(g_motor_sta); line_inpolation(-5*1600, 5*1600,2000); /* 第二象限直线类似图形'\' */ while(g_motor_sta); }
实验现象：
步进电机圆弧插补
圆弧插补简介
在圆弧起点与终点间，计算逼近实际圆弧的点群，控制刀具延点运动，加工出圆弧曲线。
逐点比较法圆弧插补原理
偏差判别：
坐标进给：
当我们知道动点与圆弧的相对位置后，接着就是坐标进给，需往误差减小的方向进给。
当F = 0时，在圆弧上，此时不管是往x轴步进还是往y轴步进它们之间的误差是差不多的，所以其运动方向统一默认和F > 0一样，即往-X的方向步进。
注：当在X轴上时，需要往+Y方向步进才可减小误差，其它的位置均与F > 0的步进方向一致。
当F > 0时，在圆弧外部，需要往圆内-X方向进给一步。
当F < 0时，在圆弧内部，需要往圆外+Y方向进给一步。
按照上述方法步进就可以走出类似目标圆弧轮廓。
偏差计算：
终点判别：
本实验使用总步长法，所以在插补处理开始前，先设置一个总步长计数器n：n = |Xe - Xa| + |Ye - Ya|。其中，圆弧终点坐标E（Xe，Ye），起点坐标A（Xa，Ya）。
每当步进一步，计数器n就减一，直到n=0时插补结束。
任意象限圆弧插补原理
可以将表中数据主要分为两组：第一组（SR1、NR2、SR3、NR4）和第二组（NR1、SR2、NR3、SR4），各组之间有共同点如下：
第一组的共同点：
当 Fi ≥ 0 时，Y 轴进给，SR1、NR2走 -Y 方向，SR3、NR4走 +Y 方向；
当 Fi < 0 时，X 轴进给，SR1、NR4走 +X 方向，NR2、SR3走 -X 方向。
第二组的共同点：
当 Fi ≥ 0 时，X 轴进给，NR1、SR4走 -X 方向，SR2、NR3走 +X 方向；
当 Fi < 0 时，Y 轴进给，NR1、SR2走 +Y 方向，NR3、SR4走 -Y 方向。
偏差的计算公式格式是不变的，只是符号的正负以及 X， Y 的区别，所以在代码实现部分需要去判断当前所要走的圆弧属于第几象限以及顺逆方向，再根据表 19.3.2 代入各自所对应的偏差判别式进行偏差计算即可。
步进电机圆弧插补实验
编写步骤：
1、初始化定时器：初始化IO，设置ARR、PSC，计数方式以及脉冲输出模式等。
2、设置圆弧方向：需提前设置好是顺圆弧还是逆圆弧。
3、电机旋转方向判定：实现任意象限直线插补，需提前判断所属象限，设置电机旋转方向。
4、编写圆弧插补算法：实现插补第一步，偏差判别，坐标进给，偏差计算并开始脉冲输出与中断使能。
5、编写中断服务函数：计算动点新坐标，实现除第一步以外的其他步数的偏差判别、坐标进给、偏差计算、终点判别。
硬件资源：
步进电机接口三（X轴）：
PI7：TIM8_CH3
PB2：DIR3
PF11：EN3
步进电机接口四（Y轴）：
PC9：TIM8_CH4
PH2：DIR4
PH3：EN4
设置圆弧插补方向：
/** * @brief 设置圆弧插补方向 * @param coordsX,coordsY:x,y坐标,dir: 圆弧的插补方向 * @retval 无 */ void setarcdir(int32_t coords_x,int32_t coords_y,int8_t dir) { g_pol_par.inter_dir = dir; if(g_pol_par.inter_dir == CCW) { if(coords_x > 0) /* 坐标x > 0 起点在X轴的正半轴,顺时针Y轴进给方向是负的 */ { if(coords_y >= 0) /* 如果y > 0,则是第一象限 */ { g_pol_par.qua_points = FIRST_QUADRANT; g_pol_par.x_dir = CCW; g_pol_par.y_dir = CW; sign_dir[AXIS_X] = -1; sign_dir[AXIS_Y] = 1; ST_LINE_DIR(CCW,AXIS_X); ST_LINE_DIR(CW,AXIS_Y); } else { g_pol_par.qua_points = FOURTH_QUADRANT; g_pol_par.x_dir = CW; /* y <= 0, 在第四象限 */ g_pol_par.y_dir = CW; sign_dir[AXIS_X] = 1; sign_dir[AXIS_Y] = 1; ST_LINE_DIR(CW,AXIS_X); ST_LINE_DIR(CW,AXIS_Y); } } else if(coords_x < 0) /* X轴的负半轴 */ { if(coords_y <= 0) /* y < 0,则是第三象限 */ { g_pol_par.qua_points = THIRD_QUADRANT; g_pol_par.x_dir = CW; g_pol_par.y_dir = CCW; sign_dir[AXIS_X] = 1; sign_dir[AXIS_Y] = -1; ST_LINE_DIR(CW,AXIS_X); ST_LINE_DIR(CCW,AXIS_Y); } else /* y >= 0,第二象限 */ { g_pol_par.qua_points = SECOND_QUADRANT; g_pol_par.x_dir = CCW; g_pol_par.y_dir = CCW; sign_dir[AXIS_X] = -1; sign_dir[AXIS_Y] = -1; ST_LINE_DIR(CCW,AXIS_X); ST_LINE_DIR(CCW,AXIS_Y); } } else if(coords_x == 0) /* x = 0,在Y轴上的特殊起点 */ { if(coords_y > 0) { g_pol_par.qua_points = SECOND_QUADRANT; g_pol_par.x_dir = CCW; g_pol_par.y_dir = CCW; sign_dir[AXIS_X] = -1; sign_dir[AXIS_Y] = -1; ST_LINE_DIR(CCW,AXIS_X); ST_LINE_DIR(CCW,AXIS_Y); } else { g_pol_par.qua_points = FOURTH_QUADRANT; g_pol_par.x_dir = CW; g_pol_par.y_dir = CW; sign_dir[AXIS_X] = 1; sign_dir[AXIS_Y] = 1; ST_LINE_DIR(CW,AXIS_X); ST_LINE_DIR(CW,AXIS_Y); } } } else /* CW方向 */ { if(coords_x > 0) /* 坐标x > 0 起点在X轴的正半轴,顺时针Y轴进给方向是负的 */ { if(coords_y >0) /* 如果y > 0,则是第一象限 */ { g_pol_par.qua_points = FIRST_QUADRANT; g_pol_par.x_dir = CW; g_pol_par.y_dir = CCW; sign_dir[AXIS_X] = 1; sign_dir[AXIS_Y] = -1; ST_LINE_DIR(CW,AXIS_X); ST_LINE_DIR(CCW,AXIS_Y); } else { g_pol_par.qua_points = FOURTH_QUADRANT; g_pol_par.x_dir = CCW; /* y <= 0, 在第四象限 */ g_pol_par.y_dir = CCW; sign_dir[AXIS_X] = -1; sign_dir[AXIS_Y] = -1; ST_LINE_DIR(CCW,AXIS_X); ST_LINE_DIR(CCW,AXIS_Y); } } else if(coords_x < 0) /* X轴的负半轴 */ { if(coords_y < 0) /* y < 0,则是第三象限 */ { g_pol_par.qua_points = THIRD_QUADRANT; g_pol_par.x_dir = CCW; g_pol_par.y_dir = CW; sign_dir[AXIS_X] = -1; sign_dir[AXIS_Y] = 1; ST_LINE_DIR(CCW,AXIS_X); ST_LINE_DIR(CW,AXIS_Y); } else /* y >= 0,第二象限 */ { g_pol_par.qua_points = SECOND_QUADRANT; g_pol_par.x_dir = CW; g_pol_par.y_dir = CW; sign_dir[AXIS_X] = 1; sign_dir[AXIS_Y] = 1; ST_LINE_DIR(CW,AXIS_X); ST_LINE_DIR(CW,AXIS_Y); } } else if(coords_x == 0) /* x = 0,在Y轴上的特殊起点 */ { if(coords_y > 0) { g_pol_par.qua_points = FIRST_QUADRANT; g_pol_par.x_dir = CW; g_pol_par.y_dir = CCW; sign_dir[AXIS_X] = 1; sign_dir[AXIS_Y] = -1; ST_LINE_DIR(CW,AXIS_X); ST_LINE_DIR(CCW,AXIS_Y); } else { g_pol_par.qua_points = THIRD_QUADRANT; g_pol_par.x_dir = CCW; g_pol_par.y_dir = CW; sign_dir[AXIS_X] = -1; sign_dir[AXIS_Y] = 1; ST_LINE_DIR(CCW,AXIS_X); ST_LINE_DIR(CW,AXIS_Y); } } } }
在XOY平面内画任意圆弧：
/** * @brief 在XOY平面内画任意圆弧 * @param start_x_point,start_y_point: 分别是起点坐标X,Y * @param stop_x_point,stop_y_point: 分别是终点坐标X,Y * @param speed: 速度值 * @param dir: 圆弧的方向 * @retval 无 */ void arc_incmove(int32_t start_x_point,int32_t start_y_point,int32_t stop_x_point,int32_t stop_y_point,uint32_t speed,int8_t dir) { if(g_motor_sta == STATE_RUN) /* 当前电机正在运动 */ return ; /* 不符合圆的坐标方程 */ if( (pow(start_x_point,2)+pow(start_y_point,2)) != (pow(stop_x_point,2)+pow(stop_y_point,2))) /* 需要满足半径一致才是一个圆 */ return ; g_pol_par.moving_mode = ARC; /* 圆弧标志 */ g_pol_par.f_e = 0; /* 偏差方程置零 */ g_pol_par.start_point[AXIS_X] = start_x_point; g_pol_par.start_point[AXIS_Y] = start_y_point; g_pol_par.end_x = stop_x_point; g_pol_par.end_y = stop_y_point; /* 设置电机做逆时针圆弧轨迹的运动方向 */ setarcdir(g_pol_par.start_point[AXIS_X],g_pol_par.start_point[AXIS_Y],dir); /* 设置圆弧插补方向 */ /* 计算总的步数 */ g_pol_par.end_pulse = abs((stop_y_point-start_y_point))+ abs((stop_x_point-start_x_point)); /* 从起点到终点的脉冲数 */ /* 起点坐标x = 0,说明起点位于y轴上,此时往X轴进给误差会减少 */ if(g_pol_par.start_point[AXIS_X] == 0) { g_pol_par.act_axis = AXIS_X; /* 第一步给X轴 */ /* 根据圆弧方向决定向X轴进给的偏差方程 */ g_pol_par.f_e = g_pol_par.f_e + sign_dir[AXIS_X]*g_pol_par.start_point[AXIS_X]*2 + 1; /* 偏差方程的计算 */ } else { g_pol_par.act_axis = AXIS_Y; /* 第一步给Y轴 */ g_pol_par.f_e = g_pol_par.f_e + sign_dir[AXIS_Y]*g_pol_par.start_point[AXIS_Y]*2 + 1; /* 偏差方程的计算 */ } ST_LINE_EN(EN,AXIS_X); ST_LINE_EN(EN,AXIS_Y); __HAL_TIM_SET_COMPARE(&g_atimx_handle,st_motor[AXIS_X].pulse_channel,speed); /* 设置通道的比较值 */ __HAL_TIM_SET_COMPARE(&g_atimx_handle,st_motor[AXIS_Y].pulse_channel,speed); __HAL_TIM_SET_AUTORELOAD(&g_atimx_handle,speed*2); /* ARR设置为比较值2倍，这样输出的波形就是50%的占空比 */ TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_ENABLE); HAL_TIM_Base_Start_IT(&g_atimx_handle); /* 使能定时器以及开启更新中断 */ g_motor_sta = STATE_RUN; /* 标记电机正在运动 */ }
定时器中断回调函数：
/** * @brief 定时器中断回调函数 * @param htim ： 定时器句柄 * @retval 无 */ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { __IO uint32_t axis = 0; /* 进给轴 */ axis = g_pol_par.act_axis; /* 当前进给轴 */ /* 判断是否到达终点或者还没开始运动 */ if(g_pol_par.end_pulse == 0) return; /* 计算新的动点坐标 */ if(g_pol_par.act_axis == AXIS_X) { if(g_pol_par.x_dir == CCW) { g_pol_par.start_point[AXIS_X]--; } else { g_pol_par.start_point[AXIS_X]++; } } if(g_pol_par.act_axis == AXIS_Y) { if(g_pol_par.y_dir == CCW) { g_pol_par.start_point[AXIS_Y]--; } else { g_pol_par.start_point[AXIS_Y]++; } } /* 根据进给方向 更新坐标值 */ if(g_pol_par.moving_mode == ARC) { /* 根据上一次的偏差判断下一步进给方向,同时计算下一次的偏差 */ if(g_pol_par.inter_dir == CCW) /* 插补方向:逆时针圆弧 */ { if(g_pol_par.f_e < 0) /* 偏差方程 < 0 ,说明当前位置位于圆弧内侧,应向圆外进给 */ { if( (g_pol_par.qua_points == SECOND_QUADRANT) || (g_pol_par.qua_points == FOURTH_QUADRANT) )/* 第二和第四象限,当偏差<0时都是向X轴进给 */ { g_pol_par.act_axis = AXIS_X; } else /* 在第一和第三象限,偏差<0,都是向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; } } else if(g_pol_par.f_e >= 0) /* 偏差方程 >= 0 ,说明当前位置位于圆弧外侧,应向圆内进给 */ { if( (g_pol_par.qua_points == SECOND_QUADRANT) || (g_pol_par.qua_points == FOURTH_QUADRANT) ) { g_pol_par.act_axis = AXIS_Y; } else { g_pol_par.act_axis = AXIS_X; } } } else { if(g_pol_par.f_e < 0) /* 偏差方程 < 0 ,说明当前位置位于圆弧内侧,应向圆外进给 */ { if( (g_pol_par.qua_points == FIRST_QUADRANT) || (g_pol_par.qua_points == THIRD_QUADRANT) )/* 第一和第三象限,当偏差<0时都是向X轴进给 */ { g_pol_par.act_axis = AXIS_X; } else /* 在第二和第四象限,偏差<0,都是向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; } } else if(g_pol_par.f_e >= 0) /* 偏差方程 >= 0 ,说明当前位置位于圆弧外侧,应向圆内进给 */ { if( (g_pol_par.qua_points == FIRST_QUADRANT) || (g_pol_par.qua_points == THIRD_QUADRANT) ) { g_pol_par.act_axis = AXIS_Y; } else { g_pol_par.act_axis = AXIS_X; } } } /* 计算当前坐标与目标曲线的偏差 */ g_pol_par.f_e = g_pol_par.f_e + 2*sign_dir[g_pol_par.act_axis]*g_pol_par.start_point[g_pol_par.act_axis] + 1; /* 偏差方程的计算 */ } /* 判断是否需要跟换进给轴 */ if(axis != g_pol_par.act_axis) { TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[axis].pulse_channel, TIM_CCx_DISABLE); TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_ENABLE); } /* 终点判别:总步长 */ g_pol_par.end_pulse--; if(g_pol_par.end_pulse == 0) { g_motor_sta = STATE_STOP; /* 到达终点 */ TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_DISABLE); /* 关闭当前轴输出 */ HAL_TIM_Base_Stop_IT(&g_atimx_handle); /* 停止定时器 */ } }
实验操作：
if(key == KEY0_PRES) /* 按下KEY0开始插补 */ { arc_incmove(800 * 5, 0, 0, -800 * 5, 2000, CW); /* 第四象限圆弧 */ while(g_motor_sta); arc_incmove(0, -800 * 5, -800 * 5, 0, 2000, CW); /* 第三象限圆弧 */ while(g_motor_sta); arc_incmove(-800 * 5, 0, 0, 800 * 5, 2000, CW); /* 第二象限圆弧 */ while(g_motor_sta); arc_incmove(0, 800 * 5, 800 * 5, 0, 2000, CW); /* 第一象限圆弧 */ while(g_motor_sta); }
步进电机直线+圆弧插补实验
定时器中断回调函数：
/** * @brief 定时器中断回调函数 * @param htim ： 定时器句柄 * @retval 无 */ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { __IO uint32_t axis = 0; /* 进给轴 */ axis = g_pol_par.act_axis; /* 当前进给轴 */ /* 判断是否到达终点或者还没开始运动 */ if(g_pol_par.end_pulse == 0) return; if(g_pol_par.act_axis == AXIS_X) { if(g_pol_par.x_dir == CCW) { g_pol_par.start_point[AXIS_X]--; } else { g_pol_par.start_point[AXIS_X]++; } } if(g_pol_par.act_axis == AXIS_Y) { if(g_pol_par.y_dir == CCW) { g_pol_par.start_point[AXIS_Y]--; } else { g_pol_par.start_point[AXIS_Y]++; } } /* 根据进给方向 更新坐标值 */ if(g_pol_par.moving_mode == ARC) { /* 根据上一次的偏差判断下一步进给方向,同时计算下一次的偏差 */ if(g_pol_par.inter_dir == CCW) /* 插补方向:逆时针圆弧 */ { if(g_pol_par.f_e < 0) /* 偏差方程 < 0 ,说明当前位置位于圆弧内侧,应向圆外进给 */ { if( (g_pol_par.qua_points == SECOND_QUADRANT) || (g_pol_par.qua_points == FOURTH_QUADRANT) )/* 第二和第四象限,当偏差<0时都是向X轴进给 */ { g_pol_par.act_axis = AXIS_X; } else /* 在第一和第三象限,偏差<0,都是向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; } } else if(g_pol_par.f_e >= 0) /* 偏差方程 >= 0 ,说明当前位置位于圆弧外侧,应向圆内进给 */ { if( (g_pol_par.qua_points == SECOND_QUADRANT) || (g_pol_par.qua_points == FOURTH_QUADRANT) ) { g_pol_par.act_axis = AXIS_Y; } else { g_pol_par.act_axis = AXIS_X; } } } else { if(g_pol_par.f_e < 0) /* 偏差方程 < 0 ,说明当前位置位于圆弧内侧,应向圆外进给 */ { if( (g_pol_par.qua_points == FIRST_QUADRANT) || (g_pol_par.qua_points == THIRD_QUADRANT) )/* 第一和第三象限,当偏差<0时都是向X轴进给 */ { g_pol_par.act_axis = AXIS_X; } else /* 在第二和第四象限,偏差<0,都是向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; } } else if(g_pol_par.f_e >= 0) /* 偏差方程 >= 0 ,说明当前位置位于圆弧外侧,应向圆内进给 */ { if( (g_pol_par.qua_points == FIRST_QUADRANT) || (g_pol_par.qua_points == THIRD_QUADRANT) ) { g_pol_par.act_axis = AXIS_Y; } else { g_pol_par.act_axis = AXIS_X; } } } /* 计算当前坐标与目标曲线的偏差 */ g_pol_par.f_e = g_pol_par.f_e + 2*sign_dir[g_pol_par.act_axis]*g_pol_par.start_point[g_pol_par.act_axis] + 1;/* 偏差方程的计算 */ } if(g_pol_par.moving_mode == LINE) { if(g_pol_par.f_e > 0) /* 偏差方程 > 0 ,说明当前位置位于直线上方,应向X轴进给 */ { g_pol_par.act_axis = AXIS_X; g_pol_par.f_e = g_pol_par.f_e - g_pol_par.end_y; /* 第一象限的X轴进给时,偏差计算 */ } else if(g_pol_par.f_e < 0) /* 偏差方程 < 0 ,说明当前位置位于直线下方,应向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; g_pol_par.f_e = g_pol_par.f_e+g_pol_par.end_x; /* 第一象限的Y轴进给时,偏差计算 */ } /* 偏差为0的时候,判断x,y轴终点的大小决定进给方向 */ else if(g_pol_par.f_e == 0) /* 偏差方程 = 0 ,说明当前位置位于直线,应判断终点坐标再进给 */ { if(g_pol_par.end_y > g_pol_par.end_x) /* 当Y轴更长的话，应向Y轴进给 */ { g_pol_par.act_axis = AXIS_Y; g_pol_par.f_e = g_pol_par.f_e+g_pol_par.end_x; /* 第一象限的Y轴进给时,偏差计算 */ } else { g_pol_par.act_axis = AXIS_X; g_pol_par.f_e = g_pol_par.f_e - g_pol_par.end_y; } } } /* 判断是否需要跟换进给轴 */ if(axis != g_pol_par.act_axis) { TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[axis].pulse_channel, TIM_CCx_DISABLE); TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_ENABLE); } /* 终点判别:总步长 */ g_pol_par.end_pulse--; if(g_pol_par.end_pulse == 0) { g_motor_sta = STATE_STOP; /* 到达终点 */ TIM_CCxChannelCmd(ATIM_TIMX_PWM, st_motor[g_pol_par.act_axis].pulse_channel, TIM_CCx_DISABLE);/* 关闭当前轴输出 */ HAL_TIM_Base_Stop_IT(&g_atimx_handle); /* 停止定时器 */ } }
步进电机闭环控制（位置环）
步进电机闭环系统
闭环系统的组成：步进电机 + 编码器，编码器反馈步进电机的实际旋转位置（脉冲）。
开环系统缺点：单向、无反馈信号、无法纠正偏差、可靠性低。
闭环系统优点：有反馈信号、可消除位置误差、精度更高、更可靠。
电机板的编码器接口
由于编码器是开漏输出的，只有低电平和高阻态。需要高电平时需要接个上拉电阻。
扫一扫
3006
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
