步进电机梯形加减速开环速度模式的实现

了然一目

已于 2024-01-11 10:39:37 修改

阅读量439

点赞数 6

文章标签：单片机嵌入式硬件算法

于 2024-01-10 11:10:51 首次发布

本文链接：https://blog.csdn.net/TopVB/article/details/135474448

版权

步进电机梯形加减速开环速度模式的实现

背景介绍

背景介绍

很多介绍步进电机梯形加减速的都源于参考书目《AVR446_Linear speed control of stepper motor.pdf》，正点原子，野火，硬石等都有相应的教程，都讲了固定脉冲数时的梯形加减速原理及对应程序。
在实际项目中，步进除了工作于精确的位置模式，有时还需要步进电机工作于速度模式，并可以方便的更改其旋转速度。本文主要解决步进工作在速度模式下如何通过梯形加减速来实现变速运行。

梯形加减速算法原理详解

梯形加减速详细的算法原理有很多资料参考，不再本文范围内，后续主要介绍梯形加减速如何工作在速度模式下。基本原理介绍可以参考此处步进电机梯形加减速实现

如何工作于速度模式下

只需要将工作模式设置于速度模式，加速完毕后，一直处于RUN即可

步进处于运行时，如何更改速度及加速度

根据新速度，及步进当前速度，判断步进应加速，减速或运行。
当处于RUN时，当前速度就是之前的设定速度。
当处于加减速时当前速度可以由下面公式获得。

cur_speed = (int32_t)(A_T_x10 /step_delay);

根据新旧加减速度，判断是否更新accel_count
在这里插入图片描述
通过公式8我们可以计算出新的加速时，对应的C0值，注意此处无需乘以0.676这个修正系数。
通过公式9，我们可以反算出脉冲周期数n=(Cn/C0-C0/Cn)^2/4
Cn是我们当前的定时器周期计数值，C0由新的加速度计算获得。我们就可以得到当前速度对应于新的加速度时所处的脉冲周期数n。用n更新accel_count即可改变加速度大小。
注意此方法忽略了rest的影响。

具体实现代码

