步进电机S加减速，SPTA结合S加减速

goethe

已于 2023-12-01 08:47:23 修改

阅读量1.5k

点赞数 1

文章标签： S型加减速 spta加减速步进电机

于 2023-12-01 08:43:47 首次发布

原文链接：https://blog.csdn.net/qq_41544116/article/details/106690377

版权

在网格图上，速度的变化现是缓慢增大，然后快速增大，再是缓慢增大，再到匀速。也就是说加速度是变化的。

f(x) = 1/(1+e^-x)，这是y从左到右增加时的S曲线的原始函数，用这个生成一个表，加速正着用，减速倒着用，e是自然常数，约为2.71828。当-3 ≤ x ≤ 3，函数收敛较为明显，此时，y轴高度基本处于0到1之间，更精确点，当x = -3时， y ≈ 0.04742，当x = 3时，y ≈ 0.95257。如果精度不够，就加大x的取值范围。现在假设3 ≤ x ≤ 3时，y从0到1。

实际使用时，x作为加速次数，y就是速度。加速次数不可能为负数，所以需要对函数进行向右平移，也就是1/(1+e^(-x + 3))，这样当 x = 0时，y ≈ 0，这时0 ≤ x ≤ 6。但是加速次数也不可能为小数，而且6次加速太少了，所以要横向拉长函数。改变x的系数就可以拉长或缩短y ≈ 0时x的范围。这个函数的x的系数越小，函数看起来横向越长（实际无限长），x能取得的整数越多。

当系数为0.1时，1/(1+e^(-0.1x + 3))，x的有效值就是0到60，当系数为2时，1/(1+e^(-2x + 3))，x的有效值就是0到3。也就是说x取值的整数范围是系数的倒数，即1/0.1 x 6个，和1/2 x 6个。如果决定加速次数，那么系数就是6/加速次数，当加速次数为100次时，也就是1/(1+e^(-(6/100)x + 3))。

假如最大脉冲频率为1MHz，y乘以最大频率就可以知道当前实际频率了，即1000000 x 0.95257。如果将1/(1+e^-x)计算后的速度比例存起来，下次使用时只需查表，再乘以最大频率即可。

实现：

//buff_pa是速度表数组。count_va是加速次数。pan_right_va是右移量，代表了精度，即右移多少后，y强制为0，一般用7，最后一次速度在99.9%。count_va越大，S曲线越平缓，加速过程越平滑，加速时间越长；count_va越小，S曲线越陡峭，加速过程越急剧，加速时间越短。

void curve_s_init(float *buff_pa, uint16_t count_va, uint8_t pan_right_va)

{

for (uint16_t x = 0; x < count_va; x++)

buff_pa[x] = 1.0 / (1.0 + exp(((float)pan_right_va * 2 / count_va * -x) + pan_right_va));

//buff_pa[x] = 1.0 / (1.0 + pow(sqrt(m_e_v), (float)pan_right_va * 2 / count_va * -x + pan_right_va)); //这是通用计算方法，pow函数的第一个参数越接近1，该算式计算出的曲线越平缓，可以改的和线性加减速无异

}

例：

float curve_s_table_v[500];

curve_s_init(curve_s_table_v, 500, 7);

enum motor_state_t
{
    motor_stop_state_v,
    motor_accelerate_state_v,
    motor_speed_max_state_v,
    motor_decelerate_state_v
};
 
struct motor_spta_t
{
    int32_t total_step_v;           //设定运行总脉冲数。负数就往复位方向移动
    uint32_t step_counter_v;        //已经运行的脉冲数
    uint16_t speed_v;               //当前速度
    uint32_t speed_max_threshold_v; //最大速度阈值，值越大，速度就能分的越精细，相当于速度百分比。用户设定的最大速度小于等于这个值
    enum motor_state_t state_v;     //运行状态
 
    bool enable_interrupt_counter_v;                    //定时器中断计数的开关
    uint16_t accelerate_decelerate_action_threshold_v;  //加减速一次的频率阈值，当中断次数达到这个值后就会加减速一次
    uint16_t accelerate_decelerate_interrupt_counter_v; //加减速时的定时器中断次数计数
    uint32_t accelerate_decelerate_speed_counter_v;     //加减速次数计数
    uint32_t accelerate_decelerate_step_v;              //加减速阶段的脉冲数
    uint32_t speed_accumulator_v;                       //速度累加器
 
    uint8_t motor_id_v;                                                                  //用于查询
    int32_t absolute_position_step_v;                                                    //绝对位置，以步数为单位
    GPIO_TypeDef *step_gpio_array_v, *direction_gpio_array_v; //IO组，脉冲、方向
    uint16_t step_gpio_v, direction_gpio_v;                          //IO号，脉冲、方向
};
 
#define curve_s_table_total_v 500 //s曲线参数个数
float curve_s_table_v[curve_s_table_total_v];
 
