项目中对步进电机运行速度有了新要求，所以尝试实现梯形加减速算法，S形加减速算法。
本文主要实现梯形加减速算法。
原理/思路
- 步进电机有启动频率这一概念。在启动时有一个最大启动频率，在低于最大启动频率的速度下开始运行，然后通过逐渐加速而达到较高的运行速度，减速亦然。
- 梯形加减速算法，可以通过提前设置速度表或通过输入参数计算出速度表，这两种方式来实现（明明是一种）。我的代码中，梯形加速过程中每个频率走10步，再跳变到下一个频率。这个可以根据实际情况进行调整，毕竟加减速过程也不宜过长。
- 之前我对步进电机的控制采用的是主从定时器的方式，但这种方式来进行变频率加减速的实现效果并不理想（自己分析是中断程序的问题）。经过学习（搜刮资料），改用定时器输出比较模式中翻转电平的方式输出PWM波。既节省了定时器的资源，又弥补我的一个知识盲区。
代码实现
- Y_Step_Motor.h和Y_Step_Motor.c，是一个步进电机的控制。
- Y_Step_Motor.h:
#ifndef __Y_STEP_MOTOR_H
#define __Y_STEP_MOTOR_H
#include "sys.h"
#include "delay.h"
#define Left 1
#define Right 0
// Motor Parameter
#define Y_TIM4_FREQ (84000000 / Y_TIM4_Prescaler) // 2MHz
#define Y_TIM4_Pulse (Y_TIM4_FREQ / 500) // 启动频率
#define Y_Per_Accel_Step 10 // 每个速度加速步数
#define Y_SpeedList_LEN 10 // 速度表大小
// Motor State
#define Y_ACCEL 1 // acceleration
#define Y_COSTT 2 // constant
#define Y_DECEL 3 // deceleration
#define Y_UNIFM 4 // uniform
#define Y_STOP 0 // stop
#define TRUE 1
#define FALSE 0
// Y - TIM4
#define Y_TIM4_Prescaler 42
#define Y_TIM4_Period 0xFFFF
#define Y_TIM4_IRQHandler TIM4_IRQHandler
// Calculate
void Y_Calculate_SpeedList(u32 Y_PulseNum);
// Y
void Y_GPIO_Init(void);
void Y_TIM4_Config(void);
void Y_TIM4_IRQHandler(void);
// Y 电机控制函数
void Y_Trapezoid_Output_Left(void);
void Y_Trapezoid_Output_Right(void);
void Y_Uniform_Output_Left(u32 Y_PulseNum);
void Y_Uniform_Output_Right(u32 Y_PulseNum);
void Y_Stop(void);
#endif /* __Y_STEP_MOTOR_H */
/****************************END OF FILE****************************/
- Y_Step_Motor.c
#include "Y_Step_Motor.h"
u8 Y_Stage = 0;
u8 Y_Motion_Status = 0; // 0:停止，1:加速，2:匀速，3:减速, 4:匀速
u32 Y_Step_Position = 0; // 当前位置
float Y_Fre_List[Y_SpeedList_LEN] = {500, 1000, 1500, 2000, 2500,
3000, 3500, 4000, 4500, 5000}; // 频率列表，速度表
u16 Y_Toggle_Pulse[Y_SpeedList_LEN]; // 频率对应的脉冲个数
u32 Y_CosTTNum = 0; // Y 匀速阶段的脉冲个数
/******************** Y - GPIO *********************/
void Y_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOB, ENABLE);
RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM4, ENABLE);
// GPIOB B6
GPIO_PinAFConfig( GPIOB, GPIO_PinSource6, GPIO_AF_TIM4);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; // TIM4_CH1 PB6
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(GPIOB, &GPIO_InitStructure);
// GPIOB B7
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(GPIOB, &GPIO_InitStructure);
}
/******************** Y - TIM4 *********************/
void Y_TIM4_Config(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
// 时钟频率设置
TIM_TimeBaseStructure.TIM_Prescaler = Y_TIM4_Prescaler - 1;
TIM_TimeBaseStructure.TIM_Period = Y_TIM4_Period;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit( TIM4, &TIM_TimeBaseStructure);
TIM_OCInitTypeDef TIM_OCInitStructure;
// 设置工作模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle; // 比较输出模式，反转输出
TIM_OCInitStructure.TIM_Pulse = Y_TIM4_Pulse / 2; // 第一个脉冲是500Hz
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; // 使能比较输出
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; // 输出极性
TIM_OC1Init( TIM4, &TIM_OCInitStructure); // 初始化
TIM_OC1PreloadConfig( TIM4, TIM_OCPreload_Disable); // CH1预装载使能
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init( &NVIC_InitStructure);
}
// 事先写好了速度表，若不事先设置，可以在此计算得出
void Y_Calculate_SpeedList(u32 Y_PulseNum)
{
u8 i_y = 0;
Y_CosTTNum = Y_PulseNum - (Y_Per_Accel_Step * 10 * 2 + 1); // 得到匀速阶段步数
for (i_y = 0; i_y < (u8)Y_SpeedList_LEN; i_y++)
{
Y_Toggle_Pulse[i_y] = (u16)(Y_TIM4_FREQ / Y_Fre_List[i_y]);
// 串口输出速度表
//printf("%d step: frequency: %.2f, pulse:%d.\r\n", i_y, Y_Fre_List[i_y], Y_Toggle_Pulse[i_y]);
}
}
/***** 固定频率 匀速运动 500Hz *****/
void Y_Uniform_Output_Left(u32 Y_PulseNum)
{
Y_Step_Position = 0;
Y_Motion_Status = Y_UNIFM; // 匀速状态 4
Y_CosTTNum = Y_PulseNum;
GPIO_SetBits(GPIOB, GPIO_Pin_7);
delay_us(100);
GPIO_ResetBits(GPIOB, GPIO_Pin_7);
delay_us(125); // > 125us
Y_TIM4_Config();
TIM_ClearITPendingBit( TIM4, TIM_IT_CC1);
TIM_ITConfig( TIM4, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
void Y_Uniform_Output_Right(u32 Y_PulseNum)
{
Y_Step_Position = 0;
Y_Motion_Status = Y_UNIFM; // 匀速状态 4
Y_CosTTNum = Y_PulseNum;
GPIO_ResetBits(GPIOB, GPIO_Pin_7);
delay_us(100);
GPIO_SetBits(GPIOB, GPIO_Pin_7);
delay_us(125); // > 125us
Y_TIM4_Config();
TIM_ClearITPendingBit( TIM4, TIM_IT_CC1);
TIM_ITConfig( TIM4, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
void Y_Stop(void)
{
Y_Step_Position = 0;
Y_Motion_Status = Y_STOP;
TIM_ITConfig( TIM4, TIM_IT_CC1, DISABLE);
TIM_Cmd(TIM4, DISABLE); // 关闭定时器
}
/***** 梯形加减速算法 *****/
void Y_Trapezoid_Output_Left(void)
{
Y_Step_Position = 0;
Y_Motion_Status = Y_ACCEL;
// 驱动器方向信号逻辑电平
GPIO_SetBits(GPIOB, GPIO_Pin_7);
delay_us(100);
GPIO_ResetBits(GPIOB, GPIO_Pin_7);
delay_us(125); // > 125us
Y_TIM4_Config();
TIM_ClearITPendingBit( TIM4, TIM_IT_CC1);
TIM_ITConfig( TIM4, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
void Y_Trapezoid_Output_Right(void)
{
Y_Step_Position = 0;
Y_Motion_Status = Y_ACCEL;
// 驱动器方向信号逻辑电平
GPIO_ResetBits(GPIOB, GPIO_Pin_7);
delay_us(100);
GPIO_SetBits(GPIOB, GPIO_Pin_7);
delay_us(125); // > 125us
Y_TIM4_Config();
TIM_ClearITPendingBit( TIM4, TIM_IT_CC1);
TIM_ITConfig( TIM4, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
/******************** Y - IRQ *********************/
void Y_TIM4_IRQHandler(void)
{
u16 Y_TIM_Count = 0;
static u8 j_y = 0;
volatile static float Y_TIM_Pulse = (Y_TIM4_Pulse / 2); // 第一个脉冲 500Hz
if (TIM_GetITStatus( TIM4, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit( TIM4, TIM_IT_CC1); // 清楚中断标志位
Y_TIM_Count = TIM_GetCapture1(TIM4);
j_y++;
if (j_y == 2)
{
j_y = 0;
if (Y_Motion_Status == Y_ACCEL)
{
Y_Step_Position++;
if (!(Y_Step_Position < Y_Per_Accel_Step))
{
Y_Step_Position = 0;
Y_Stage++; // 某加速阶段频率走完固定步数，取下一加速频率
if (Y_Stage < (Y_SpeedList_LEN - 1))
{
Y_TIM_Pulse = Y_Toggle_Pulse[Y_Stage] / 2; // -1
}
else
{
Y_TIM_Pulse = Y_Toggle_Pulse[Y_Stage] / 2; // 最后一阶段，应该是5kHz
if (Y_CosTTNum > 0)
{
Y_Motion_Status = Y_COSTT;
}
else
{
Y_Motion_Status = Y_DECEL;
}
Y_Stage = 0;
}
}
}
else if (Y_Motion_Status == Y_COSTT)
{
Y_Step_Position++; // 当前将要执行的步数 Step_Position - 1 是已经执行的步数
Y_TIM_Pulse = Y_Toggle_Pulse[Y_SpeedList_LEN - 1] / 2;
if (Y_Step_Position == Y_CosTTNum)
{
Y_Motion_Status = Y_DECEL;
Y_Step_Position = 0;
}
}
else if (Y_Motion_Status == Y_DECEL)
{
Y_Step_Position++;
if (!(Y_Step_Position < Y_Per_Accel_Step))
{
Y_Step_Position = 0;
Y_Stage++;
if (Y_Stage < (Y_SpeedList_LEN + 1))
{
Y_TIM_Pulse = Y_Toggle_Pulse[Y_SpeedList_LEN - Y_Stage] / 2;
}
else
{
TIM_ITConfig( TIM4, TIM_IT_CC1, DISABLE);
TIM_Cmd(TIM4, DISABLE); // 关闭定时器
Y_Stage = 0;
Y_Motion_Status = Y_STOP;
}
}
}
else if (Y_Motion_Status == Y_UNIFM)
{
Y_Step_Position++;
if (Y_Step_Position < Y_CosTTNum)
{
Y_TIM_Pulse = Y_TIM4_Pulse / 2;
}
else
{
TIM_ITConfig( TIM4, TIM_IT_CC1, DISABLE);
TIM_Cmd(TIM4, DISABLE); // 关闭定时器
Y_Step_Position = 0;
Y_Motion_Status = Y_STOP;
}
}
}
}
TIM_SetCompare1(TIM4, (u16)(Y_TIM_Count + Y_TIM_Pulse));
}
/****************************END OF FILE****************************/
- main.c
/**
**************************************************************************************
* @file main.c
* @author SieYuan
* @version V1.0
* @date 2021-01-19
* @brief 实现梯形加减速算法
* @Update
**************************************************************************************
*/
#include "stm32f4xx.h"
#include "sys.h"
#include "led.h"
#include "key.h"
#include "exti.h"
#include "delay.h"
#include "Y_Step_Motor.h"
u16 Y_status = 0; // 0 停止， 1 运动中
u16 RUN_status = 0;
int main(void)
{
/* 程序初始化：对【LED】【KEY】【EXIT】【USART】*/
LED_Init();
KEY_Init();
EXTIx_Init();
Y_GPIO_Init();
delay_init(168);
Y_Calculate_SpeedList(300);
Y_Trapezoid_Output_Right();
while(1);
}
/****************************END OF FILE****************************/
实现效果
小结
-
以上就是梯形加减速算法的简单实现。
-
我这里使用的是TIM4，如果是使用TIM1\TIM8等高级定时器，注意在配置定时器时添加下面这句，详细可查看 STM32：F103/F407定时器主从模式输出精准脉冲个数.一文中后面的部分。
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- CSDN下载链接: 未通过审核。