舵机S型加减速

czy8787475
已于 2025-02-14 16:17:23 修改
阅读量135
点赞数 2
分类专栏： Motor STM32 文章标签：算法 stm32
于 2025-02-14 16:16:48 首次发布
本文链接：https://blog.youkuaiyun.com/czy8787475/article/details/145635923
版权
STM32 同时被 2 个专栏收录
23 篇文章
订阅专栏
Motor
6 篇文章
订阅专栏
#include "stm32f4xx_hal_tim.h"
#include "pulse_width_data_z.h"
#include "pulse_width_data_z.h"
#include "fifo.h"
#include "gpio.h"
#include "urgent_event.h"
#include "fan.h"
#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"
#include "linear_actuator.h"
#include "user_mb_app.h"
#include "math.h"

DIR_STEERING Dir_Steering_Z;

// 平台舵机的频率为330Hz，计数频率为21MHz

#define Z_STEERING_CCW_MAX_SPEEND_PULSE_WIDTH_VALUE 10500 // Pull 500us
#define Z_STEERING_STOP_PULSE_WIDTH_VALUE 31500           // Stop 1500us
#define Z_STEERING_CW_MAX_SPEEND_PULSE_WIDTH_VALUE 52500  // Push 2500us
#define Z_STEERING_FREQUENCY 330.0      // 平台Z舵机频率
#define Z_STEERING_TIM_FREQUENCY 21.0   // 平台Z舵机定时器计数频率(MHz)

#if USE_ARRAY_BUFFER
static uint16_t pulse_width_ccw_z_arr_index;
static uint16_t pulse_width_cw_z_arr_index;
#else
static uint16_t z_pull_min_speed = 1500 - 330;
static uint16_t z_pull_max_speed = 1500 - 650;
static uint16_t z_push_min_speed = 1500 + 330;
static uint16_t z_push_max_speed = 1500 + 650;
static double z_acc_dec_cnt = Z_STEERING_FREQUENCY * 2.0;   // 平台Z舵机频率 * 加(减)速时间
static double z_constant_cnt = Z_STEERING_FREQUENCY * 1.5;  // 平台Z舵机频率 * 匀速时间
static double z_k_sigmoid = 0.0;
static double z_b_sigmoid = 0.0;
static double z_pull_k1 = 0.0;
static double z_pull_b1 = 0.0;
static double z_push_k1 = 0.0;
static double z_push_b1 = 0.0;
static uint32_t z_ccw_sigmiod_pulse_width_index = 0;
static uint32_t z_cw_sigmiod_pulse_width_index = 0;
static uint16_t z_pwm_index = 0;

void Steering_Data_Init(void)
{
  z_acc_dec_cnt = Z_STEERING_FREQUENCY * 2.0;  // 平台Z舵机频率 * 加(减)速时间
  z_constant_cnt = Z_STEERING_FREQUENCY * 1.5;  // 平台Z舵机频率 * 匀速时间

  z_k_sigmoid = 10.0 / (z_acc_dec_cnt - 1.0);
  z_b_sigmoid = -z_k_sigmoid - 5;

  z_pull_k1 = z_pull_max_speed * Z_STEERING_TIM_FREQUENCY - z_pull_min_speed * Z_STEERING_TIM_FREQUENCY;
  z_pull_b1 = z_pull_min_speed * Z_STEERING_TIM_FREQUENCY;

  z_push_k1 = z_push_max_speed * Z_STEERING_TIM_FREQUENCY - z_push_min_speed * Z_STEERING_TIM_FREQUENCY;
  z_push_b1 = z_push_min_speed * Z_STEERING_TIM_FREQUENCY;
}
#endif

/**
 * @brief       定时器更新中断回调函数
 * @param       htim:定时器句柄指针
 * @note        此函数会被HAL_TIM_PeriodElapsedCallback调用
 * @retval      无
 */
