STM32：F407步进电机S形加减速算法的实现
本文主要实现S形加减速算法。
原理/思路
- 步进电机有启动频率这一概念。在启动时有一个最大启动频率，在低于最大启动频率的速度下开始运行，然后通过逐渐加速而达到较高的运行速度，减速亦然。
- S形加减速算法，也可以通过提前设置速度表或通过输入参数计算出速度表，这两种方式来实现。上一篇梯形加减速算法中我提前设置的速度表，所以这篇我将通过计算的方式得到速度表。
- S形加减速算法的原型是Sigmoid函数，这部分理论 “pengzhihui2012”的步进电机S型曲线加减速算法与实现.中讲解得比较详细，也感谢这篇文章的作者。我也是主要基于这篇文章做了一个复现。
代码实现
- X_Step_Motor.h和X_Step_Motor.c，是一个步进电机的控制。
- X_Step_Motor.h:
#ifndef __X_STEP_MOTOR_H
#define __X_STEP_MOTOR_H
#include "sys.h"
#include "math.h" // exp()
#include - X_Step_Motor.c:
/**
******************************************************************************
* @file X_Step_Motor.c
* @author SieYuan
* @version V1.0
* @date 2021-01-19
* @brief 反转电平输出脉冲，输出固定脉冲数的PWM波
STM32F407 84MHz！ 高级定时器 168MHz
******************************************************************************
*/
#include "X_Step_Motor.h"
X_SpeedList_TypeDef X_Speed;
u32 X_Step_Position = 0; // 当前位置
u8 X_Motion_Status = 0; // 0:停止，1:加速，2:匀速，3:减速
float X_Fre_List[X_SpeedList_LEN]; // 频率列表
u16 X_Toggle_Pulse[X_SpeedList_LEN]; // 频率对应的脉冲个数
u32 X_CosTTNum = 0; // X 匀速阶段的脉冲个数
/******************** X - GPIO *********************/
void X_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM3, ENABLE);
RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOA, ENABLE);
// GPIOA A6
GPIO_PinAFConfig( GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; // TIM3_CH1 - PA6
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; // 复用
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; // 推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // 上拉
GPIO_Init(GPIOA, &GPIO_InitStructure);
// GPIOA A7
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; // 输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
/******************** X - TIM3 *********************/
void X_TIM3_Config(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
// 时钟频率设置
TIM_TimeBaseStructure.TIM_Prescaler = X_TIM3_Prescaler - 1;
TIM_TimeBaseStructure.TIM_Period = X_TIM3_Period;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit( TIM3, &TIM_TimeBaseStructure);
TIM_OCInitTypeDef TIM_OCInitStructure;
// 设置工作模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle; // 比较输出模式，反转输出
TIM_OCInitStructure.TIM_Pulse = X_TIM3_Pulse / 2; // 让第一个脉冲是500Hz
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; // 使能比较输出
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; // 输出极性
TIM_OC1Init( TIM3, &TIM_OCInitStructure); // 初始化
TIM_OC1PreloadConfig( TIM3, TIM_OCPreload_Disable); // CH1预装载使能，修改
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init( &NVIC_InitStructure);
}
/**
* 函数功能：速度表计算函数
* 输入参数：fre_List[], 各个步的频率数组
* toggle_pulse[], 对应频率的Period
* step[], 变速过程中步数要求， 实际加减速阶段总步数 = 1 + 2 * step = 101
* fre_min - 初始频率，Hz / step/s
* fre_max - 最高频率，Hz / step/s
* jerk, S型的平滑程度，越小越平滑
* 说 明：根据速度曲线和加速时间，将数据密集化，即计算每一步的速度值，并存放在内存中。
* 这里采用的数学模型是匀变速直线运动，加加速-减加速-匀速-加减速-减减速。
**/
void X_Calculate_SpeedList(u32 X_PulseNum)
{
u8 i_x = 0;
X_Speed.X_Step = X_Accel_Step;
X_Speed.X_Fre_Min = X_FREQ_MIN;
X_Speed.X_Fre_Max = X_FREQ_MAX;
X_Speed.X_Jerk = X_JERK;
float num_x; // 其实是整数
float molecule_x; // 公式分子
float denominator_x; // 公式分母
num_x = X_Speed.X_Step / 2;
// step = step + 1; // 有一个起始频率，现在用的就是50步。
molecule_x = X_Speed.X_Fre_Max - X_Speed.X_Fre_Min;
X_CosTTNum = X_PulseNum - (2 * X_Speed.X_Step + 1);
for (i_x = 0; i_x < (u8)X_Speed.X_Step; i_x++)
{
denominator_x = 1.0f + (float)exp(-X_Speed.X_Jerk * (i_x - num_x) / num_x);
X_Fre_List[i_x] = X_Speed.X_Fre_Min + molecule_x / denominator_x;
X_Toggle_Pulse[i_x] = (u16)(X_TIM3_FREQ / X_Fre_List[i_x]);
//printf("%d step: frequency: %.2f, pulse:%d.\r\n", i_x, X_Fre_List[i_x], X_Toggle_Pulse[i_x]);
}
}
void X_PWM_S_Output_Left(void)
{
X_Step_Position = 0;
X_Motion_Status = X_ACCEL;
GPIO_SetBits(GPIOA, GPIO_Pin_7);
delay_us(100);
GPIO_ResetBits(GPIOA, GPIO_Pin_7);
delay_us(125); // > 125us
X_TIM3_Config();
TIM_ClearITPendingBit( TIM3, TIM_IT_CC1);
TIM_ITConfig( TIM3, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM3, ENABLE);
}
void X_PWM_S_Output_Right(void)
{
X_Step_Position = 0;
X_Motion_Status = X_ACCEL;
GPIO_ResetBits(GPIOA, GPIO_Pin_7);
delay_us(100);
GPIO_SetBits(GPIOA, GPIO_Pin_7);
delay_us(125); // > 125us
X_TIM3_Config();
TIM_ClearITPendingBit( TIM3, TIM_IT_CC1);
TIM_ITConfig( TIM3, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM3, ENABLE);
}
void X_Uniform_Output_Left(u32 X_PulseNum)
{
X_Step_Position = 0;
X_Motion_Status = X_UNIFM;
X_CosTTNum = X_PulseNum;
GPIO_SetBits(GPIOA, GPIO_Pin_7);
delay_us(100);
GPIO_ResetBits(GPIOA, GPIO_Pin_7);
delay_us(125); // > 125us
X_TIM3_Config();
TIM_ClearITPendingBit( TIM3, TIM_IT_CC1);
TIM_ITConfig( TIM3, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM3, ENABLE);
}
void X_Uniform_Output_Right(u32 X_PulseNum)
{
X_Step_Position = 0;
X_Motion_Status = X_UNIFM;
X_CosTTNum = X_PulseNum;
GPIO_ResetBits(GPIOA, GPIO_Pin_7);
delay_us(100);
GPIO_SetBits(GPIOA, GPIO_Pin_7);
delay_us(125); // > 125us
X_TIM3_Config();
TIM_ClearITPendingBit( TIM3, TIM_IT_CC1);
TIM_ITConfig( TIM3, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM3, ENABLE);
}
void X_Stop(void)
{
TIM_Cmd(TIM3, DISABLE); // 关闭定时器
TIM_ITConfig( TIM3, TIM_IT_CC1, DISABLE);
X_Step_Position = 0;
X_Motion_Status = X_STOP;
}
/******************** X - IRQ *********************/
void X_TIM3_IRQHandler(void)
{
u16 X_TIM_Count = 0;
static u8 j_x = 0;
volatile static float X_TIM_Pulse = (X_TIM3_Pulse / 2); // 第一个脉冲 500Hz
if (TIM_GetITStatus( TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit( TIM3, TIM_IT_CC1); // 清楚中断标志位
X_TIM_Count = TIM_GetCapture1(TIM3);
j_x++;
if (j_x == 2)
{
j_x = 0;
if (X_Motion_Status == X_ACCEL)
{
X_Step_Position++;
if (X_Step_Position < X_SpeedList_LEN)
{
X_TIM_Pulse = X_Toggle_Pulse[X_Step_Position - 1] / 2; // -1
}
else
{
// 加速阶段第50步，配置好下一个状态
X_TIM_Pulse = X_Toggle_Pulse[X_Step_Position - 1] / 2;
if (X_CosTTNum > 0)
{
X_Motion_Status = X_COSTT;
}
else
{
X_Motion_Status = X_DECEL;
}
X_Step_Position = 0;
}
}
else if (X_Motion_Status == X_COSTT)
{
X_Step_Position++; // 当前将要执行的步数 Step_Position - 1 是已经执行的步数
X_TIM_Pulse = X_Toggle_Pulse[X_SpeedList_LEN - 1] / 2;
if (X_Step_Position == X_CosTTNum)
{
X_Motion_Status = X_DECEL;
X_Step_Position = 0;
}
}
else if (X_Motion_Status == X_DECEL)
{
X_Step_Position++;
if (X_Step_Position < (X_SpeedList_LEN + 1))
{
X_TIM_Pulse = X_Toggle_Pulse[X_SpeedList_LEN - X_Step_Position] / 2;
}
else
{
TIM_ITConfig( TIM3, TIM_IT_CC1, DISABLE);
TIM_Cmd(TIM3, DISABLE); // 关闭定时器
X_Step_Position = 0;
X_Motion_Status = X_STOP;
}
}
else if (X_Motion_Status == X_UNIFM)
{
X_Step_Position++;
if (X_Step_Position < X_CosTTNum)
{
X_TIM_Pulse = X_TIM3_Pulse / 2;
}
else
{
TIM_ITConfig( TIM3, TIM_IT_CC1, DISABLE);
TIM_Cmd(TIM3, DISABLE); // 关闭定时器
X_Step_Position = 0;
X_Motion_Status = X_STOP;
}
}
}
}
TIM_SetCompare1(TIM3, (u16)(X_TIM_Count + X_TIM_Pulse));
}
/****************************END OF FILE****************************/
- main.c
/**
**************************************************************************************
* @file main.c
* @author SieYuan
* @version V1.0
* @date 2021-01-19
* @brief 实现 S 形加减速算法
**************************************************************************************
*/
#include "stm32f4xx.h"
#include "sys.h"
#include "led.h"
#include "key.h"
#include "exti.h"
#include "delay.h"
#include "X_Step_Motor.h"
int main(void)
{
/* 程序初始化：对【LED】【KEY】【EXIT】【USART】*/
LED_Init();
KEY_Init();
EXTIx_Init();
X_GPIO_Init();
delay_init(168);
X_Calculate_SpeedList(300);
X_PWM_S_Output_Left();
while(1);
}
/****************************END OF FILE****************************/
实现效果
小结
-
以上就是S形加减速算法的简单实现。此段代码，加速/减速过程为100步，每个频率走1步。这个部分可以通过调整宏定义进行修改。
-
我这里使用的是TIM3，如果是使用TIM1\TIM8等高级定时器，注意在配置定时器时添加下面这句，详细可查看 STM32：F103/F407定时器主从模式输出精准脉冲个数.一文中后面的部分。
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- CSDN下载链接: STM32：F407步进电机S形加减速算法的实现。