tao_z
2021-07-14 82e38738a4d532cc3d56cbf80c1a4093f23cdd6a
USR/SRC/bldc_ctrl.c
@@ -1,6 +1,11 @@
#include <math.h>
#include <stdlib.h>
#include "bldc_ctrl.h"
#include "motor.h"
#include "gpio.h"
#include "pwm.h"
#include "bsp_pid.h"
/* Ë½ÓбäÁ¿ */
static bldcm_data_t bldcm_data;
@@ -15,9 +20,14 @@
  */
void bldcm_init(void)
{
    PWM_TIMx_Configuration(); // µç»ú¿ØÖƶ¨Ê±Æ÷£¬Òý½Å³õʼ»¯
    hall_tim_config();        // »ô¶û´«¸ÐÆ÷³õʼ»¯
    sd_gpio_config();         // sd Òý½Å³õʼ»¯
  // PWM_TIMx_Configuration(); // µç»ú¿ØÖƶ¨Ê±Æ÷£¬Òý½Å³õʼ»¯
  // hall_tim_config();        // »ô¶û´«¸ÐÆ÷³õʼ»¯
  // sd_gpio_config(); // sd Òý½Å³õʼ»¯
  Set_SDH2136_Enable();
  // Set_SDH2136_Disable();
  bldcm_data.direction = MOTOR_FWD;
  bldcm_data.is_enable = 0;
  bldcm_data.dutyfactor = 2000;
}
/**
@@ -27,24 +37,7 @@
  */
static void sd_gpio_config(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    /* ¶¨Ê±Æ÷ͨµÀ¹¦ÄÜÒý½Å¶Ë¿ÚʱÖÓʹÄÜ */
    SHUTDOWN_GPIO_CLK_ENABLE();
    /* Òý½ÅIO³õʼ»¯ */
    /*ÉèÖÃÊä³öÀàÐÍ*/
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    /*ÉèÖÃÒý½ÅËÙÂÊ */
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    /*Ñ¡ÔñÒª¿ØÖƵÄGPIOÒý½Å*/
    GPIO_InitStruct.Pin = SHUTDOWN_PIN;
    /*µ÷Óÿ⺯Êý£¬Ê¹ÓÃÉÏÃæÅäÖõÄGPIO_InitStructure³õʼ»¯GPIO*/
    HAL_GPIO_Init(SHUTDOWN_GPIO_PORT, &GPIO_InitStruct);
    BLDCM_ENABLE_SD(); // Ä¬ÈÏ¿ªÆô
    HAL_Delay(1);
  Set_SDH2136_Disable();
}
/**
@@ -54,9 +47,9 @@
  */
void set_bldcm_speed(uint16_t v)
{
    bldcm_data.dutyfactor = v;
  bldcm_data.dutyfactor = v;
    set_pwm_pulse(v); // ÉèÖÃËÙ¶È
  set_pwm_pulse(v); // ÉèÖÃËÙ¶È
}
/**
@@ -66,7 +59,7 @@
  */
void set_bldcm_direction(motor_dir_t dir)
{
    bldcm_data.direction = dir;
  bldcm_data.direction = dir;
}
/**
@@ -76,7 +69,7 @@
  */
motor_dir_t get_bldcm_direction(void)
{
    return bldcm_data.direction;
  return bldcm_data.direction;
}
/**
@@ -86,8 +79,8 @@
  */
void set_bldcm_enable(void)
{
    bldcm_data.is_enable = 1;
    hall_enable();
  bldcm_data.is_enable = 1;
  hall_enable();
}
/**
@@ -97,14 +90,14 @@
  */
void set_bldcm_disable(void)
{
    /* ½ûÓûô¶û´«¸ÐÆ÷½Ó¿Ú */
    hall_disable();
  /* ½ûÓûô¶û´«¸ÐÆ÷½Ó¿Ú */
  hall_disable();
    /* Í£Ö¹ PWM Êä³ö */
    stop_pwm_output();
  /* Í£Ö¹ PWM Êä³ö */
  stop_pwm_output();
    /* ¹Ø±Õ MOS ¹Ü */
    bldcm_data.is_enable = 0;
  /* ¹Ø±Õ MOS ¹Ü */
  bldcm_data.is_enable = 0;
}
/**
@@ -114,32 +107,32 @@
  */
void bldcm_pid_control(void)
{
    int32_t speed_actual = get_motor_speed(); // µç»úÐýתµÄµ±Ç°ËÙ¶È
  int32_t speed_actual = get_motor_speed(); // µç»úÐýתµÄµ±Ç°ËÙ¶È
    if (bldcm_data.is_enable)
  if (bldcm_data.is_enable)
  {
    float cont_val = 0; // µ±Ç°¿ØÖÆÖµ
    cont_val = PID_realize(speed_actual);
    if (cont_val < 0)
    {
        float cont_val = 0; // µ±Ç°¿ØÖÆÖµ
        cont_val = PID_realize(speed_actual);
        if (cont_val < 0)
        {
            cont_val = -cont_val;
            bldcm_data.direction = MOTOR_REV;
        }
        else
        {
            bldcm_data.direction = MOTOR_FWD;
        }
        cont_val = (cont_val > PWM_PERIOD_COUNT) ? PWM_PERIOD_COUNT : cont_val; // ÉÏÏÞ´¦Àí
        set_bldcm_speed(cont_val);
#ifdef PID_ASSISTANT_EN
        set_computer_value(SEND_FACT_CMD, CURVES_CH1, &speed_actual, 1); // ¸øÍ¨µÀ 1 ·¢ËÍʵ¼ÊÖµ
#else
        printf("ʵ¼ÊÖµ£º%d, Ä¿±êÖµ£º%.0f£¬¿ØÖÆÖµ: %.0f\n", speed_actual, get_pid_target(), cont_val);
#endif
      cont_val = -cont_val;
      bldcm_data.direction = MOTOR_REV;
    }
    else
    {
      bldcm_data.direction = MOTOR_FWD;
    }
    cont_val = (cont_val > PERIOD_CMP) ? PERIOD_CMP : cont_val; // ÉÏÏÞ´¦Àí
    set_bldcm_speed(cont_val);
    // #ifdef PID_ASSISTANT_EN
    //     set_computer_value(SEND_FACT_CMD, CURVES_CH1, &speed_actual, 1); // ¸øÍ¨µÀ 1 ·¢ËÍʵ¼ÊÖµ
    // #else
    //     printf("ʵ¼ÊÖµ£º%d, Ä¿±êÖµ£º%.0f£¬¿ØÖÆÖµ: %.0f\n", speed_actual, get_pid_target(), cont_val);
    // #endif
  }
}