.vscode/settings.json | ●●●●● patch | view | raw | blame | history | |
USR/INC/bldc_ctrl.h | ●●●●● patch | view | raw | blame | history | |
USR/INC/clock.h | ●●●●● patch | view | raw | blame | history | |
USR/INC/gd32e23x_it.h | ●●●●● patch | view | raw | blame | history | |
USR/INC/gpio.h | ●●●●● patch | view | raw | blame | history | |
USR/INC/motor.h | ●●●●● patch | view | raw | blame | history | |
USR/INC/pwm.h | ●●●●● patch | view | raw | blame | history | |
USR/SRC/Clock.c | ●●●●● patch | view | raw | blame | history | |
USR/SRC/GPIO.c | ●●●●● patch | view | raw | blame | history | |
USR/SRC/Motor.c | ●●●●● patch | view | raw | blame | history | |
USR/SRC/bldc_ctrl.c | ●●●●● patch | view | raw | blame | history | |
USR/SRC/gd32e23x_it.c | ●●●●● patch | view | raw | blame | history | |
USR/SRC/main.c | ●●●●● patch | view | raw | blame | history | |
USR/SRC/pwm.c | ●●●●● patch | view | raw | blame | history | |
pid/bsp_pid.c | ●●●●● patch | view | raw | blame | history | |
pid/bsp_pid.h | ●●●●● patch | view | raw | blame | history |
.vscode/settings.json
@@ -1,4 +1,7 @@ { "files.autoGuessEncoding": true, "files.encoding": "gb2312" "files.encoding": "gb2312", "files.associations": { "math.h": "c" } } USR/INC/bldc_ctrl.h
@@ -43,4 +43,6 @@ void set_bldcm_enable(void); void set_bldcm_disable(void); void bldcm_pid_control(void); // extern void BLDC_SpeedAndPID(void); #endif USR/INC/clock.h
@@ -1,7 +1,7 @@ #ifndef _CLOCK_H_ #define _CLOCK_H_ #include "stdint.h" #define SYSTEM_CLOCK (48000000UL) #define SYSTEM_CLOCK (64000000UL) void Clock_Config(void); void Ostick_config(void); void delay_1ms(uint32_t count); USR/INC/gd32e23x_it.h
@@ -60,5 +60,5 @@ void SysTick_Handler(void); /* this function handles XTI4-15 exception */ void EXTI4_15_IRQHandler(void); void TIMER2_IRQHandler(void); #endif /* GD32E23X_IT_H */ USR/INC/gpio.h
@@ -24,10 +24,10 @@ #define HALL_SENSOR_A_PORT GPIOB #define HALL_SENSOR_A_PIN GPIO_PIN_4 #define HALL_SENSOR_B_PORT GPIOB #define HALL_SENSOR_B_PIN GPIO_PIN_5 #define HALL_SENSOR_C_PORT GPIOA #define HALL_SENSOR_C_PIN GPIO_PIN_15 #define HALL_SENSOR_C_PORT GPIOB #define HALL_SENSOR_C_PIN GPIO_PIN_5 #define HALL_SENSOR_B_PORT GPIOA #define HALL_SENSOR_B_PIN GPIO_PIN_15 //¶¨ÒåADCÒý½Å #define CURRENT_AI_PORT GPIOA USR/INC/motor.h
@@ -1,6 +1,6 @@ #ifndef MOTOR_H #define MOTOR_H #include "" #include "gd32e23x_exti.h" /* µç»ú¿ØÐýתʵÏֽṹÌå */ #define SPEED_FILTER_NUM 30 // ËÙ¶ÈÂ˲¨´ÎÊý @@ -16,16 +16,25 @@ /* ÀÛ¼Æ TIM_Period¸öºó²úÉúÒ»¸ö¸üлòÕßÖÐ¶Ï µ±¶¨Ê±Æ÷´Ó0¼ÆÊýµ½65535£¬¼´Îª65535´Î£¬ÎªÒ»¸ö¼ÆÊýÖÜÆÚ */ #define HALL_PERIOD_COUNT (0xFFFF) /* ͨÓÿØÖƶ¨Ê±Æ÷ʱÖÓÔ´TIMxCLK = HCLK = 72MHz É趨¶¨Ê±Æ÷ƵÂÊΪ = TIMxCLK / (PWM_PRESCALER_COUNT) / PWM_PERIOD_COUNT = 9.987Hz ÖÜÆÚ T = 100ms */ #define SPEED_PRESCALER_COUNT (6400u) #define SPEED_PERIOD_COUNT (300u) /* ͨÓÿØÖƶ¨Ê±Æ÷ʱÖÓÔ´TIMxCLK = HCLK = 64MHz É趨¶¨Ê±Æ÷ƵÂÊΪ = TIMxCLK / (SPEED_PRESCALER_COUNT) / SPEED_PERIOD_COUNT = 33.333Hz ÖÜÆÚ T = 30ms */ #define HALL_PRESCALER_COUNT (110) #define HALL_A_EXTI (EXTI_4) #define HALL_B_EXTI (EXTI_5) #define HALL_C_EXTI (EXTI_15) extern void HAL_HallExti_TriggerCallback(void); #define MOTOR_OUT_CH_U (TIMER_CH_1) #define MOTOR_OUT_CH_V (TIMER_CH_0) #define MOTOR_OUT_CH_W (TIMER_CH_2) #endif extern void HAL_HallExti_TriggerCallback(void); extern void BLDC_SpeedAndPID(void); extern float get_motor_speed(void); void hall_enable(void); void hall_disable(void); void set_pwm_pulse(uint16_t pulse); #endif USR/INC/pwm.h
@@ -1,13 +1,17 @@ #ifndef _PWM_H_ #define _PWM_H_ #include "gd32e23x_timer.h" //1us * 10000 = 10ms #define PERIOD_CAP (10000lU) //100us * 300 = 30ms #define PERIOD_CAP (300U) //48Mzhz / 3000 = 16Khz = 62.5us #define PERIOD_CMP (3000u) //64Mzhz / 4000 = 16Khz = 62.5us #define PERIOD_CMP (4000u) void TimerInit(void); void SetPwmDuty(uint16_t ch, uint32_t duty); void SetPwmPeriod(uint32_t period); extern void StartSpeedTime(void); extern void StopSpeedTime(void); extern void stop_pwm_output(void); #endif USR/SRC/Clock.c
@@ -12,11 +12,11 @@ rcu_osci_on(RCU_IRC8M); //ʹÓÃÄÚ²¿8MRCʱÖÓ rcu_osci_stab_wait(RCU_IRC8M); rcu_system_clock_source_config(RCU_CKSYSSRC_PLL); //sysclk is PLL rcu_pll_config(RCU_PLLSRC_IRC8M_DIV2, RCU_PLL_MUL12); //8MHz/2 =4MHz PLL =4MHz*12 =48MHz rcu_ahb_clock_config(RCU_AHB_CKSYS_DIV1); //AHB 48M rcu_apb1_clock_config(RCU_APB1_CKAHB_DIV1); //APB1 48M rcu_apb2_clock_config(RCU_APB2_CKAHB_DIV1); //APB2 48M rcu_adc_clock_config(RCU_ADCCK_APB2_DIV4); //max 14M,current:12M rcu_pll_config(RCU_PLLSRC_IRC8M_DIV2, RCU_PLL_MUL16); //8MHz/2 =4MHz PLL =4MHz*16 =64MHz rcu_ahb_clock_config(RCU_AHB_CKSYS_DIV1); //AHB 64M rcu_apb1_clock_config(RCU_APB1_CKAHB_DIV1); //APB1 64M rcu_apb2_clock_config(RCU_APB2_CKAHB_DIV1); //APB2 64M rcu_adc_clock_config(RCU_ADCCK_APB2_DIV4); //max 14M,current:64/4=16M rcu_usart_clock_config(RCU_USART0SRC_IRC8M); //USART0 8M rcu_rtc_clock_config(RCU_RTCSRC_NONE); USR/SRC/GPIO.c
@@ -1,4 +1,6 @@ #include "gpio.h" #include "gd32e23x_exti.h" #include "gd32e23x_syscfg.h" extern void GPIO_Init(void) { @@ -7,13 +9,16 @@ rcu_periph_clock_enable(RCU_GPIOF); //初始化配置霍尔输入引脚 gpio_mode_set(HALL_SENSOR_A_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, HALL_SENSOR_A_PIN); gpio_af_set(HALL_SENSOR_A_PORT, GPIO_AF_1, HALL_SENSOR_A_PIN); gpio_mode_set(HALL_SENSOR_B_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, HALL_SENSOR_B_PIN); gpio_af_set(HALL_SENSOR_B_PORT, GPIO_AF_1, HALL_SENSOR_B_PIN); gpio_mode_set(HALL_SENSOR_A_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, HALL_SENSOR_A_PIN); gpio_mode_set(HALL_SENSOR_B_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, HALL_SENSOR_B_PIN); gpio_mode_set(HALL_SENSOR_C_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, HALL_SENSOR_C_PIN); /* connect EXTI line to GPIO pin */ syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN4); syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN5); syscfg_exti_line_config(EXTI_SOURCE_GPIOA, EXTI_SOURCE_PIN15); exti_init(EXTI_4 | EXTI_5 | EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING); //配置外部上升沿中断 exti_interrupt_flag_clear(EXTI_4 | EXTI_5 | EXTI_15); //初始化PWM引脚 gpio_mode_set(PWM_HIN1_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, PWM_HIN1_PIN); @@ -39,13 +44,6 @@ gpio_mode_set(PWM_LIN3_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, PWM_LIN3_PIN); gpio_af_set(PWM_LIN3_PORT, GPIO_AF_2, PWM_LIN3_PIN); gpio_output_options_set(PWM_LIN3_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, PWM_LIN3_PIN); // //初始化配置LED引脚 // gpio_mode_set(LED_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED_PIN); //LED // gpio_bit_write(LED_PORT, LED_PIN, RESET); //led on // //初始化配置开关输入 // gpio_mode_set(SW_IN_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, SW_IN_PIN); //初始化配置SDH2136 gpio_mode_set(SDH2136_EN_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, SDH2136_EN_PIN); //EN USR/SRC/Motor.c
@@ -1,7 +1,13 @@ #include "gpio.h" #include "motor.h" #include "gd32e23x_exti.h" static motor_rotate_t motor_drive = {0}; #include "gd32e23x_timer.h" #include "bldc_ctrl.h" #include "pwm.h" static volatile motor_rotate_t motor_drive = {0}; static uint32_t motor_pluse = 0; static void motor_phasechange(void); static void update_speed_dir(uint8_t dir_in); /** * @brief ʹÄÜ»ô¶û´«¸ÐÆ÷ * @param ÎÞ @@ -13,12 +19,8 @@ exti_interrupt_enable(HALL_A_EXTI); exti_interrupt_enable(HALL_B_EXTI); exti_interrupt_enable(HALL_C_EXTI); // HAL_TIMEx_HallSensor_Start(&htimx_hall); // LED1_OFF; // HAL_TIM_TriggerCallback(&htimx_hall); // Ö´ÐÐÒ»´Î»»Ïà StartSpeedTime(); //start speed timer motor_phasechange(); // Ö´ÐÐÒ»´Î»»Ïà motor_drive.enable_flag = 1; } @@ -35,6 +37,7 @@ exti_interrupt_disable(HALL_B_EXTI); exti_interrupt_disable(HALL_C_EXTI); // HAL_TIMEx_HallSensor_Stop(&htimx_hall); StopSpeedTime(); //stop speed timer motor_drive.enable_flag = 0; motor_drive.speed = 0; } @@ -42,8 +45,6 @@ uint8_t get_hall_state(void) { uint8_t state = 0; #if 1 /* ¶ÁÈ¡»ô¶û´«¸ÐÆ÷ U µÄ״̬ */ if (Get_HallSensorA_State()) { @@ -61,9 +62,6 @@ { state |= 0x01U << 2; } #else state = (GPIOH->IDR >> 10) & 7; // ¶Á 3 ¸ö»ô¶û´«¸ÐÆ÷µÄ״̬ #endif return state; // ·µ»Ø´«¸ÐÆ÷״̬ } @@ -75,14 +73,14 @@ float f = 0; /* ¼ÆËãËÙ¶È£º µç»úÿתһȦ¹²ÓÃ12¸öÂö³å£¬(1.0/(48000000.0/128.0)Ϊ¼ÆÊýÆ÷µÄÖÜÆÚ£¬(1.0/(48000000.0/128.0) * time)Ϊʱ¼ä³¤¡£ µç»úÿתһȦ¹²ÓÃ12¸öÂö³å£¬(1.0/(64000000.0/6400)Ϊ¼ÆÊýÆ÷µÄÖÜÆÚ£¬(1.0/(48000000.0/6400) * time)Ϊʱ¼ä³¤¡£ */ if (time == 0) motor_drive.speed_group[count++] = 0; else { f = (1.0f / (48000000.0f / HALL_PRESCALER_COUNT) * time); f = (1.0f / (64000000.0f / SPEED_PRESCALER_COUNT) * time); f = (1.0f / 12.0f) / (f / 60.0f); motor_drive.speed_group[count++] = f; } @@ -194,159 +192,188 @@ void HAL_HallExti_TriggerCallback(void) { /* »ñÈ¡»ô¶û´«¸ÐÆ÷Òý½Å״̬,×÷Ϊ»»ÏàµÄÒÀ¾Ý */ uint8_t step = 0; step = get_hall_state(); if (exti_interrupt_flag_get(HALL_A_EXTI | HALL_B_EXTI | HALL_C_EXTI)) // ÅжÏÊÇ·ñÓÉ´¥·¢ÖжϲúÉú { // update_motor_speed(step, __HAL_TIM_GET_COMPARE(htim, TIM_CHANNEL_1));//TODO Ö÷¶¨Ê±Æ÷´¦»ñµÃ¼ÆÊýʱ¼ä motor_drive.timeout = 0; exti_interrupt_flag_clear(HALL_A_EXTI | HALL_B_EXTI | HALL_C_EXTI); } motor_phasechange(); // HAL_TIM_GenerateEvent(&htimx_bldcm, TIM_EVENTSOURCE_COM); // Èí¼þ²úÉú»»Ïàʼþ£¬´Ëʱ²Å½«ÅäÖÃдÈë } static void motor_phasechange(void) { uint8_t step = 0; step = get_hall_state(); if (get_bldcm_direction() == MOTOR_FWD) { switch (step) { case 1: /* U+ W- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û step = 7u - step; } switch (step) { /* next step: step 2 configuration .A-C` breakover---------------------------- */ case 1: /* channel U configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_U, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCX_ENABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* channel V configuration */ timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, bldcm_pulse); // ͨµÀ 1 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; /* channel W configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_W, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCXN_ENABLE); case 2: /* V+ U- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û step++; break; __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* next step: step 3 configuration .B-C` breakover---------------------------- */ case 2: /* channel U configuration */ timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, bldcm_pulse); // ͨµÀ 2 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û /* channel V configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_V, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCX_ENABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCXN_DISABLE); break; /* channel W configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_W, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCXN_ENABLE); case 3: /* V+ W- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û step++; break; __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* next step: step 4 configuration .B-A` breakover---------------------------- */ case 3: /* channel U configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_U, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCXN_ENABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, bldcm_pulse); // ͨµÀ 2 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; /* channel V configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_V, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCX_ENABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCXN_DISABLE); case 4: /* W+ V- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* channel W configuration */ timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û step++; break; __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, bldcm_pulse); // ͨµÀ 3 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; /* next step: step 5 configuration .C-A` breakover---------------------------- */ case 4: /* channel U configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_U, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCXN_ENABLE); case 5: /* U+ V -*/ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* channel V configuration */ timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* channel W configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_W, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCX_ENABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, bldcm_pulse); // ͨµÀ 1 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; step++; break; case 6: /* W+ U- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* next step: step 6 configuration .C-B` breakover---------------------------- */ case 5: /* channel U configuration */ timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCXN_DISABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û /* channel V configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_V, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCXN_ENABLE); __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, bldcm_pulse); // ͨµÀ 3 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; } /* channel W configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_W, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCX_ENABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCXN_DISABLE); step++; break; /* next step: step 1 configuration .A-B` breakover---------------------------- */ case 6: /* channel U configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_U, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCX_ENABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_U, TIMER_CCXN_DISABLE); /* channel V configuration */ timer_channel_output_mode_config(TIMER0, MOTOR_OUT_CH_V, TIMER_OC_MODE_PWM1); timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_V, TIMER_CCXN_ENABLE); /* channel W configuration */ timer_channel_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, MOTOR_OUT_CH_W, TIMER_CCXN_DISABLE); step = 1; break; } } /** * @brief Í£Ö¹pwmÊä³ö * @param ÎÞ * @retval ÎÞ */ extern void stop_pwm_output(void) { timer_channel_output_state_config(TIMER0, TIMER_CH_0, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_0, TIMER_CCXN_DISABLE); timer_channel_output_state_config(TIMER0, TIMER_CH_1, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_1, TIMER_CCXN_DISABLE); timer_channel_output_state_config(TIMER0, TIMER_CH_2, TIMER_CCX_DISABLE); timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_2, TIMER_CCXN_DISABLE); } /** * @brief ÉèÖÃpwmÊä³öµÄÕ¼¿Õ±È * @param pulse:ÒªÉèÖõÄÕ¼¿Õ±È * @retval ÎÞ */ void set_pwm_pulse(uint16_t pulse) { /* ÉèÖö¨Ê±Æ÷ͨµÀÊä³ö PWM µÄÕ¼¿Õ±È */ motor_pluse = pulse; //SetPwmDuty(); if (motor_drive.enable_flag) { motor_phasechange(); } } extern void BLDC_SpeedAndPID(void) { if (motor_drive.timeout++ > 100) // ÓÐÒ»´ÎÔÚ²úÉú¸üÐÂÖжÏǰ»ô¶û´«¸ÐÆ÷ûÓв¶»ñµ½Öµ { // printf("¶Âת³¬Ê±\r\n"); motor_drive.timeout = 0; /* ¶Âת³¬Ê±Í£Ö¹ PWM Êä³ö */ hall_disable(); // ½ûÓûô¶û´«¸ÐÆ÷½Ó¿Ú stop_pwm_output(); // Í£Ö¹ PWM Êä³ö set_bldcm_disable(); motor_drive.speed = 0; } else { switch (step) { case 1: /* W+ U- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, bldcm_pulse); // ͨµÀ 3 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; case 2: /* U+ V -*/ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, bldcm_pulse); // ͨµÀ 1 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; case 3: /* W+ V- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, bldcm_pulse); // ͨµÀ 3 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; case 4: /* V+ W- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, bldcm_pulse); // ͨµÀ 2 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; case 5: /* V+ U- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, 0); // ͨµÀ 1 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, bldcm_pulse); // ͨµÀ 2 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; case 6: /* U+ W- */ __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_2, 0); // ͨµÀ 2 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM2_GPIO_PORT, MOTOR_OCNPWM2_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_3, 0); // ͨµÀ 3 ÅäÖÃΪ 0 HAL_GPIO_WritePin(MOTOR_OCNPWM1_GPIO_PORT, MOTOR_OCNPWM1_PIN, GPIO_PIN_RESET); // ¹Ø±ÕÏÂÇÅ±Û __HAL_TIM_SET_COMPARE(&htimx_bldcm, TIM_CHANNEL_1, bldcm_pulse); // ͨµÀ 1 ÅäÖõÄÕ¼¿Õ±È HAL_GPIO_WritePin(MOTOR_OCNPWM3_GPIO_PORT, MOTOR_OCNPWM3_PIN, GPIO_PIN_SET); // ¿ªÆôÏÂÇÅ±Û break; } bldcm_pid_control(); } HAL_TIM_GenerateEvent(&htimx_bldcm, TIM_EVENTSOURCE_COM); // Èí¼þ²úÉú»»Ïàʼþ£¬´Ëʱ²Å½«ÅäÖÃдÈë } 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,9 @@ */ void bldcm_init(void) { PWM_TIMx_Configuration(); // µç»ú¿ØÖƶ¨Ê±Æ÷£¬Òý½Å³õʼ»¯ hall_tim_config(); // »ô¶û´«¸ÐÆ÷³õʼ»¯ sd_gpio_config(); // sd Òý½Å³õʼ»¯ // PWM_TIMx_Configuration(); // µç»ú¿ØÖƶ¨Ê±Æ÷£¬Òý½Å³õʼ»¯ // hall_tim_config(); // »ô¶û´«¸ÐÆ÷³õʼ»¯ sd_gpio_config(); // sd Òý½Å³õʼ»¯ } /** @@ -27,24 +32,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 +42,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 +54,7 @@ */ void set_bldcm_direction(motor_dir_t dir) { bldcm_data.direction = dir; bldcm_data.direction = dir; } /** @@ -76,7 +64,7 @@ */ motor_dir_t get_bldcm_direction(void) { return bldcm_data.direction; return bldcm_data.direction; } /** @@ -86,8 +74,8 @@ */ void set_bldcm_enable(void) { bldcm_data.is_enable = 1; hall_enable(); bldcm_data.is_enable = 1; hall_enable(); } /** @@ -97,14 +85,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 +102,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 } } USR/SRC/gd32e23x_it.c
@@ -151,5 +151,26 @@ */ void EXTI4_15_IRQHandler(void) { HAL_HallExti_TriggerCallback(); if (RESET != exti_interrupt_flag_get(EXTI_4 | EXTI_5 | EXTI_15)) { HAL_HallExti_TriggerCallback(); } exti_interrupt_flag_clear(EXTI_4 | EXTI_5 | EXTI_15); } /*! \brief this function handles time2 \param[in] none \param[out] none \retval none */ void TIMER2_IRQHandler(void) { if (SET == timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_UP)) //time2 count up overflow { BLDC_SpeedAndPID(); timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_UP); } } USR/SRC/main.c
@@ -1,6 +1,7 @@ #include "gd32e23x.h" #include <stdio.h> #include "gpio.h" #include "adc.h" #include "pwm.h" #include "uart.h" #include "clock.h" USR/SRC/pwm.c
@@ -11,8 +11,8 @@ #include "pwm.h" /***************************************************************************************************** Timer2 is used to capture hall input APB1 is 48M ,so Timer0 clock is 48M,after prescaler 480 is 0.1M(10us) Timer2 is used to speed cal APB1 is 64M ,so Timer2 clock is 64M,after prescaler 480 is 0.1M(10us) input pin: PA15-CO PB4-AO @@ -20,41 +20,19 @@ *****************************************************************************************************/ static void Timer2Init(void) { timer_ic_parameter_struct capturecfg[2]; timer_parameter_struct timercontralcfg; timer_deinit(TIMER2); capturecfg[0].icfilter = 0; //no filter capturecfg[0].icpolarity = TIMER_IC_POLARITY_BOTH_EDGE; //both edge capturecfg[0].icprescaler = TIMER_IC_PSC_DIV1; capturecfg[0].icselection = TIMER_IC_SELECTION_DIRECTTI; //input and icy is mapped on TIy capturecfg[1].icfilter = 0; capturecfg[1].icpolarity = TIMER_IC_POLARITY_BOTH_EDGE; capturecfg[1].icprescaler = TIMER_IC_PSC_DIV1; capturecfg[1].icselection = TIMER_IC_SELECTION_DIRECTTI; timercontralcfg.prescaler = 0; timercontralcfg.alignedmode = 0; timercontralcfg.counterdirection = 0; timercontralcfg.period = PERIOD_CAP; timer_input_capture_config(TIMER2, TIMER_CH_0, &capturecfg[0]); timer_input_capture_config(TIMER2, TIMER_CH_1, &capturecfg[1]); timercontralcfg.prescaler = 6399; //64Mhz /6400 =0.01Mhz = 100us timercontralcfg.alignedmode = TIMER_COUNTER_EDGE; timercontralcfg.counterdirection = TIMER_COUNTER_UP; timercontralcfg.period = PERIOD_CAP; //100us*10000=1s timer_init(TIMER2, &timercontralcfg); timer_interrupt_flag_clear(TIMER2, TIMER_INT_CH0); timer_interrupt_flag_clear(TIMER2, TIMER_INT_CH1); timer_interrupt_flag_clear(TIMER2, TIMER_INT_TRG); timer_interrupt_enable(TIMER2, TIMER_INT_CH0); timer_hall_mode_config(TIMER2, TIMER_HALLINTERFACE_ENABLE); timer_interrupt_enable(TIMER2, TIMER_INT_TRG); timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); timer_interrupt_enable(TIMER2, TIMER_INT_UP); nvic_irq_enable(TIMER2_IRQn, 1); timer_prescaler_config(TIMER2, 47, TIMER_PSC_RELOAD_NOW); //48Mhz /48 =1Mhz = 1us } /***************************************************************************************************** @@ -70,16 +48,17 @@ *****************************************************************************************************/ static void Timer0Init(void) { uint16_t Duty = 0; uint16_t Duty = PERIOD_CMP + 1; timer_parameter_struct timercontralcfg; timer_oc_parameter_struct timeroutcfg[3]; timer_break_parameter_struct timer_breakpara; //config timer0 timercontralcfg.prescaler = 0; //counter prescaler is 0 timercontralcfg.alignedmode = TIMER_COUNTER_CENTER_UP; //center align and counter style depend on dir set timercontralcfg.counterdirection = TIMER_COUNTER_UP; //counter style is up timercontralcfg.clockdivision = TIMER_CKDIV_DIV1; //Timer0 is 48M timercontralcfg.period = PERIOD_CMP; // set period(counter value 0~2^15) timercontralcfg.prescaler = 0; //counter prescaler is 0 timercontralcfg.alignedmode = TIMER_COUNTER_EDGE; //center align and counter style depend on dir set timercontralcfg.counterdirection = TIMER_COUNTER_UP; //counter style is up timercontralcfg.clockdivision = TIMER_CKDIV_DIV1; //Timer0 is 64M timercontralcfg.period = PERIOD_CMP; // set period(counter value 0~2^15) timer_init(TIMER0, &timercontralcfg); timeroutcfg[0].ocpolarity = TIMER_OC_POLARITY_HIGH; //channel output polarity is high @@ -87,21 +66,21 @@ timeroutcfg[0].outputstate = TIMER_CCX_ENABLE; //channel enable timeroutcfg[0].outputnstate = TIMER_CCXN_ENABLE; //channel complementary enable timeroutcfg[0].ocidlestate = TIMER_OC_IDLE_STATE_LOW; timeroutcfg[0].ocnidlestate = TIMER_OCN_IDLE_STATE_HIGH; timeroutcfg[0].ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; timeroutcfg[1].ocpolarity = TIMER_OC_POLARITY_HIGH; //channel output polarity is high timeroutcfg[1].ocnpolarity = TIMER_OCN_POLARITY_HIGH; timeroutcfg[1].outputstate = TIMER_CCX_ENABLE; //channel enable timeroutcfg[1].outputnstate = TIMER_CCXN_ENABLE; //channel complementary enable timeroutcfg[1].ocidlestate = TIMER_OC_IDLE_STATE_LOW; timeroutcfg[1].ocnidlestate = TIMER_OCN_IDLE_STATE_HIGH; timeroutcfg[1].ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; timeroutcfg[2].ocpolarity = TIMER_OC_POLARITY_HIGH; //channel output polarity is high timeroutcfg[2].ocnpolarity = TIMER_OCN_POLARITY_HIGH; timeroutcfg[2].outputstate = TIMER_CCX_ENABLE; //channel enable timeroutcfg[2].outputnstate = TIMER_CCXN_ENABLE; //channel complementary enable timeroutcfg[2].ocidlestate = TIMER_OC_IDLE_STATE_LOW; timeroutcfg[2].ocnidlestate = TIMER_OCN_IDLE_STATE_HIGH; timeroutcfg[2].ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; timer_channel_output_config(TIMER0, TIMER_CH_0, &timeroutcfg[0]); timer_channel_output_config(TIMER0, TIMER_CH_1, &timeroutcfg[1]); @@ -111,17 +90,25 @@ timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_1, Duty); timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_2, Duty); timer_channel_output_mode_config(TIMER0, TIMER_CH_0, TIMER_OC_MODE_PWM1); //set pwm1 mode timer_channel_output_mode_config(TIMER0, TIMER_CH_0, TIMER_OC_MODE_PWM1); //set pwm1 mode first low then high timer_channel_output_mode_config(TIMER0, TIMER_CH_1, TIMER_OC_MODE_PWM1); timer_channel_output_mode_config(TIMER0, TIMER_CH_2, TIMER_OC_MODE_PWM1); timer_channel_output_shadow_config(TIMER0, TIMER_CH_0, TIMER_OC_SHADOW_DISABLE); //shadow disable timer_channel_output_shadow_config(TIMER0, TIMER_CH_1, TIMER_OC_SHADOW_DISABLE); timer_channel_output_shadow_config(TIMER0, TIMER_CH_2, TIMER_OC_SHADOW_DISABLE); timer_channel_output_shadow_config(TIMER0, TIMER_CH_0, TIMER_OC_SHADOW_ENABLE); //shadow ENable timer_channel_output_shadow_config(TIMER0, TIMER_CH_1, TIMER_OC_SHADOW_ENABLE); timer_channel_output_shadow_config(TIMER0, TIMER_CH_2, TIMER_OC_SHADOW_ENABLE); timer_interrupt_enable(TIMER0, TIMER_INT_CH0); timer_interrupt_enable(TIMER0, TIMER_INT_CH1); timer_interrupt_enable(TIMER0, TIMER_INT_CH2); /* configure TIMER break function */ timer_break_struct_para_init(&timer_breakpara); /* automatic output enable, break, dead time and lock configuration*/ timer_breakpara.runoffstate = TIMER_ROS_STATE_DISABLE; timer_breakpara.ideloffstate = TIMER_IOS_STATE_DISABLE; timer_breakpara.deadtime = 255; timer_breakpara.breakpolarity = TIMER_BREAK_POLARITY_LOW; timer_breakpara.outputautostate = TIMER_OUTAUTO_ENABLE; timer_breakpara.protectmode = TIMER_CCHP_PROT_0; timer_breakpara.breakstate = TIMER_BREAK_ENABLE; timer_break_config(TIMER0, &timer_breakpara); timer_primary_output_config(TIMER0, ENABLE); timer_auto_reload_shadow_enable(TIMER0); @@ -132,13 +119,13 @@ // rcu_periph_clock_enable(RCU_TIMER2); rcu_periph_clock_enable(RCU_TIMER0); Timer0Init(); // Timer2Init(); Timer2Init(); timer_enable(TIMER0); // timer_enable(TIMER2); } void SetPwmDuty(uint16_t ch, uint32_t duty) { uint32_t duty_temp = (PERIOD_CMP >= duty) ? (PERIOD_CMP - duty) : 0; timer_channel_output_pulse_value_config(TIMER0, ch, duty); } @@ -146,3 +133,13 @@ { TIMER_CAR(TIMER0) = period; } extern void StartSpeedTime(void) { timer_enable(TIMER2); } extern void StopSpeedTime(void) { timer_disable(TIMER2); } pid/bsp_pid.c
New file @@ -0,0 +1,90 @@ #include "bsp_pid.h" #include "bldc_ctrl.h" //¶¨ÒåÈ«¾Ö±äÁ¿ pid_t pid; /** * @brief PID²ÎÊý³õʼ»¯ * @note ÎÞ * @retval ÎÞ */ void PID_param_init() { /* ³õʼ»¯²ÎÊý */ pid.target_val = 0.0; pid.actual_val = 0.0; pid.err = 0.0; pid.err_last = 0.0; pid.integral = 0.0; pid.Kp = 0.07; //24 pid.Ki = 0.03; pid.Kd = 0.0; #if defined(PID_ASSISTANT_EN) float pid_temp[3] = {pid.Kp, pid.Ki, pid.Kd}; set_computer_value(SEND_P_I_D_CMD, CURVES_CH1, pid_temp, 3); // ¸øÍ¨µÀ 1 ·¢ËÍ P I D Öµ #endif } /** * @brief ÉèÖÃÄ¿±êÖµ * @param val Ä¿±êÖµ * @note ÎÞ * @retval ÎÞ */ void set_pid_target(float temp_val) { pid.target_val = temp_val; // ÉèÖõ±Ç°µÄÄ¿±êÖµ } /** * @brief »ñȡĿ±êÖµ * @param ÎÞ * @note ÎÞ * @retval Ä¿±êÖµ */ float get_pid_target(void) { return pid.target_val; // ÉèÖõ±Ç°µÄÄ¿±êÖµ } /** * @brief ÉèÖñÈÀý¡¢»ý·Ö¡¢Î¢·ÖϵÊý * @param p£º±ÈÀýϵÊý P * @param i£º»ý·ÖϵÊý i * @param d£ºÎ¢·ÖϵÊý d * @note ÎÞ * @retval ÎÞ */ void set_p_i_d(float p, float i, float d) { pid.Kp = p; // ÉèÖñÈÀýϵÊý P pid.Ki = i; // ÉèÖûý·ÖϵÊý I pid.Kd = d; // ÉèÖÃ΢·ÖϵÊý D } /** * @brief PIDË㷨ʵÏÖ * @param actual_val:ʵ¼ÊÖµ * @note ÎÞ * @retval ͨ¹ýPID¼ÆËãºóµÄÊä³ö */ float PID_realize(float actual_val) { /*¼ÆËãÄ¿±êÖµÓëʵ¼ÊÖµµÄÎó²î*/ pid.err = pid.target_val - actual_val; pid.integral += pid.err; /*PIDË㷨ʵÏÖ*/ pid.actual_val = pid.Kp * pid.err + pid.Ki * pid.integral + pid.Kd * (pid.err - pid.err_last); /*Îó²î´«µÝ*/ pid.err_last = pid.err; /*·µ»Øµ±Ç°Êµ¼ÊÖµ*/ return pid.actual_val; } pid/bsp_pid.h
New file @@ -0,0 +1,27 @@ #ifndef __BSP_PID_H #define __BSP_PID_H #include "gd32e23x.h" // #include "stm32f1xx.h" // #include "./usart/bsp_debug_usart.h" // #include <stdio.h> // #include <stdlib.h> // #include <math.h> typedef struct { float target_val; //Ä¿±êÖµ float actual_val; //ʵ¼ÊÖµ float err; //¶¨Ò寫²îÖµ float err_last; //¶¨ÒåÉÏÒ»¸öÆ«²îÖµ float Kp, Ki, Kd; //¶¨Òå±ÈÀý¡¢»ý·Ö¡¢Î¢·ÖϵÊý float integral; //¶¨Òå»ý·ÖÖµ } pid_t; void PID_param_init(void); void set_pid_target(float temp_val); float get_pid_target(void); void set_p_i_d(float p, float i, float d); float PID_realize(float actual_val); void time_period_fun(void); #endif