void curve_s_init(float *buff_pa, uint16_t count_va, uint8_t pan_right_va)
{
    for (uint16_t x = 0; x < count_va; x++)
    {
        *buff_pa++ = 1.0 / (1.0 + exp(((float)pan_right_va * 2 / count_va * -x) + pan_right_va));
    }
}
 
void motor_arg_init(struct motor_spta_t *motor_pa, uint8_t motor_id_va, GPIO_TypeDef *step_gpio_array_va, uint16_t step_gpio_va, GPIO_TypeDef *direction_gpio_array_va, uint16_t direction_gpio_va)
{
    motor_pa->motor_id_v = motor_id_va;
    motor_pa->step_gpio_array_v = step_gpio_array_va;
    motor_pa->step_gpio_v = step_gpio_va;
    motor_pa->direction_gpio_array_v = direction_gpio_array_va;
    motor_pa->direction_gpio_v = direction_gpio_va;
 
    motor_pa->speed_max_threshold_v = 0xffff;
    motor_pa->absolute_position_step_v = 0;
}
 
void motor_move(struct motor_spta_t *motor_pa, int32_t total_step_va, uint16_t speed_max_va, uint16_t accelerate_decelerate_action_threshold_va)
{
    if (total_step_va == 0)
        return;
 
    motor_pa->total_step_v = total_step_va;
    motor_pa->step_counter_v = 0;
    motor_pa->speed_v = 0;
    motor_pa->speed_max_v = speed_max_va > motor_pa->speed_max_threshold_v ? motor_pa->speed_max_threshold_v : speed_max_va;
    motor_pa->enable_interrupt_counter_v = true;
    motor_pa->accelerate_decelerate_interrupt_counter_v = 0;
    motor_pa->accelerate_decelerate_speed_counter_v = 0;
    motor_pa->accelerate_decelerate_step_v = 0;
    motor_pa->accelerate_decelerate_action_threshold_v = accelerate_decelerate_action_threshold_va;
    motor_pa->speed_accumulator_v = 0;
 
    if (motor_pa->total_step_v > 0)
    {
        if (motor_pa->reset_direction_io_level_v == GPIO_PIN_SET)
            HAL_GPIO_WritePin(motor_pa->direction_gpio_array_v, motor_pa->direction_gpio_v, GPIO_PIN_RESET);
        else
            HAL_GPIO_WritePin(motor_pa->direction_gpio_array_v, motor_pa->direction_gpio_v, GPIO_PIN_SET);
    }
    else
        HAL_GPIO_WritePin(motor_pa->direction_gpio_array_v, motor_pa->direction_gpio_v, motor_pa->reset_direction_io_level_v);
    motor_pa->state_v = motor_accelerate_state_v;
}
 
