bsp_motor.h
#ifndef __BSP_MOTOR_H
#define __BSP_MOTOR_H
#include "main.h"
#include "tim.h"
#define FORM_LEN 11000 //速度表空间大小
#define TIM_FREQ (SystemCoreClock / ((TIM3 -> PSC) + 1)) //频率ft值
#define STEP_ANGLE 1.8f //步进电机的步距角 单位：度
#define FSPR (360.0f / 1.8f) //步进电机的一圈所需脉冲数
#define MICRO_STEP 1 //细分器细分数
#define SPR (FSPR * MICRO_STEP) //细分后一圈所需脉冲数
#define CONVER(speed) (float)(speed * SPR / 60.0f) //根据电机转速(r / min) , 计算电机步速(step / s)
#define MIN_SPEED (float)(TIM_FREQ / 65535) //最低频率 / 速度
#define MOTOR_ENABLE 1
#define MOTOR_DISABLE 0
#define MOTOR_CW 0
#define MOTOR_CCW 1
typedef struct{
__IO uint8_t state; //状态
__IO uint16_t compare; //比较值
__IO uint32_t location; //位置
}motorState_Typedef;
typedef struct{
__IO int32_t vStart; //初始速度
__IO int32_t vUniform; //目标速度
__IO int32_t accelTotalStep; //加速度总步数
__IO int32_t decLocation; //开始减速的位置
__IO int32_t totalStep; //完整曲线总步数
__IO int32_t incAccelTotalStep;//加加速度步数
__IO int32_t decAccelTotalStep;//减加速度步数
__IO float Form[FORM_LEN]; //S加减速 速度表
}motorSpeed_Typedef;
typedef enum{
STOP = 0, //停止状态
ACCEL, //加速状态
UNIFORM, //匀速状态
DECEL, //减速状态
}motorStateEnum_Typedef;
/**
* @brief 电机使能/失能
* @param 参数
* @retval 无
* @note 无
*/
__STATIC_INLINE void Motor_Turn(uint8_t flag)
{
if(flag)
{
HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port , MOTOR_EN_Pin , GPIO_PIN_RESET);
}else{
HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port , MOTOR_EN_Pin , GPIO_PIN_SET);
}
}
/**
* @brief 电机顺/逆时针旋转
* @param 参数一:方向，0为顺时针旋转 ， 1为逆时针旋转
* @retval 无
* @note 无
*/
__STATIC_INLINE void Motor_Dir(uint8_t dir)
{
if(dir)
{
HAL_GPIO_WritePin(MOTOR_DIR_GPIO_Port , MOTOR_DIR_Pin , GPIO_PIN_SET);
}else{
HAL_GPIO_WritePin(MOTOR_DIR_GPIO_Port , MOTOR_DIR_Pin , GPIO_PIN_RESET);
}
}
extern motorSpeed_Typedef motorSpeed;
extern motorState_Typedef motorState;
bool Stepper_Move_S(int16_t end_speed , float acc_tim , int32_t step);
#endif
bsp_motor.c
#include "bsp_motor.h"
motorSpeed_Typedef motorSpeed = {0};
motorState_Typedef motorState = {0};
bool Generate_Speed_Table(int32_t vUniform , float accel_tim)
{
float vMiddle = 0.0f; //中间点速度
float k = 0.0f; //加加速度
float ti = 0.0f; //时间间隔 dt
float sumT = 0.0f; //时间累加量
float deltaV = 0.0f; //速度的增量dv
float temp = 0.0f; //中间变量
motorSpeed.vUniform = CONVER(vUniform);
accel_tim /= 2.0f; //加加速段的时间(加速度斜率>0的时间)
vMiddle = motorSpeed.vUniform / 2.0f; //计算中点的速度
k = fabsf((2.0f * vMiddle) / (accel_tim * accel_tim)); //根据中点速度计算加加速度
motorSpeed.incAccelTotalStep = (int32_t)((k * accel_tim * accel_tim * accel_tim) / 6.0f); //加加速度需要的步数
accel_tim *= 2.0f;
motorSpeed.decAccelTotalStep = (int32_t)(((k * accel_tim * accel_tim * accel_tim) / 6.0f) - motorSpeed.incAccelTotalStep); //减加速度需要的步数
accel_tim /= 2.0f;
/* 计算共需要的步数 ， 并校验内存大小 ， 申请内存空间存放速度表*/
motorSpeed.accelTotalStep = motorSpeed.decAccelTotalStep + motorSpeed.incAccelTotalStep; //加速需要的步数
if(motorSpeed.accelTotalStep % 2 != 0)
{
motorSpeed.accelTotalStep += 1;
}
if(FORM_LEN < motorSpeed.accelTotalStep)
{
printf("FORM_LEN 缓存长度不足\r\n , 请将 FORM_LEN修改为%d\r\n" , motorSpeed.accelTotalStep);
return false;
}
ti = pow(6.0f * 1.0f / k , 1.0f / 3.0f);
sumT += ti;
deltaV = 0.5f * k * sumT * sumT;
motorSpeed.Form[0] = deltaV;
if(motorSpeed.Form[0] <= MIN_SPEED)
{
motorSpeed.Form[0] = MIN_SPEED;
}
for(int i = 1 ; i < motorSpeed.accelTotalStep ; i ++)
{
ti = 1.0f / motorSpeed.Form[i - 1];
if(i < motorSpeed.incAccelTotalStep)
{
sumT += ti;
deltaV = 0.5f * k * sumT * sumT;
motorSpeed.Form[i] = deltaV;
if(i == motorSpeed.incAccelTotalStep - 1)
{
sumT = fabsf(sumT - accel_tim);
}
}else{
sumT += ti;
temp = fabsf(accel_tim - sumT);
deltaV = 0.5f * k * temp * temp;
motorSpeed.Form[i] = motorSpeed.vUniform - deltaV;
}
}
return true;
}
/**
* @brief 速度决策
* @param pScurve：S曲线结构体指针
* @retval 无
* @note 在中断中使用，每进一次中断，决策一次
*/
void Speed_Decision(void)
{
static uint8_t i = 0;
static uint32_t index = 0;
i ++;
if(i == 2)
{
i = 0;
switch(motorState.state)
{
case ACCEL:
if(motorState.location >= (motorSpeed.accelTotalStep - 1))
{
motorState.state = UNIFORM;
index -= 1;
break;
}
motorState.compare = (uint16_t)(TIM_FREQ / motorSpeed.Form[index] / 2.0f);
index ++;
break;
case DECEL:
if(motorState.location >= (motorSpeed.totalStep - 1))
{
HAL_TIM_OC_Stop_IT(&htim3 , TIM_CHANNEL_1);
Motor_Turn(MOTOR_DISABLE);
memset((void*)motorSpeed.Form , 0 , sizeof(float) * FORM_LEN);
index = 0;
motorState.state = STOP;
break;
}
motorState.compare = (uint16_t)(TIM_FREQ / motorSpeed.Form[index] / 2.0f);
index --;
break;
case UNIFORM:
if(motorState.location >= motorSpeed.decLocation)
{
motorState.state = DECEL;
}
break;
}
motorState.location ++;
}
uint32_t tim_count = __HAL_TIM_GET_COUNTER(&htim3);
uint16_t tmp = tim_count + motorState.compare;
__HAL_TIM_SET_COMPARE(&htim3 , TIM_CHANNEL_1 , tmp);
}
/**
* @brief 步进电机S曲线加减速
* @param axis：想要控制的轴编号
* @param start_speed：启动速度，单位：转/分钟
* @param end_speed：目标速度，单位：转/分钟
* @param acc_time：加速时间，单位：秒
* @param step：运动步数，单位：步（需考虑细分）
* @retval true：正常
* @retval false：参数设置错误或速度表空间不足
* @note 无
*/
bool Stepper_Move_S(int16_t end_speed , float acc_time , int32_t step)
{
if(motorState.state != STOP)
{
return false;
}
if(Generate_Speed_Table(end_speed , acc_time) != true)
{
return false;
}
if(step < 0)
{
step = -step;
Motor_Dir(MOTOR_CCW);
}else{
Motor_Dir(MOTOR_CW);
}
if(step >= motorSpeed.accelTotalStep * 2)
{
motorSpeed.totalStep = step;
motorSpeed.decLocation = motorSpeed.totalStep - motorSpeed.accelTotalStep;
}else{
printf("加减速参数设置错误! \r\n");
return false;
}
memset(&motorState , 0 , sizeof(motorState));
motorState.state = ACCEL;
motorState.location = 0;
motorState.compare = (uint16_t)(TIM_FREQ / motorSpeed.Form[0]);
__HAL_TIM_SET_COUNTER(&htim3 , 0);
__HAL_TIM_SET_COMPARE(&htim3 , TIM_CHANNEL_1 , motorState.compare / 2.0f);
Motor_Turn(MOTOR_ENABLE);
HAL_TIM_OC_Start_IT(&htim3 , TIM_CHANNEL_1);
return true;
}
/**
* @brief 比较输出中断回调函数
* @param NULL;
* @retval NULL;
*/
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim -> Instance == TIM3)
{
__HAL_TIM_SetCounter(&htim3 , 0);
Speed_Decision();
}
}
浙公网安备 33010602011771号