void HAL_TIM_Steering_Callback(TIM_HandleTypeDef *htim)
{
    uint32_t percent = 0;
    uint32_t z_sigmiod_pulse_width_value = 0;

    if (htim->Instance == ATIM_TIMX_PWM)
    {
        if (Dir_Steering_Z == CCW)
        {
            #if USE_ARRAY_BUFFER

            __HAL_TIM_SetCompare(&g_atimx_pwm_chy_handle, TIM_CHANNEL_2, pulse_width_ccw_z_arr[pulse_width_ccw_z_arr_index]); /* 设置比较值 */

            fifo_push('Z');
            fifo_push(':');
            fifo_push(' ');
            fifo_push_short(pulse_width_ccw_z_arr_index);
            // fifo_push(',');
            // fifo_push(' ');
            // fifo_push_short(pulse_width_ccw_z_arr[pulse_width_ccw_z_arr_index]);
            fifo_push('\n');

            if (pulse_width_ccw_z_arr_index < ((sizeof(pulse_width_ccw_z_arr) / sizeof(pulse_width_ccw_z_arr[0])) - 1))
            {
                pulse_width_ccw_z_arr_index++;
            }

            percent = 100 * pulse_width_ccw_z_arr_index / ((sizeof(pulse_width_ccw_z_arr) / sizeof(pulse_width_ccw_z_arr[0])) + 1);

            Slave_station.usSRegHoldBuf[1617] = percent;   // 平台运动进度 ?%

            #else

            if (z_pwm_index < (z_acc_dec_cnt + z_constant_cnt + z_acc_dec_c
            {
                ++z_pwm_index;
            }

            if (z_pwm_index < z_acc_dec_cnt)
            {
                z_ccw_sigmiod_pulse_width_index++;
            }
            else if (z_pwm_index > (z_acc_dec_cnt + z_constant_cnt + 1))
            {
                if (z_ccw_sigmiod_pulse_width_index > 2)
                {
                    z_ccw_sigmiod_pulse_width_index--;
                }
            }
            else
            {
                z_ccw_sigmiod_pulse_width_index = z_acc_dec_cnt;
            }

            z_sigmiod_pulse_width_value = z_pull_k1 / (1 + exp(-(z_ccw_sigmiod_pulse_width_index * z_k_sigmoid + z_b_sigmoid))) + z_pull_b1;
            
            __HAL_TIM_SetCompare(&g_atimx_pwm_chy_handle, TIM_CHANNEL_2, z_sigmiod_pulse_width_value); /* 设置比较值 */

            percent = 100 * z_pwm_index / (z_acc_dec_cnt + z_constant_cnt + z_acc_dec_cnt + 1);
            
            Slave_station.usSRegHoldBuf[1617] = percent;   // 平台运动进度 ?%

            fifo_push('Z');
            fifo_push(':');
            fifo_push(' ');
            fifo_push_short(z_pwm_index);
            fifo_push('\n');

            #endif
        }
        else if (Dir_Steering_Z == CW)
        {
            #if USE_ARRAY_BUFFER
            
            __HAL_TIM_SetCompare(&g_atimx_pwm_chy_handle, TIM_CHANNEL_2, pulse_width_cw_z_arr[pulse_width_cw_z_arr_index]); /* 设置比较值 */

            fifo_push('Z');
            fifo_push(':');
            fifo_push(' ');
            fifo_push_short(pulse_width_cw_z_arr_index);
            // fifo_push(',');
            // fifo_push(' ');
            // fifo_push_short(pulse_width_cw_z_arr[pulse_width_cw_z_arr_index]);
            fifo_push('\n');

            if (pulse_width_cw_z_arr_index < ((sizeof(pulse_width_cw_z_arr) / sizeof(pulse_width_cw_z_arr[0])) - 1))
            {
                pulse_width_cw_z_arr_index++;
            }

            percent = 100 * pulse_width_cw_z_arr_index / ((sizeof(pulse_width_cw_z_arr) / sizeof(pulse_width_cw_z_arr[0])) + 1);

            Slave_station.usSRegHoldBuf[1617] = percent;   // 平台运动进度 ?%

            #else
            
            if (z_pwm_index < (z_acc_dec_cnt + z_constant_cnt + z_acc_dec_cnt - 1))
            {
                ++z_pwm_index;
            }

            if (z_pwm_index < z_acc_dec_cnt)
            {
                z_cw_sigmiod_pulse_width_index++;
            }
            else if (z_pwm_index > (z_acc_dec_cnt + z_constant_cnt + 1))
            {
                if (z_cw_sigmiod_pulse_width_index > 2)
                {
                    z_cw_sigmiod_pulse_width_index--;
                }
            }
            else
            {
                z_cw_sigmiod_pulse_width_index = z_acc_dec_cnt;
            }

            z_sigmiod_pulse_width_value = z_push_k1 / (1 + exp(-(z_cw_sigmiod_pulse_width_index * z_k_sigmoid + z_b_sigmoid))) + z_push_b1;
            
            __HAL_TIM_SetCompare(&g_atimx_pwm_chy_handle, TIM_CHANNEL_2, z_sigmiod_pulse_width_value); /* 设置比较值 */

            percent = 100 * z_pwm_index / (z_acc_dec_cnt + z_constant_cnt + z_acc_dec_cnt + 1);
            
            Slave_station.usSRegHoldBuf[1617] = percent;   // 平台运动进度 ?%

            fifo_push('Z');
            fifo_push(':');
            fifo_push(' ');
            fifo_push_short(z_pwm_index);
            fifo_push('\n'
        }
    }
}