步进电机的基本参数及梯形加减速算法
描述
1 背景
在2021年接触到步进电机，当时是用来驱动热敏打印机，没有用到加减速算法，速度时间表好像是日本客户那边提供过来的，这次调试加减速算法，遇到了不少问题，在这里记录一下，希望能帮到未来对此有困惑的自己，如果能帮到其他人也算是有幸。
2 步进电机基本参数
2.1 驱动器
2.2 电机
2.3 接线
2.4 细分及电流配置
2.5 驱动方式
一般有3种：定时器中断+GPIO、GPIO+延时、PWM比较输出，这里使用的是PWM比较输出模式，在比较中断中不断地更新PWM周期来达到改变速度的目的。
3 梯形加减速算法
3.1 推导过程
3.1.1 组成部分
步进电机的动作过程是由加速、匀速、减速三个部分组成，转速的单位是rpm，每分钟多少转，最终需要rpm转换成每秒的脉冲数也就是频率，速度越快频率越大，对应的脉冲周期T就越小。
3.1.2 速度和脉冲周期的关系
第1个脉冲周期t=t1-t0，第2个脉冲周期t=t2-t1，那么t=计数值*计数时间，例如72MHz的情况下，定时器预分配为72-1,计数值设置为0xFFFF，脉冲周期t=65535*(1/1MHz)，使用PWM的翻转模式，翻转模式下，2次翻转为一个完整脉冲，那么设置比较值为0xFFFF/2，那么t=65.535ms，速度pps=15.2Hz，也就是1秒钟15.2个脉冲。
3.1.3 加速度和距离的关系
s=vot + (1/2)at²，由于初速度为0，那么S=(1/2)*a*t²
s=alpha(步距角)*n(脉冲数),那么(1/2)at²=alpha*n
整个加速时间t=tn-t0，t0=0，那么tn=t，那么tn=√(2*n *alpha/a)
tn=cn*tt(计数时间)
3.1.4 下一个脉冲计数
cn*tt=t(n+1)-tn=√(2*(n-1) *alpha/a)-√(2*(n) *alpha/a)
cn=1/tt (√(n+1)-√n)√(2*alpha/a)
c0=1/tt √(2alpha/a)
cn=c0*(√(n+1)-√n)
根据麦克劳林公式
√(n+-1) = 1+-1/2n-1/8*n²+O(1/n3)
cn/cn-1 = c0(√(n+1)-√n)/c0(√(n)-√n-n)
最终化简后得到
cn=cn-1 - (2cn-1 / 4n+1)
3.1.5 误差和放大倍数
当n=1时有0.4485的偏差,将C0乘以0.676来解决这个误差
假设主频为72M预分频系数为72-1，那么
C0 = 10000000.676√(2alpha/a)
为了保证计算的精度，需要对相关变量进行放大处理，这里对加速度放大10倍
C0=10000000.676 √(2alpha/a10)
那么对2*alpha乘以100000，
根号中2alpha100000/a10=2alpha*10000/a
100*√(2*alpha/a)
对1000000*0.676除以10倍，然后再对整个结果除以10，那么
C0=(10000000.676 /10√(2*alpha/a))/10
3.1.6 相关宏定义
#define FSPR 200
#define MICRO_STEP 8
#define SPR (FSPR*MICRO_STEP)
#define ALPHA 2*3.14159/SPR
#define GAIN_X 10
#define FREQUENCY_1 1000000*0.676/GAIN_X
#define ALPHA_2_GAIN_100000 2ALPHA100000
3.2 单位转换
通常情况下加减速时间(ms)、最大速度(mm/s)、距离(mm)是已知的，我们需要将转速mm/s转换成角速度(rad/s)，将加减速时间转换成加速度(rad/sec^2)
已知公式角速度=转速*2π/60
假设转速有9.55rpm，那么角速度=9.5523.14159/60=1(rad/s)
3.2.1 单位转换相关宏定义
假设轴直径是5mm，默认情况下1转的距离是(5*3.1415)
#define CONFIG_ROUND_MM (5*3.1415)
#define MMPS_TO_RPM(SPEED) (SPEED*60/CONFIG_ROUND_MM)
#define RPM_TO_RADPS(SPEED) (SPEED*2π/60)
#define MMPS_TO_RAD_PS(SPEED) (SPEED*2π/CONFIG_ROUND_MM)
#define MMPS_TO_RAD_0_1_PS(SPEED) (SPEED102π/CONFIG_ROUND_MM)
3.3 代码部分
3.3.1 初始化
PA1作为PWM，PA2作为DIR
TIM_HandleTypeDef htim2;
void TIM_PWM_Init(u16 arr,u16 psc)
{
TIM_OC_InitTypeDef sConfigOC = {0};
htim2.Instance = TIM2;
htim2.Init.Prescaler = psc;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = arr;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
htim2.Init.RepetitionCounter = 0;
HAL_TIM_OC_Init(&htim2);
sConfigOC.OCMode = TIM_OCMODE_TOGGLE;;
sConfigOC.Pulse = arr/2;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_OC_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2);
HAL_TIM_Base_Start(&htim2);
}
void HAL_TIM_OC_MspInit(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM2)
{
GPIO_InitTypeDef GPIO_Initure;
__HAL_RCC_TIM2_CLK_ENABLE();
__HAL_AFIO_REMAP_TIM2_PARTIAL_2();
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_Initure.Pin=GPIO_PIN_1; //PB5
GPIO_Initure.Mode=GPIO_MODE_AF_PP;
GPIO_Initure.Pull=GPIO_PULLUP;
GPIO_Initure.Speed=GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA,&GPIO_Initure);
HAL_NVIC_SetPriority(TIM2_IRQn, 2, 2);
HAL_NVIC_EnableIRQ(TIM2_IRQn);
}
}
3.3.2 加减速算法初始化
输入参数是总步数、加减速时间ms、最大速度mm/s
__IO uint16_t tim_count; /* 达到最大速度时的步数*/
__IO uint32_t max_s_lim; /* 必须要开始减速的步数（如果加速没有达到最大速度）*/
__IO uint32_t accel_lim;
if(g_motion_sta != STOP) /* 只允许步进电机在停止的时候才继续*/
return;
__IO int32_t accel = (speed*1000)/accel_time_ms;
__IO int32_t decel = accel;
if(step < 0) /* 步数为负数 */
{
g_srd.dir = CCW; /* 逆时针方向旋转 */
step = -step; /* 获取步数绝对值 */
}
else
{
g_srd.dir = CW; /* 顺时针方向旋转 */
}
if(step == 1)
{
g_srd.accel_count = -1;
g_srd.run_state = DECEL;
g_srd.step_delay = 1000;
}
else if(step != 0) /* 如果目标运动步数不为0*/
{
g_srd.min_delay = (int32_t)(A_T_x10 /speed); //匀速运行时的计数值
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; /* 电机为运动状态 */
tim_count=__HAL_TIM_GET_COUNTER(&htimx_STEPMOTOR);
__HAL_TIM_SET_COMPARE(&htimx_STEPMOTOR,STEPMOTOR_TIM_CHANNELn,tim_count+g_srd.step_delay/2); /* 设置定时器比较值 */
3.3.4 中断处理
void TIM2_IRQHandler(void)
{
HAL_TIM_IRQHandler(&htim2);
}
void HAL_TIM_OC_DelayElapsedCallbackstepper_ctl_t *p_ctl,(TIM_HandleTypeDef *htim)
{
__IO static uint8_t i = 0;
uint32_t capture = 0;
if(htim==p_ctl->htim)
{
capture = tmr_channel_value_get(p_ctl->tmr_x, p_ctl->timer_channel);
tmr_channel_value_set(p_ctl->tmr_x, p_ctl->timer_channel, capture + p_ctl->step_delay/2);
if (STOP_MONITOR_LEVEL == rt_pin_read(STOP_PIN))
{
stepper_ctl_stop(p_ctl);
}
i++;
if (i == 2)
{
i = 0;
if (IRON_ACTION_RUN == p_ctl->action_type)
{
switch(p_ctl->run_state)
{
case STOP:
{
stepper_ctl_stop(p_ctl);
p_ctl->step_count = 0;
p_ctl->rest_delay = 0;
p_ctl->last_accel_delay = 0;
p_ctl->new_step_delay = 0;
i = 0;
}
break;
case ACCEL:
{
p_ctl->step_count++;
if (p_ctl->dir == CW)
{
p_ctl->step_position++;
}
else
{
p_ctl->step_position--;
}
p_ctl->accel_count++;
p_ctl->new_step_delay = p_ctl->step_delay - (((2 *p_ctl->step_delay) + p_ctl->rest_delay)/(4 * p_ctl->accel_count + 1));
p_ctl->rest_delay = ((2 * p_ctl->step_delay)+p_ctl->rest_delay)%(4 * p_ctl->accel_count + 1);
if (p_ctl->step_count >= p_ctl->decel_start)
{
p_ctl->accel_count = p_ctl->decel_val;
p_ctl->run_state = DECEL;
get_acc_real=p_ctl->step_count;
}
else if (p_ctl->new_step_delay <= p_ctl->min_delay)
{
p_ctl->last_accel_delay = p_ctl->new_step_delay;
p_ctl->new_step_delay = p_ctl->min_delay;
p_ctl->rest_delay = 0;
p_ctl->run_state = RUN;
get_acc_real=p_ctl->step_count;
}
}
break;
case RUN:
{
p_ctl->step_count++;
if (p_ctl->dir == CW)
{
p_ctl->step_position++;
}
else
{
p_ctl->step_position--;
}
p_ctl->new_step_delay = p_ctl->min_delay;
if (p_ctl->step_count >= p_ctl->decel_start)
{
p_ctl->accel_count = p_ctl->decel_val;
p_ctl->new_step_delay = p_ctl->last_accel_delay;
p_ctl->run_state = DECEL;
}
}
break;
case DECEL:
{
p_ctl->step_count++;
if (p_ctl->dir == CW)
{
p_ctl->step_position++;
}
else
{
p_ctl->step_position--;
}
p_ctl->accel_count++;
p_ctl->new_step_delay = p_ctl->step_delay - (((2 * p_ctl->step_delay) + p_ctl->rest_delay)/(4 * p_ctl->accel_count + 1));
p_ctl->rest_delay = ((2 * p_ctl->step_delay)+p_ctl->rest_delay)%(4 * p_ctl->accel_count + 1);
if (p_ctl->accel_count >= 0)
{
p_ctl->run_state = STOP;
}
}
break;
default:
{
}
break;
}
p_ctl->step_delay = p_ctl->new_step_delay;
}
else if (IRON_ACTION_MANUAL == p_ctl->action_type)
{
p_ctl->step_count++;
if (p_ctl->step_count >= p_ctl->manual_limit_steps)
{
stepper_ctl_stop(p_ctl);
p_ctl->step_count = 0;
}
}
else if (IRON_ACTION_RESET == p_ctl->action_type)
{
p_ctl->step_count++;
if (p_ctl->step_count >= p_ctl->manual_limit_steps)
{
stepper_ctl_stop(p_ctl);
p_ctl->step_count = 0;
}
if (ZERO_MONITOR_LEVEL == rt_pin_read(ZERO_PIN))
{
stepper_ctl_stop(p_ctl);
}
}
}
}
}
3.3.5 测试
3.3.5.1 测试生成指定脉冲个数
生成10个脉冲，脉冲宽度为1*0xFFFF/1000000=0.065535S
#define MAX_TIMER_CNT 0xFFFF
TIM_PWM_Init(MAX_TIMER_CNT,TIM_PRESCALER);
uint32_t set_cnt_pwm = 10;
uint32_t get_acc_real = 0;
uint32_t set_cnt_pwm = 10;
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
__IO uint32_t tim_count = 0;
__IO uint32_t tmp = 0;
__IO static uint8_t i = 0;
if(htim->Instance==TIM2)
{
tim_count = __HAL_TIM_GET_COUNTER(&htimx_STEPMOTOR);
tmp = tim_count + MAX_TIMER_CNT/2;
__HAL_TIM_SET_COMPARE(&htimx_STEPMOTOR,STEPMOTOR_TIM_CHANNELn,tmp);
i++;
if(i == 2)
{
i = 0;
if(i == 2)
{
i = 0;
set_cnt_pwm--;
if (0 == set_cnt_pwm)
{
set_cnt_pwm = 10;
HAL_TIM_OC_Stop_IT(&htim2,TIM_CHANNEL_2);
}
}
}
}
}
实测波形
3.3.5.2 测试设置指定参数
如果一圈的脉冲数是1600，位移是5*3.14159mm，那么100mm/s就需要10185.9个脉冲每秒，1000000/10186，那么最大速度时的计数为98，脉冲周期也就是98us左右
#define CONFIG_SPEED_MMPS 100
#define MMPS_TO_RAD_0_1_PS(SPEED) ((SPEED*2*3.14159*10)/5*3.14159)
float set_speed = MMPS_TO_RAD_0_1_PS(CONFIG_SPEED_MMPS);
create_t_ctrl_param(CONFIG_STEPS_PER_ROUND*2, 100, set_speed);
实测波形
A1-A2的时间刚好是100ms，脉冲个数是516个，这个加速阶段的步数比理论值要大，
脉冲周期是100us左右，比理论上要大2us。
3.3.5.3 测试执行连续的动作
假设需要完成2个动作，每一次的距离是2mm，间隔是200ms，2mm对应的步数=204
#define ROUND_UP_CNT_GAIN_100 (50)
#define ROUND_UP(M,N) ((((M*100)/N)+ROUND_UP_CNT_GAIN_100)/100)
#define CONFIG_STEPS_PER_ROUND (1600)
uint32_t distance_to_steps_mm_gain(uint32_t set_mm_gain, uint32_t set_gain)
{
uint32_t set_gain_val = CONFIG_ROUND_MM*set_gain;
return ROUND_UP(set_mm_gain*CONFIG_STEPS_PER_ROUND,set_gain_val);
}
create_t_ctrl_param(distance_to_steps_mm_gain(200,100), 10, set_speed);
HAL_TIM_OC_Start_IT(&htim2,TIM_CHANNEL_2);
while (g_motion_sta)
{
}
delay_ms(200);
create_t_ctrl_param(distance_to_steps_mm_gain(200,100), 10, set_speed);
HAL_TIM_OC_Start_IT(&htim2,TIM_CHANNEL_2);
实测波形
4 实测效果
正向转4次，每次1圈，速度是10mm/s，反向转1次，转1圈，速度是2mm/s
-
STM32：F407步进电机S形加减速算法的实现相关资料分享2021-06-28 0
-
启动停止频繁步进电机加减速算法目的是什么2021-07-07 0
-
步进电机梯形加减速算法的原理是什么2021-07-07 0
-
步进电机驱动算法——S形加减速算法原理 精选资料下载2021-07-08 0
-
F407步进电机梯形加减速算法是如何实现的？2021-07-08 0
-
步进电机梯形匀加速算法是什么2021-07-08 0
-
浅析步进电机S型曲线加减速算法2021-09-03 0
-
怎么实现STM32步进电机S型加减速算法？2021-10-11 0
-
怎么实现步进电机S型曲线加减速算法？2021-10-12 0
-
步进电机加减速算法相关资料分享2022-01-11 0
-
梯形加减速算法实现2022-01-11 0
-
梯形加减速的相关资料推荐2022-01-24 0
-
步进电机S加减速算法与查表方式哪个更好？2023-11-08 0
-
STM32 多步进电机加减速算法精确控制 亲测可用~~~2017-04-02 4939
-
步进电机驱动算法之梯形加减速算法2023-03-17 645
全部0条评论
快来发表一下你的评论吧 !
