编程语言
首页 > 编程语言> > STM32:F407步进电机梯形加减速算法的实现

STM32:F407步进电机梯形加减速算法的实现

作者:互联网

项目中对步进电机运行速度有了新要求,所以尝试实现梯形加减速算法,S形加减速算法。

本文主要实现梯形加减速算法。

原理/思路

代码实现

#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****************************/

#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****************************/

/**
**************************************************************************************
* @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****************************/

实现效果

示波器中观察实现效果

小结

TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;

标签:TIM4,void,STM32,TIM,Step,InitStructure,F407,步进,GPIO
来源: https://blog.csdn.net/qq_40435336/article/details/112251607