void motor_spta_algorithm(struct motor_spta_t *motor_pa)
{
    if (motor_pa->state_v == motor_stop_state_v)
        return;
 
    bool carry_v = false;
 
    //拉低脉冲信号
    HAL_GPIO_WritePin(motor_pa->step_gpio_array_v, motor_pa->step_gpio_v, GPIO_PIN_RESET);
 
    motor_pa->speed_accumulator_v += motor_pa->speed_v;                   //叠加速度
    if (motor_pa->speed_accumulator_v >= motor_pa->speed_max_threshold_v) //阈值，如果当前速度达到阈值，发送脉冲的速度就达到最快了，这个阈值就是算法的最大速度
    {
        carry_v = true;
        motor_pa->speed_accumulator_v -= motor_pa->speed_max_threshold_v;
    }
 
    if (carry_v == true) //判断是否溢出
    {
        motor_pa->step_counter_v++;
        //拉高脉冲信号产生
        HAL_GPIO_WritePin(motor_pa->step_gpio_array_v, motor_pa->step_gpio_v, GPIO_PIN_SET);
 
        if (motor_pa->total_step_v > 0)
            motor_pa->absolute_position_step_v++;
        else
            motor_pa->absolute_position_step_v--;
    }
 
    //根据电机的状态进行工作
    switch (motor_pa->state_v)
    {
    case motor_accelerate_state_v:
 
        //记录加速的步数
        if (carry_v == true)
            motor_pa->accelerate_decelerate_step_v++;
 
        if (motor_pa->enable_interrupt_counter_v)
        {
            motor_pa->accelerate_decelerate_interrupt_counter_v++;
            if (motor_pa->accelerate_decelerate_interrupt_counter_v >= motor_pa->accelerate_decelerate_action_threshold_v)
            {
                motor_pa->accelerate_decelerate_interrupt_counter_v = 0;
                motor_pa->accelerate_decelerate_speed_counter_v++; //计录加速的次数
 
                motor_pa->speed_v = curve_s_table_v[motor_pa->accelerate_decelerate_speed_counter_v] * motor_pa->speed_max_v; //计算当前速度
 
                //如果加速次数达到最高次数，就停止加速，并让速度等于最大速度，再切换状态
                if (motor_pa->accelerate_decelerate_speed_counter_v >= curve_s_table_total_v)
                {
                    motor_pa->enable_interrupt_counter_v = false;
                    motor_pa->speed_v = motor_pa->speed_max_v;
                    motor_pa->state_v = motor_speed_max_state_v;
                }
            }
        }
 
        //如果总步数大于1步
        if ((uint32_t)fabs(motor_pa->total_step_v) > 1) //fabs取绝对值的c库函数
        {
            if (motor_pa->step_counter_v >= (uint32_t)fabs(motor_pa->total_step_v) / 2) //如果已走步数大于最大步数的一半
            {
                //加速时中断计数被断，所以要调整，最后一次加速多长时间，减速的第一次的速度就维持多长时间
                motor_pa->accelerate_decelerate_interrupt_counter_v = motor_pa->accelerate_decelerate_action_threshold_v - motor_pa->accelerate_decelerate_interrupt_counter_v;
                motor_pa->state_v = motor_decelerate_state_v;
            }
        }
        else if (motor_pa->step_counter_v > 0) //只有1步也至少走1步
            motor_pa->state_v = motor_decelerate_state_v;
 
        break;
 
    case motor_speed_max_state_v:
        if ((uint32_t)fabs(motor_pa->total_step_v) - motor_pa->step_counter_v == motor_pa->accelerate_decelerate_step_v)
        {
            motor_pa->enable_interrupt_counter_v = true;
            //进入减速状态就要切换为减速状态时的最大速度
            motor_pa->accelerate_decelerate_speed_counter_v--;
            motor_pa->speed_v = curve_s_table_v[motor_pa->accelerate_decelerate_speed_counter_v] * motor_pa->speed_max_v; //计算当前速度
            motor_pa->state_v = motor_decelerate_state_v;
        }
 
        break;
 
    case motor_decelerate_state_v:
 
        if (motor_pa->enable_interrupt_counter_v)
        {
            motor_pa->accelerate_decelerate_interrupt_counter_v++;
            if (motor_pa->accelerate_decelerate_interrupt_counter_v >= motor_pa->accelerate_decelerate_action_threshold_v) //如果中断次数达到设定次数
            {
                motor_pa->accelerate_decelerate_interrupt_counter_v = 0;
                motor_pa->accelerate_decelerate_speed_counter_v--;
 
                motor_pa->speed_v = curve_s_table_v[motor_pa->accelerate_decelerate_speed_counter_v] * motor_pa->speed_max_v; //计算当前速度
 
                if (motor_pa->speed_v <= motor_pa->speed_min_v)
                {
                    motor_pa->enable_interrupt_counter_v = false;
                }
            }
        }
 
        if (motor_pa->step_counter_v >= (uint32_t)fabs(motor_pa->total_step_v))
        {
            //设定电机状态
            motor_pa->state_v = motor_stop_state_v;
        }
        break;
 
    default:
        break;
    }
}
 
void my_MX_TIM2_Init(void)
{
 
    /* USER CODE BEGIN TIM2_Init 0 */
 
    /* USER CODE END TIM2_Init 0 */
 
    TIM_ClockConfigTypeDef sClockSourceConfig = {0};
    TIM_MasterConfigTypeDef sMasterConfig = {0};
 
    /* USER CODE BEGIN TIM2_Init 1 */
 
    /* USER CODE END TIM2_Init 1 */
    htim2.Instance = TIM2;
    htim2.Init.Prescaler = 72 - 1;
    htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim2.Init.Period = 20 - 1;
    htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
    {
        Error_Handler();
    }
    sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
    if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
    {
        Error_Handler();
    }
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    /* USER CODE BEGIN TIM2_Init 2 */
 
    /* USER CODE END TIM2_Init 2 */
}
 
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    switch ((uint32_t)htim->Instance)
    {
    case (uint32_t)TIM2:
        motor_spta_algorithm(&motor1_v);
        break;
 
    default:
        break;
    }
}

//示例：
struct motor_spta_t motor1_v;
int main()
{
    curve_s_init(curve_s_table_v, curve_s_table_total_v, 7);
 
    my_MX_TIM2_Init();
    HAL_TIM_Base_Start_IT(&htim2);
 
    motor_arg_init(&motor1_v, 1, GPIOA, GPIO_PIN_0, GPIOA, GPIO_PIN_1);
    while(1)
    {
        motor_move(&motor1_v, 5000, 10000, 100);
    }
}

本文链接：https://blog.csdn.net/qq_41544116/article/details/106690377

二次转载的链接：步进电机S加减速，SPTA结合S加减速 - 灰信网（软件开发博客聚合） (freesion.com)