#define T1_FREQ_148_NO_676 ((float)((T1_FREQ ) / 10)) 
/// @brief 计算当前速度在新的加速度下的位于第n次脉冲
/// @param cur_delay 当前速度对应的定时器计数值
/// @param accel  新的加速度
/// @return 返回当前速度在新的加速度下的脉冲数
uint32_t get_Accel_Count(uint32_t cur_delay, uint32_t accel)
{
    int32_t C0_delay;
    float ftemp;
    // 计算C0
    C0_delay = (int32_t)((T1_FREQ_148_NO_676 * sqrt(A_SQ / accel)) / 10);
    // 公式Cn/C0=(sqrt(n+1)-sqrt(n)) 逆运算 n=(Cn/C0-C0/Cn)^2/4
    // 计算出当前速度(定时器计数值cur_delay)对应的n
    ftemp = (float)cur_delay / C0_delay - (float)C0_delay / cur_delay;
    // 由新的accel更新accel_count
    return (ftemp * ftemp) / 4;
}
/// @brief 电机旋转函数
/// @param speed 速度 speed更改大于SPEED_THRESHOLD才有效
/// @param accel 加减速 accel更改大于ACCEL_THRESHOLD才有效 speed大于当前速度 则表示加速度 speed小于当前速度 则表示减速度
/// @param dir 方向 仅启动时有效，在更改速度时，无效。如需更改方向，需要先停止再换向。
/// @param channel 通道号
void STEPMOTOR_Rotate(uint32_t speed, uint32_t accel, uint8_t dir, uint8_t channel)
{
    stcPluseChannelState *pPlusChState;
    pPlusChState = &pPluseChannel[channel].State;
    // 达到最大速度时的步数
    // uint32_t max_s_lim;
    uint32_t cur_speed;
    uint32_t old_speed;
    uint32_t old_accel;

    if (pPluseChannel[channel].Para.movemode == FIXEDLENGTH_MODE && pPlusChState->run_state != STOP)
        return;
    if (pPlusChState->run_state == STOP)
    {
        if (speed == 0 || accel == 0)
            return;
        // 记录参数
        pPluseChannel[channel].Para.dir = dir;
        pPluseChannel[channel].Para.movemode = SPEED_MODE;
        pPluseChannel[channel].Para.set_speed = speed;
        pPluseChannel[channel].Para.step_accel = accel;
        pPluseChannel[channel].Para.step_decel = accel;
        pPlusChState->dir = dir;
        pPlusChState->steps = 0;
        pPlusChState->step_count = 0;
        // pPlusChState->step_position = 0;

        pPlusChState->min_delay = (int32_t)(A_T_x10 / speed);
        pPlusChState->step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel)) / 10);

        pPlusChState->run_state = ACCEL;        // 以加速开始，遇到极限之后就停下
        pPlusChState->decel_start = 0xFFFFFFFF; // 开始减速的位置设为最大，步数达不到最大值就不会减速
        // 复位加速度计数值
        // accel_count必须复位否则加速效果有误
        pPlusChState->accel_count = 0;
        STEPMOTOR_StartChannel(dir, channel, pPlusChState->step_delay);
    }
    else
    {
        if (accel == 0)
            return;
        old_speed = pPluseChannel[channel].Para.set_speed; //(int32_t)(A_T_x10 / pPlusChState->min_delay);
        if (abs(speed - old_speed) < SPEED_THRESHOLD)//
        { // 如果目标速度没有改变
            old_accel = pPluseChannel[channel].Para.step_accel;
            // 如更改加速度，就重新计算 accel_count
            if (abs(accel - old_accel) > ACCEL_THRESHOLD)
            {
                pPlusChState->accel_count = get_Accel_Count(pPlusChState->step_delay, accel);

                if (pPlusChState->run_state == DECEL)
                {
                    pPluseChannel[channel].Para.step_decel = accel;
                    if (pPlusChState->accel_count > 0)
                    {
                        pPlusChState->accel_count = -pPlusChState->accel_count;
                    }
                }
                else if (pPlusChState->run_state == ACCEL)
                {
                    pPluseChannel[channel].Para.step_accel = accel;
                }
                else if (pPlusChState->run_state == RUN)
                {
                }
            }
        }
        else
        {
            cur_speed = (int32_t)(A_T_x10 / pPlusChState->step_delay);
            if (abs(speed - cur_speed) < SPEED_THRESHOLD)
            { // 新速度刚好等于当前速度
                if (pPlusChState->accel_count < 0)
                    pPlusChState->accel_count = -pPlusChState->accel_count;
                pPlusChState->steps = 0;
                pPlusChState->step_count = 0;
                pPlusChState->run_state = RUN;
            }
            else if (speed > cur_speed)
            { // 新速度大于当前速度，继续加速
                old_accel = pPluseChannel[channel].Para.step_accel;
                // 如更改加速度，就重新计算 accel_count
                if (abs(accel - old_accel) > ACCEL_THRESHOLD)
                {
                    pPlusChState->accel_count = get_Accel_Count(pPlusChState->step_delay, accel);
                    pPluseChannel[channel].Para.step_accel = accel;
                }
                if (pPlusChState->run_state == DECEL)
                {
                    if (pPlusChState->accel_count < 0)
                    {
                        pPlusChState->accel_count = -pPlusChState->accel_count;
                    }
                }
                else if (pPlusChState->run_state == ACCEL)
                {
                }
                else if (pPlusChState->run_state == RUN)
                {
                }
                pPlusChState->min_delay = (int32_t)(A_T_x10 / speed);
                pPlusChState->run_state = ACCEL;
            }
            else
            { // 新速度小于当前速度，则减速
                old_accel = pPluseChannel[channel].Para.step_decel;
                // 如更改加速度，就重新计算 accel_count
                if (abs(accel - old_accel) > ACCEL_THRESHOLD)
                {
                    pPlusChState->accel_count = get_Accel_Count(pPlusChState->step_delay, accel);
                    pPluseChannel[channel].Para.step_decel = accel;
                }

                if (pPlusChState->accel_count > 0)
                {
                    pPlusChState->accel_count = -pPlusChState->accel_count;
                }
                if (speed > 0)
                {
                    pPlusChState->min_delay = (int32_t)(A_T_x10 / speed);
                }
                else
                {
                    pPlusChState->min_delay = INT32_MAX; // 最大的int32值。
                }
                pPlusChState->run_state = DECEL;
            }
        }
        pPluseChannel[channel].Para.set_speed = speed;
    }
}
//速度决策函数，中断内调用
__STATIC_INLINE void IsrProcess(uint8_t channel)
{
    // 保存新（下）一个延时周期
    uint32_t new_step_delay = 0;
    int32_t temp1 = 0;
    int32_t temp2 = 0;
    __IO static uint32_t last_accel_delay = 0;

    // 记录new_step_delay中的余数，提高下一步计算的精度
    __IO static int32_t rest = 0;
    //	  __IO static  uint8_t flg=0;
    stcPluseChannelState *pPlusChState;
    pPlusChState = &pPluseChannel[channel].State;

    switch (pPlusChState->run_state) // 加减速曲线阶段
    {
    case STOP:
        pPlusChState->step_delay = 0; // 清零步数计数器
        rest = 0;                     // 清零余值
                                      // 关闭通道
        STEPMOTOR_StopChannel(channel);
        //         	flg=0;
        break;
    case ACCEL:
        pPlusChState->step_count++; // 步数加1
        if (pPlusChState->dir == CW)
        {
            pPlusChState->step_position++; // 绝对位置加1
        }
        else
        {
            pPlusChState->step_position--; // 绝对位置减1
        }
        pPlusChState->accel_count++; // 加速计数值加1
        temp1 = ((2 * pPlusChState->step_delay) + rest);
        temp2 = (4 * pPlusChState->accel_count + 1);
        new_step_delay = pPlusChState->step_delay - temp1 / temp2;
        rest = temp1 % temp2;
        //        new_step_delay = pPlusChState->step_delay - (((2 *pPlusChState->step_delay) + rest)/(4 * pPlusChState->accel_count + 1));//计算新(下)一步脉冲周期(时间间隔)
        //        rest = ((2 * pPlusChState->step_delay)+rest)%(4 * pPlusChState->accel_count + 1);// 计算余数，下次计算补上余数，减少误差

        if (new_step_delay <= pPlusChState->min_delay) // 检查是否到达期望的最大速度
        {
            last_accel_delay = new_step_delay;        // 保存加速过程中最后一次延时（脉冲周期）
            new_step_delay = pPlusChState->min_delay; // 使用min_delay（对应最大速度speed）
            rest = 0;                                 // 清零余值
            pPlusChState->run_state = RUN;            // 设置为匀速运行状态
                                                      //            printf("srd.run_state =%d srd.accel_count=%d \r\n",pPlusChState->run_state,pPlusChState->accel_count);
        }
        if (pPluseChannel[channel].Para.movemode == FIXEDLENGTH_MODE)
        {
            if (pPlusChState->step_count >= pPlusChState->decel_start) // 检查是否应该开始减速
            {
                pPlusChState->accel_count = pPlusChState->decel_val; // 加速计数值为减速阶段计数值的初始值
                pPlusChState->run_state = DECEL;                     // 下个脉冲进入减速阶段
            }
            // 检查是否为最后一步
            if (pPlusChState->step_count >= pPlusChState->steps)
            {
                pPlusChState->run_state = STOP;
            }
        }
        break;
    case RUN:
        pPlusChState->step_count++; // 步数加1
        if (pPlusChState->dir == CW)
        {
            pPlusChState->step_position++; // 绝对位置加1
        }
        else
        {
            pPlusChState->step_position--; // 绝对位置减1
        }
        new_step_delay = pPlusChState->min_delay; // 使用min_delay（对应最大速度speed）
        //					flg=0;
        // 检查是否为最后一步//pPlusChState->steps=0表示一直运行
        if (pPluseChannel[channel].Para.movemode == FIXEDLENGTH_MODE)
        {
            if (pPlusChState->step_count >= pPlusChState->decel_start) // 需要开始减速
            {
                pPlusChState->accel_count = pPlusChState->decel_val; // 减速步数做为加速计数值
                new_step_delay = last_accel_delay;                   // 加阶段最后的延时做为减速阶段的起始延时(脉冲周期)
                pPlusChState->run_state = DECEL;                     // 状态改变为减速
            }
            if (pPlusChState->step_count >= pPlusChState->steps)
            {
                pPlusChState->run_state = STOP;
            }
        }
        break;

    case DECEL:
        pPlusChState->step_count++; // 步数加1

        if (pPlusChState->dir == CW)
        {
            pPlusChState->step_position++; // 绝对位置加1
        }
        else
        {
            pPlusChState->step_position--; // 绝对位置减1
        }
        pPlusChState->accel_count++;
        temp1 = ((2 * pPlusChState->step_delay) + rest);
        temp2 = (4 * pPlusChState->accel_count + 1);
        new_step_delay = pPlusChState->step_delay - temp1 / temp2;
        rest = temp1 % temp2;
        //        new_step_delay = pPlusChState->step_delay - (((2 * pPlusChState->step_delay) + rest)/(4 * pPlusChState->accel_count + 1)); //计算新(下)一步脉冲周期(时间间隔)
        //        rest = ((2 * pPlusChState->step_delay)+rest)%(4 * pPlusChState->accel_count + 1);// 计算余数，下次计算补上余数，减少误差
        if (pPluseChannel[channel].Para.movemode == SPEED_MODE)
        {
            // 检查是否为最后一步
            if (pPlusChState->accel_count >= 0)
            {
                if (pPluseChannel[channel].Para.set_speed == 0)
                {
                    pPlusChState->run_state = STOP;
                    break;
                }
            }

            if (new_step_delay > pPlusChState->min_delay)
            {
                new_step_delay = pPlusChState->min_delay;
                pPlusChState->min_delay = pPlusChState->min_delay;
                if (pPlusChState->accel_count < 0)
                    pPlusChState->accel_count = -pPlusChState->accel_count;
                pPlusChState->run_state = RUN;
            }
        }
        else if (pPluseChannel[channel].Para.movemode == FIXEDLENGTH_MODE)
        {
            // 检查是否为最后一步
            if (pPlusChState->accel_count >= 0 || pPlusChState->step_count >= pPlusChState->steps)
            {
                pPlusChState->run_state = STOP;
            }
        }

        break;
    default:
        break;
    }
    pPlusChState->step_delay = new_step_delay; // 为下个(新的)延时(脉冲周期)赋值
    // rt_kprintf("%d %d\r\n", pPlusChState->accel_count, pPlusChState->step_delay);
}

使用示例

    STEPMOTOR_Rotate(1200, 12000 ,0,0);
    rt_thread_mdelay(100);
    STEPMOTOR_Rotate(2400, 12000, 0,0);
    rt_thread_mdelay(200);
    STEPMOTOR_Rotate(2400, 120000, 0,0);
    rt_thread_mdelay(200);
    STEPMOTOR_Rotate(1200, 120000, 0,0);
    rt_thread_mdelay(200);
    STEPMOTOR_Rotate(1200, 12000, 0,0);
    rt_thread_mdelay(100);
    STEPMOTOR_Rotate(2400, 12000, 0,0);
    rt_thread_mdelay(200);
    STEPMOTOR_Rotate(0, 12000, 0,0);