#include "bsp_stepping.h"
#include "math.h"
motorCtrl_t MCHandler[MAX_USE_STEPPING_MOTOR_NUM] = {0}; // 加减速曲线变量
static int64_t last_step_position[MAX_USE_STEPPING_MOTOR_NUM] = {0};
void STEPMOTOR_AxisMoveRel(uint8_t ID,int64_t step, uint32_t accel, uint32_t decel, uint32_t speed);
int STEPMOTOR_AxisMoveing(uint8_t ID,int64_t step, uint32_t accel, uint32_t decel, uint32_t speed);
void STEPMOTOR_AxisMoveStop(uint8_t ID,uint8_t ifQuickStop);
osTimerId_t speedCalTimer = NULL;
/** 函数名：motor_enable_ctrl
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：
*/
void motor_enable_ctrl(uint8_t enable)
{
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_14,(GPIO_PinState)(1-enable));
}
/** 函数名：motor_dir_ctrl
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：
*/
void motor_dir_ctrl(uint8_t dir)
{
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,(GPIO_PinState)(1-dir));
}
/** 函数名：motor_error_get
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：
*/
uint8_t motor_error_get(void)
{
return HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_15);
}
/** 函数名：speed_timer_callback
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：XMW
*/
void speed_timer_callback(void *argument)
{
STEPMOTOR_Actual_Speed_Cal();
}
/** 函数名：STEPMOTOR_TIM_Init
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：
*/
void STEPMOTOR_TIM_Init(uint8_t id,TIM_HandleTypeDef *htim,uint32_t channel)
{
if(id >= MAX_USE_STEPPING_MOTOR_NUM)
{
return;
}
MCHandler[id].srd.htimx = htim;
MCHandler[id].srd.tim_channel_x = channel;
MCHandler[id].srd.enable_ctrl = motor_enable_ctrl;
MCHandler[id].srd.dir_ctrl = motor_dir_ctrl;
MCHandler[id].srd.error_get = motor_error_get;
MCHandler[id].isMoving = 0;
MCHandler[id].needrelocat = 0;
MCHandler[id].step_position = 0;
MCHandler[id].taget_position = 0;
MCHandler[id].srd.run_state = STOP;
HAL_TIM_OC_Start_IT(htim,channel);
if(speedCalTimer == NULL)
{
speedCalTimer = osTimerNew(speed_timer_callback, osTimerPeriodic, NULL, NULL);
osTimerStart(speedCalTimer,10);
}
// __HAL_TIM_ENABLE_IT(htim,channel);
// HAL_TIM_Base_Start_IT(htim);
}
/** 函数名：STEPMOTOR_Actual_Speed_Cal
* 功能 ：计算电机速度，10ms调用一次
* 输入 ：
* 输出 ：
* 编写人：XMW
*/
void STEPMOTOR_Actual_Speed_Cal(void)
{
for(uint8_t i=0;i<MAX_USE_STEPPING_MOTOR_NUM;i++)
{
MCHandler[i].actual_velc = (MCHandler[i].step_position - last_step_position[i]) * 100;
last_step_position[i] = MCHandler[i].step_position;
}
}
/** 函数名：STEPMOTOR_handle_get
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：
*/
motorCtrl_t *STEPMOTOR_handle_get(uint8_t id)
{
if(id >= MAX_USE_STEPPING_MOTOR_NUM)
{
return NULL;
}
else
{
return &MCHandler[id];
}
}
/** 函数名：STEPMOTOR_AxisMoveAbs
* 功能 ： 3000 19230.769 plus 6.41
* 输入 ： speed 6.41 plus/s
* 输出 ：
* 编写人：
*/
void STEPMOTOR_AxisMoveAbs(uint8_t ID,int64_t step_pos, uint32_t accel, uint32_t decel, uint32_t speed)
{
__IO int64_t step = 0;
if(ID >= MAX_USE_STEPPING_MOTOR_NUM)
{
return;
}
MCHandler[ID].srd.speed = speed / 641;
MCHandler[ID].srd.accel = accel / 641;
MCHandler[ID].srd.decel = decel / 641;
MCHandler[ID].taget_position = step_pos;
if(MCHandler[ID].step_position == MCHandler[ID].taget_position)
{
return;
}
step = MCHandler[ID].taget_position - MCHandler[ID].step_position;
if(step == 0)
return;
if(MCHandler[ID].isMoving != TRUE)
{
MCHandler[ID].needrelocat = 0;
STEP_DEBUG("STEPMOTOR_AxisMoveRel\n");
STEPMOTOR_AxisMoveRel(ID,step,MCHandler[ID].srd.accel,MCHandler[ID].srd.decel,MCHandler[ID].srd.speed);
}
else
{
STEP_DEBUG("STEPMOTOR_AxisMoveing\n");
if(MCHandler[ID].needrelocat == 1)
{
return;
}
if(STEPMOTOR_AxisMoveing(ID,step,MCHandler[ID].srd.accel,MCHandler[ID].srd.decel,MCHandler[ID].srd.speed) != 1)
{
if(MCHandler[ID].srd.speedflag == 0)
MCHandler[ID].needrelocat = 1;
}
}
}
/** 函数名：STEPMOTOR_AxisMoveRel
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：XMW
*/
void STEPMOTOR_AxisMoveRel(uint8_t ID,int64_t step, uint32_t accel, uint32_t decel, uint32_t speed)
{
__IO uint16_t tim_count;
if(MCHandler[ID].isMoving == TRUE) // 只允许步进电机在停止的时候才继续
{
return;
}
MCHandler[ID].srd.enable_ctrl(TRUE);
if(step < 0) // 步数为负数
{
MCHandler[ID].srd.dir = CCW; // 逆时针方向旋转
step =-step; // 获取步数绝对值
}
else
{
MCHandler[ID].srd.dir = CW; // 顺时针方向旋转
}
MCHandler[ID].srd.dir_ctrl(MCHandler[ID].srd.dir);
if(step == 1) // 步数为1
{
MCHandler[ID].srd.decel_start = -1;
MCHandler[ID].srd.run_state = DECEL; // 减速状态.
MCHandler[ID].srd.step_delay = 1000; // 短延时
}
else if(step != 0) // 如果目标运动步数不为0
{
// 我们的电机控制专题指导手册有详细的计算及推导过程
// 设置最大速度极限, 计算得到min_delay用于定时器的计数器的值。
// min_delay = (alpha / tt)/ w
MCHandler[ID].srd.min_delay = (int32_t)(A_T_x10/speed);
// 通过计算第一个(c0) 的步进延时来设定加速度，其中accel单位为0.1rad/sec^2
// step_delay = 1/tt * sqrt(2*alpha/accel)
// step_delay = ( tfreq*0.676/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100
MCHandler[ID].srd.step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10);
// 计算多少步之后达到最大速度的限制
// max_s_lim = speed^2 / (2*alpha*accel)
MCHandler[ID].srd.max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10));
// 如果达到最大速度小于0.5步，我们将四舍五入为0
// 但实际我们必须移动至少一步才能达到想要的速度
if(MCHandler[ID].srd.max_s_lim == 0){
MCHandler[ID].srd.max_s_lim = 1;
}
// 计算多少步之后我们必须开始减速
// n1 = (n1+n2)decel / (accel + decel)
MCHandler[ID].srd.accel_lim = (uint32_t)(step*decel/(accel+decel));
// 我们必须加速至少1步才能才能开始减速.
if(MCHandler[ID].srd.accel_lim == 0){
MCHandler[ID].srd.accel_lim = 1;
}
// 使用限制条件我们可以计算出减速阶段步数
if(MCHandler[ID].srd.accel_lim <= MCHandler[ID].srd.max_s_lim){
MCHandler[ID].srd.decel_val = MCHandler[ID].srd.accel_lim - step;
}
else{
MCHandler[ID].srd.decel_val = -(MCHandler[ID].srd.max_s_lim*accel/decel);
}
// 当只剩下一步我们必须减速
if(MCHandler[ID].srd.decel_val == 0){
MCHandler[ID].srd.decel_val = -1;
}
// 计算开始减速时的步数
MCHandler[ID].srd.decel_start = step + MCHandler[ID].srd.decel_val;
// 如果最大速度很慢，我们就不需要进行加速运动
if(MCHandler[ID].srd.step_delay <= MCHandler[ID].srd.min_delay){
MCHandler[ID].srd.step_delay = MCHandler[ID].srd.min_delay;
MCHandler[ID].srd.run_state = RUN;
}
else{
MCHandler[ID].srd.run_state = ACCEL;
}
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val;
}
MCHandler[ID].isMoving = TRUE;
// 复位加速度计数值
MCHandler[ID].srd.accel_count = 0;
MCHandler[ID].srd.speedflag = 0;
// MCHandler[ID].isMoving = TRUE; // 电机为运动状态
tim_count=__HAL_TIM_GET_COUNTER(MCHandler[ID].srd.htimx);
__HAL_TIM_SET_COMPARE(MCHandler[ID].srd.htimx,MCHandler[ID].srd.tim_channel_x,tim_count+MCHandler[ID].srd.step_delay); // 设置定时器比较值
// __HAL_TIM_ENABLE_IT(MCHandler[ID].srd.htimx, TIM_IT_CC1);
// HAL_TIM_OC_Start_IT(MCHandler[ID].srd.htimx, MCHandler[ID].srd.tim_channel_x);
TIM_CCxChannelCmd(MCHandler[ID].srd.htimx->Instance, MCHandler[ID].srd.tim_channel_x, TIM_CCx_ENABLE);// 使能定时器通道
}
/** 函数名：STEPMOTOR_AxisMoveStop
* 功能 ：
* 输入 ：
* 输出 ：
* 编写人：XMW
*/
void STEPMOTOR_AxisMoveStop(uint8_t ID,uint8_t ifQuickStop)
{
MCHandler[ID].srd.speedflag = 0;
MCHandler[ID].needrelocat = 0;
if(ifQuickStop)
{
// m_t->srd.run_state = STOP; // 急停状态.
if(MCHandler[ID].srd.run_state == RUN)
{
MCHandler[ID].srd.run_state = DECEL; // 减速状态.
MCHandler[ID].srd.decel_count = -1 * __fabs(MCHandler[ID].srd.decel_val/3.f);
MCHandler[ID].srd.decel_start = 0;
MCHandler[ID].srd.rest = 0;
}
else if(MCHandler[ID].srd.run_state == ACCEL)
{
MCHandler[ID].srd.run_state = DECEL; // 减速状态.
MCHandler[ID].srd.decel_count = -1 * __fabs(MCHandler[ID].srd.accel_count/ MCHandler[ID].srd.decel * MCHandler[ID].srd.accel/3.f);
MCHandler[ID].srd.decel_start = 0;
MCHandler[ID].srd.rest = 0;
// MCHandler[ID].quickflag = 1;
}else if(MCHandler[ID].srd.run_state == STOP)
{
MCHandler[ID].isMoving = FALSE;
// MCHandler[ID].quickflag = 0;
// MCHandler[ID].mvState = MotorStop;
}
else if(MCHandler[ID].srd.run_state == DECEL)
{
if(__fabs(MCHandler[ID].srd.decel_count) > __fabs(MCHandler[ID].srd.decel_val))
{
MCHandler[ID].srd.decel_count = __fabs(MCHandler[ID].srd.decel_val/3.f);
}
else if(MCHandler[ID].srd.decel_count < -2)
{
MCHandler[ID].srd.decel_count = -1 * __fabs(MCHandler[ID].srd.decel_count/3.f);
MCHandler[ID].srd.decel_start = 0;
MCHandler[ID].srd.rest = 0;
}
// MCHandler[ID].quickflag = 1;
}
}
else
{
if(MCHandler[ID].srd.run_state == RUN)
{
MCHandler[ID].srd.run_state = DECEL; // 减速状态.
MCHandler[ID].srd.decel_count = -1 * __fabs(MCHandler[ID].srd.decel_val);;
MCHandler[ID].srd.decel_start = 0;
}
else if(MCHandler[ID].srd.run_state == ACCEL)
{
MCHandler[ID].srd.run_state = DECEL; // 减速状态.
MCHandler[ID].srd.decel_count = -1 * __fabs(MCHandler[ID].srd.accel_count / MCHandler[ID].srd.decel * MCHandler[ID].srd.accel * 0.8f);
MCHandler[ID].srd.decel_start = 0;
}
else if(MCHandler[ID].srd.run_state == STOP)
{
MCHandler[ID].isMoving = FALSE;
// MCHandler[ID].mvState = MotorStop;
}
else if(MCHandler[ID].srd.run_state == DECEL)
{
if(__fabs(MCHandler[ID].srd.decel_count) > __fabs(MCHandler[ID].srd.decel_val))
{
MCHandler[ID].srd.decel_count = __fabs(MCHandler[ID].srd.decel_val);
}
else if(MCHandler[ID].srd.decel_count < -2)
{
MCHandler[ID].srd.decel_count = -1 * __fabs(MCHandler[ID].srd.decel_count/3.f);
MCHandler[ID].srd.decel_start = 0;
MCHandler[ID].srd.rest = 0;
}
}
}
MCHandler[ID].needrelocat = 0;
for(uint32_t i=0;i<50000;i++)
{
__nop();
__nop();
__nop();
}
}
/** 函数名：Move_Speed_Contorl
* 功能 ：移动过程中速度控制
* 输入 ：
* 输出 ：
* 编写人：ZJ
*/
int STEPMOTOR_AxisMoveing(uint8_t ID,int64_t step, uint32_t accel, uint32_t decel, uint32_t speed)
{
if(MCHandler[ID].isMoving != TRUE) // 只允许步进电机在运动的时候才继续
{
return 0;
}
if(MCHandler[ID].srd.dir == CCW && step < 0)
{
MCHandler[ID].srd.dir_ctrl(MCHandler[ID].srd.dir);
step =-step;
}
else if(MCHandler[ID].srd.dir == CW && step > 0)
{
MCHandler[ID].srd.dir_ctrl(MCHandler[ID].srd.dir);
}
else
{
STEPMOTOR_AxisMoveStop(ID,0);
STEP_DEBUG("moving need stop\n");
return -1;
}
if(MCHandler[ID].srd.run_state == DECEL)
{
STEP_DEBUG("moving DECEL\n");
if(MCHandler[ID].srd.speedflag == 1)
{
// STEPMOTOR_AxisMoveStop(ID,0);
return -1;
}
else if(__fabs(step) < __fabs(MCHandler[ID].srd.decel_count))
{
return -1;
}
}
else if(MCHandler[ID].srd.run_state == RUN)
{
STEP_DEBUG("moving RUN\n");
if(__fabs(step) < __fabs(MCHandler[ID].srd.decel_count))
{
STEPMOTOR_AxisMoveStop(ID,0);
return -1;
}
}
else if(MCHandler[ID].srd.run_state == ACCEL)
{
STEP_DEBUG("moving ACCEL\n");
if(__fabs(step) < __fabs(MCHandler[ID].srd.accel_count))
{
STEPMOTOR_AxisMoveStop(ID,0);
return -1;
}
}
STEP_DEBUG("moving run\n");
// MCHandler[ID].srd.de
/*---------------------------------------------*/
/*---------------------------------------------*/
motorCtrl_t tmpMotor = {0};
if(step == 1) // 步数为1
{
STEPMOTOR_AxisMoveStop(ID,0);
return -1;
// tmpMotor.srd.accel_count = -1; // 只移动一步
// tmpMotor.srd.run_state = DECEL; // 减速状态.
// tmpMotor.srd.step_delay = 1000; // 短延时
}
else if(step != 0) // 如果目标运动步数不为0
{
// 我们的电机控制专题指导手册有详细的计算及推导过程
// 设置最大速度极限, 计算得到min_delay用于定时器的计数器的值。
// min_delay = (alpha / tt)/ w
tmpMotor.srd.min_delay = (int32_t)(A_T_x10/speed);
// 通过计算第一个(c0) 的步进延时来设定加速度，其中accel单位为0.1rad/sec^2
// step_delay = 1/tt * sqrt(2*alpha/accel)
// step_delay = ( tfreq*0.676/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100
tmpMotor.srd.step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10);
// 计算多少步之后达到最大速度的限制
// max_s_lim = speed^2 / (2*alpha*accel)
tmpMotor.srd.max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10));
// 如果达到最大速度小于0.5步，我们将四舍五入为0
// 但实际我们必须移动至少一步才能达到想要的速度
if(tmpMotor.srd.max_s_lim == 0){
tmpMotor.srd.max_s_lim = 1;
}
// 计算多少步之后我们必须开始减速
// n1 = (n1+n2)decel / (accel + decel)
tmpMotor.srd.accel_lim = (uint32_t)(step*decel/(accel+decel));
// 我们必须加速至少1步才能才能开始减速.
if(tmpMotor.srd.accel_lim == 0){
tmpMotor.srd.accel_lim = 1;
}
// 使用限制条件我们可以计算出减速阶段步数
if(tmpMotor.srd.accel_lim <= tmpMotor.srd.max_s_lim){
tmpMotor.srd.decel_val = tmpMotor.srd.accel_lim - step;
}
else{
tmpMotor.srd.decel_val = -(tmpMotor.srd.max_s_lim*accel/decel);
}
// 当只剩下一步我们必须减速
if(tmpMotor.srd.decel_val == 0){
tmpMotor.srd.decel_val = -1;
}
// 计算开始减速时的步数
tmpMotor.srd.decel_start = step + tmpMotor.srd.decel_val;
// 如果最大速度很慢，我们就不需要进行加速运动
// if(tmpMotor.srd.step_delay <= tmpMotor.srd.min_delay){
// tmpMotor.srd.step_delay = tmpMotor.srd.min_delay;
// tmpMotor.srd.run_state = RUN;
// }
// else{
// tmpMotor.srd.run_state = ACCEL;
// }
// // 复位加速度计数值
// tmpMotor.srd.accel_count = 0;
}
/*---------------------------------------------*/
/*---------------------------------------------*/
if(MCHandler[ID].srd.run_state == STOP)
{
return -1;
}
else
{
__disable_irq();
MCHandler[ID].srd.speedflag = 0;
if(MCHandler[ID].srd.step_delay < tmpMotor.srd.min_delay)
{
STEP_DEBUG("moving set <<<<\n");
MCHandler[ID].srd.run_state = DECEL;
MCHandler[ID].srd.decel_count = 0-__fabs(MCHandler[ID].srd.decel_val);//0-__fabs(MCHandler[ID].srd.accel_count *accel/decel);//tmpMotor.srd.decel_val;MCHandler[ID].srd.decel_val
MCHandler[ID].srd.min_delay = tmpMotor.srd.min_delay;
MCHandler[ID].srd.decel_start = tmpMotor.srd.decel_start;
MCHandler[ID].srd.max_s_lim = tmpMotor.srd.max_s_lim;
MCHandler[ID].srd.decel_val = tmpMotor.srd.decel_val;
MCHandler[ID].srd.speedflag = 1;
}
else if(MCHandler[ID].srd.step_delay == tmpMotor.srd.min_delay)
{
// STEP_DEBUG("moving set ===\n");
// MCHandler[ID].srd.min_delay = tmpMotor.srd.min_delay;
MCHandler[ID].srd.decel_val = tmpMotor.srd.decel_val;
MCHandler[ID].srd.decel_start = tmpMotor.srd.decel_start;
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val;
MCHandler[ID].srd.run_state = RUN;
}
else
{
STEP_DEBUG("moving set >>>>\n");
if(MCHandler[ID].srd.run_state == ACCEL)
{
MCHandler[ID].srd.min_delay = tmpMotor.srd.min_delay;
MCHandler[ID].srd.decel_val = tmpMotor.srd.decel_val;
MCHandler[ID].srd.decel_start = tmpMotor.srd.decel_start;
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val;
STEP_DEBUG("moving set ACCEL\n");
}
else if(MCHandler[ID].srd.run_state == DECEL)
{
MCHandler[ID].srd.run_state = ACCEL;
MCHandler[ID].srd.min_delay = tmpMotor.srd.min_delay;
MCHandler[ID].srd.decel_val = tmpMotor.srd.decel_val;
MCHandler[ID].srd.decel_start = tmpMotor.srd.decel_start;
MCHandler[ID].srd.accel_count = __fabs(MCHandler[ID].srd.decel_count)*accel/decel; //待测试
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val;
STEP_DEBUG("moving set DECEL\n");
}
else if(MCHandler[ID].srd.run_state == RUN)
{
MCHandler[ID].srd.run_state = ACCEL;
MCHandler[ID].srd.min_delay = tmpMotor.srd.min_delay;
MCHandler[ID].srd.decel_val = tmpMotor.srd.decel_val;
MCHandler[ID].srd.decel_start = tmpMotor.srd.decel_start;
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val;
STEP_DEBUG("moving set RUN\n");
}
}
MCHandler[ID].srd.step_count = 0;
MCHandler[ID].srd.rest = 0;
__enable_irq();
}
return 1;
}
/**
* 函数功能: 定时器比较输出中断回调函数
* 输入参数: htim：定时器句柄指针
* 返 回 值: 无
* 说 明: 无
*/
void STEPMOTOR_TIMx_IRQHandler(uint8_t ID)
{
__IO uint32_t tim_count=0;
__IO uint32_t tmp = 0;
// 清楚定时器中断
// __HAL_TIM_CLEAR_IT(MCHandler[ID].srd.htimx, TIM_IT_CC1);
// 设置比较值
tim_count=__HAL_TIM_GET_COUNTER(MCHandler[ID].srd.htimx);
tmp = tim_count+MCHandler[ID].srd.step_delay;
__HAL_TIM_SET_COMPARE(MCHandler[ID].srd.htimx,MCHandler[ID].srd.tim_channel_x,tmp);
MCHandler[ID].srd.i++; // 定时器中断次数计数值
if(MCHandler[ID].srd.i==2) // 2次，说明已经输出一个完整脉冲
{
MCHandler[ID].srd.i=0; // 清零定时器中断次数计数值
switch(MCHandler[ID].srd.run_state) // 加减速曲线阶段
{
case STOP:
MCHandler[ID].srd.step_count = 0; // 清零步数计数器
MCHandler[ID].srd.rest = 0; // 清零余值
MCHandler[ID].srd.speedflag = 0;
// 关闭通道
TIM_CCxChannelCmd(MCHandler[ID].srd.htimx->Instance, MCHandler[ID].srd.tim_channel_x, TIM_CCx_DISABLE);
// __HAL_TIM_CLEAR_FLAG(MCHandler[ID].srd.htimx, MCHandler[ID].srd.tim_channel_x);
MCHandler[ID].isMoving = FALSE;
MCHandler[ID].srd.decel_count = 0;
MCHandler[ID].srd.accel_count = 0;
MCHandler[ID].taget_position = MCHandler[ID].step_position;
// MotionStatus = 0; // 电机为停止状态
break;
case ACCEL:
MCHandler[ID].isMoving = TRUE;
MCHandler[ID].srd.step_count++; // 步数加1
if(MCHandler[ID].srd.dir==CW)
{
MCHandler[ID].step_position++; // 绝对位置加1
}
else
{
MCHandler[ID].step_position--; // 绝对位置减1
}
MCHandler[ID].srd.accel_count++; // 加速计数值加1
MCHandler[ID].srd.new_step_delay = MCHandler[ID].srd.step_delay - (((2 *MCHandler[ID].srd.step_delay) + MCHandler[ID].srd.rest)/(4 * MCHandler[ID].srd.accel_count + 1));//计算新(下)一步脉冲周期(时间间隔)
MCHandler[ID].srd.rest = ((2 * MCHandler[ID].srd.step_delay)+MCHandler[ID].srd.rest)%(4 * MCHandler[ID].srd.accel_count + 1);// 计算余数，下次计算补上余数，减少误差
if(MCHandler[ID].srd.step_count >= MCHandler[ID].srd.decel_start)// 检查是够应该开始减速
{
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val; // 加速计数值为减速阶段计数值的初始值
MCHandler[ID].srd.run_state = DECEL; // 下个脉冲进入减速阶段
}
else if(MCHandler[ID].srd.new_step_delay <= MCHandler[ID].srd.min_delay) // 检查是否到达期望的最大速度
{
MCHandler[ID].srd.last_accel_delay = MCHandler[ID].srd.new_step_delay; // 保存加速过程中最后一次延时（脉冲周期）
MCHandler[ID].srd.new_step_delay = MCHandler[ID].srd.min_delay; // 使用min_delay（对应最大速度speed）
MCHandler[ID].srd.rest = 0; // 清零余值
MCHandler[ID].srd.run_state = RUN; // 设置为匀速运行状态
}
break;
case RUN:
MCHandler[ID].isMoving = TRUE;
MCHandler[ID].srd.step_count++; // 步数加1
if(MCHandler[ID].srd.dir==CW)
{
MCHandler[ID].step_position++; // 绝对位置加1
}
else
{
MCHandler[ID].step_position--; // 绝对位置减1
}
MCHandler[ID].srd.new_step_delay = MCHandler[ID].srd.min_delay; // 使用min_delay（对应最大速度speed）
if(MCHandler[ID].srd.step_count >= MCHandler[ID].srd.decel_start) // 需要开始减速
{
MCHandler[ID].srd.decel_count = MCHandler[ID].srd.decel_val; // 减速步数做为加速计数值
MCHandler[ID].srd.new_step_delay = MCHandler[ID].srd.last_accel_delay;// 加阶段最后的延时做为减速阶段的起始延时(脉冲周期)
MCHandler[ID].srd.run_state = DECEL; // 状态改变为减速
}
break;
case DECEL:
MCHandler[ID].isMoving = TRUE;
MCHandler[ID].srd.step_count++; // 步数加1
if(MCHandler[ID].srd.dir==CW)
{
MCHandler[ID].step_position++; // 绝对位置加1
}
else
{
MCHandler[ID].step_position--; // 绝对位置减1
}
MCHandler[ID].srd.decel_count++;
MCHandler[ID].srd.new_step_delay = MCHandler[ID].srd.step_delay - (((2 * MCHandler[ID].srd.step_delay) + MCHandler[ID].srd.rest)/(4 * MCHandler[ID].srd.decel_count + 1)); //计算新(下)一步脉冲周期(时间间隔)
MCHandler[ID].srd.rest = ((2 * MCHandler[ID].srd.step_delay)+MCHandler[ID].srd.rest)%(4 * MCHandler[ID].srd.decel_count + 1);// 计算余数，下次计算补上余数，减少误差
//检查是否为最后一步
if(MCHandler[ID].srd.decel_count >= 0)
{
MCHandler[ID].srd.run_state = STOP;
}
//变换速度，高速向低速切换
else if((MCHandler[ID].srd.step_count > MCHandler[ID].srd.decel_start) \
&& MCHandler[ID].srd.step_delay <= MCHandler[ID].srd.min_delay \
&& MCHandler[ID].srd.speedflag == 1) // 需要开始减速
{
STEP_DEBUG("moving decel_start\n");
// m_t->srd.last_accel_delay = m_t->srd.step_delay; // 保存加速过程中最后一次延时（脉冲周期）
MCHandler[ID].srd.rest = 0; // 清零余值
MCHandler[ID].srd.accel_count = __fabs(MCHandler[ID].srd.max_s_lim);
MCHandler[ID].srd.run_state = ACCEL;
MCHandler[ID].srd.speedflag = 0;
}
else if((MCHandler[ID].srd.step_count < MCHandler[ID].srd.decel_start) \
&& MCHandler[ID].srd.step_delay >= MCHandler[ID].srd.min_delay && MCHandler[ID].srd.speedflag == 1)
{
STEP_DEBUG("moving min_delay\n");
// m_t->srd.last_accel_delay = m_t->srd.step_delay; // 保存加速过程中最后一次延时（脉冲周期）
MCHandler[ID].srd.rest = 0; // 清零余值
MCHandler[ID].srd.accel_count = __fabs(MCHandler[ID].srd.max_s_lim);
MCHandler[ID].srd.run_state = ACCEL;
MCHandler[ID].srd.speedflag = 0;
}
break;
}
// MCHandler[ID].srd.current_speed = (int32_t)(A_T_x10/MCHandler[ID].srd.step_delay);
MCHandler[ID].srd.step_delay = MCHandler[ID].srd.new_step_delay; // 为下个(新的)延时(脉冲周期)赋值
}
}
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
/* Prevent unused argument(s) compilation warning */
for(uint8_t i=0;i<MAX_USE_STEPPING_MOTOR_NUM;i++)
{
if(htim->Instance == MCHandler[i].srd.htimx->Instance)
{
STEPMOTOR_TIMx_IRQHandler(i);
}
}
/* NOTE : This function should not be modified, when the callback is needed,
the HAL_TIM_OC_DelayElapsedCallback could be implemented in the user file
*/
}
#ifndef __BSP_STEPPING_H__
#define __BSP_STEPPING_H__
#include "main.h"
#include "tim.h"
#define STEP_Debug 0
#if (STEP_Debug)
#define STEP_DEBUG(...) lwlog_debug(...)
#else
#define STEP_DEBUG(...)
#endif
#ifndef MAX_USE_STEPPING_MOTOR_NUM
#define MAX_USE_STEPPING_MOTOR_NUM 0x01 //最大驱动电机数量
#endif
// 定义定时器周期，输出比较模式周期设置为0xFFFF
#define STEPMOTOR_TIM_PERIOD 0xFFFF
// 定义高级定时器重复计数寄存器值
#define STEPMOTOR_TIM_REPETITIONCOUNTER 0
// 步进电机驱动器细分设置为： 32 细分
// 定义定时器预分频，定时器实际时钟频率为：168MHz/（STEPMOTOR_TIMx_PRESCALER+1）
#define STEPMOTOR_TIM_PRESCALER 400
#define FALSE 0
#define TRUE 1
#define CW 0 // 顺时针
#define CCW 1 // 逆时针
#define STOP 0 // 加减速曲线状态：停止
#define ACCEL 1 // 加减速曲线状态：加速阶段
#define DECEL 2 // 加减速曲线状态：减速阶段
#define RUN 3 // 加减速曲线状态：匀速阶段
#define T1_FREQ 500000
//#define T1_FREQ (SystemCoreClock/(STEPMOTOR_TIM_PRESCALER+1)) // 频率ft值
#define SPR (400) // 旋转一圈需要的脉冲数
// 数学常数
#define ALPHA ((float)(2*3.14159/SPR)) // α= 2*pi/spr
#define A_T_x10 ((float)(10*ALPHA*T1_FREQ))
#define T1_FREQ_148 ((float)((T1_FREQ*0.676)/10)) // 0.676为误差修正值
#define A_SQ ((float)(2*100000*ALPHA))
#define A_x200 ((float)(200*ALPHA))
/* 类型定义 ------------------------------------------------------------------*/
typedef struct {
__IO uint8_t run_state ; // 电机旋转状态
__IO uint8_t dir ; // 电机旋转方向
__IO int32_t step_delay; // 下个脉冲周期（时间间隔），启动时为加速度
__IO int64_t stepNums; // 运行总脉冲数
__IO int32_t decel_start; // 启动减速位置
__IO int32_t decel_val; // 减速阶段步数
__IO int32_t min_delay; // 最小脉冲周期(最大速度，即匀速段速度)
__IO int32_t accel_count; // 加速阶段计数值
__IO int32_t decel_count; // 减速阶段计数值
__IO int32_t new_step_delay; //保存新（下）一个延时周期
__IO int32_t last_accel_delay; // 加速过程中最后一次延时（脉冲周期）.
__IO int32_t step_count ; // 总移动步数计数器
__IO int64_t rest; // 记录new_step_delay中的余数
__IO uint32_t max_s_lim;
__IO uint32_t accel_lim;
TIM_HandleTypeDef *htimx;
uint32_t tim_channel_x;
__IO uint8_t i; //回调函数执行次数
__IO int32_t current_speed;
void (*enable_ctrl)(uint8_t);
void (*dir_ctrl)(uint8_t);
uint8_t (*error_get)(void);
__IO uint8_t speedflag;
__IO uint32_t accel;
__IO uint32_t decel;
__IO uint32_t speed;
}speedRampData;
typedef struct {
speedRampData srd;
__IO uint8_t needrelocat;
__IO uint8_t isMoving;
__IO int64_t step_position; //当前绝对位置 单位 脉冲
__IO int64_t taget_position; //目标位置 单位 脉冲
__IO int16_t actual_velc; //实际速度 单位 脉冲/s
}motorCtrl_t;
void STEPMOTOR_TIMx_IRQHandler(uint8_t ID);
void STEPMOTOR_TIM_Init(uint8_t id,TIM_HandleTypeDef *htim,uint32_t channel);
motorCtrl_t *STEPMOTOR_handle_get(uint8_t id);
void STEPMOTOR_AxisMoveAbs(uint8_t ID,int64_t step_pos, uint32_t accel, uint32_t decel, uint32_t speed);
void STEPMOTOR_AxisMoveStop(uint8_t ID,uint8_t ifQuickStop);
void STEPMOTOR_Actual_Speed_Cal(void);
//int STEPMOTOR_TIMx_GET(TIM_HandleTypeDef *htim);
#endif
Comments | NOTHING