diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..6aa1d4d --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "speed_to_hall.h": "c" + } +} \ No newline at end of file diff --git a/APP/Appcfg/app.c b/APP/Appcfg/app.c index a66f09e..7d00bbd 100644 --- a/APP/Appcfg/app.c +++ b/APP/Appcfg/app.c @@ -50,6 +50,8 @@ #include "systick.h" #include "service_error_count.h" +#include "speed_to_hall.h" + void task_printf_init(); // @@ -126,6 +128,7 @@ static void task_start (void *p_arg) // term_printf("\n udp init \r\n\r\n"); OSTimeDlyHMSM(0u, 0u, 0u, 100u); + init_hall_speed_module(); //光电开关初始化 init_photoelectric_switch_module(); diff --git a/APP/Common/ptz_default_value.h b/APP/Common/ptz_default_value.h index c53fce9..e84962e 100644 --- a/APP/Common/ptz_default_value.h +++ b/APP/Common/ptz_default_value.h @@ -336,7 +336,6 @@ #ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_24V - ///ˮƽٱ #define PTZ_HORI_MOTOR_RATIO 3.0 ///ˮƽּٱ @@ -346,7 +345,7 @@ ///ˮƽģѹֵ #define PTZ_HORI_VR_MAX 1999 ///ˮƽģѹСֵ - #define PTZ_HORI_VR_MIN 0 + #define PTZ_HORI_VR_MIN 300 ///תٵλ ת/ÿ ///ˮƽת diff --git a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c index 913f112..8d76405 100644 --- a/APP/Device/Device_speed/speed_to_bldc.c +++ b/APP/Device/Device_speed/speed_to_bldc.c @@ -7,6 +7,8 @@ #include "full_bridge.h" #include "as5047d.h" +#include "speed_to_hall.h" + #ifdef PTZ_BLDC_MOTOR //̨ʵת @@ -109,6 +111,10 @@ static float ptz_hori_pid_calculate(float H_SampSpeed) // } // H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError; + + // H_IIncPid = PTZ_HORI_PID_HORI_KP; + + //洢´μ g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; g_ptz.hori_pid.LastError = H_IError; @@ -298,7 +304,8 @@ static void ptz_hori_pid_task() #endif -#ifdef PTZ_PID_HALL_SPEED // +// #ifdef PTZ_PID_HALL_SPEED // +#if 0 g_ptz.hori_pid.hall_h1_count = 0; g_ptz.hori_pid.hall_h2_count = 0; g_ptz.hori_pid.hall_h3_count = 0; @@ -610,6 +617,42 @@ static void ptz_hori_pid_task() // } // else +#ifdef PTZ_HALL_SPEED_SL + if (g_ptz.hori_speed_actual == 0) { + h_pwm_duty_change(PTZ_HORI_VR_MIN); + } + + g_speed_to_hall.hori_speed_t.flag = calculation_enable; + + while (!(g_speed_to_hall.hori_speed_t.flag == calculation_start + || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)) { + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + } + + OSTimeDlyHMSM(0u, 0u, 0u, 100u); + g_speed_to_hall.hori_speed_t.flag = calculation_ok; + + while (!(g_speed_to_hall.hori_speed_t.flag == calculation_end + || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)) { + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + } + + int32_t time; + if (g_speed_to_hall.hori_speed_t.endTime_60ms > g_speed_to_hall.hori_speed_t.startTime_60ms) { + time = (g_speed_to_hall.hori_speed_t.endTime_60ms - g_speed_to_hall.hori_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD + + g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us); + } + else if (g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.hori_speed_t.startTime_60ms) { + time = g_speed_to_hall.hori_speed_t.endTime_us - g_speed_to_hall.hori_speed_t.startTime_us; + } + else { + time = (g_speed_to_hall.hori_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.hori_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD + + g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us); + } + + g_ptz.hori_speed_actual = (float)g_speed_to_hall.hori_speed_t.hallNum * 5000000.0 / (float)time / PTZ_HORI_RATIO; +#endif + if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2) {//ٶȴ趨ٶȵ3 g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * 0.7;////ֱ߼ @@ -661,7 +704,7 @@ static void ptz_hori_pid_task() //PIDĵתģѹȼ뵽ģѹоƬ // hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint); - h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); + // h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); //ǰPIDֵ g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h index 5d28824..119116c 100644 --- a/APP/Device/Device_speed/speed_to_bldc.h +++ b/APP/Device/Device_speed/speed_to_bldc.h @@ -230,6 +230,8 @@ //#define PTZ_VERT_PID_JY02A_SPEED #define PTZ_PID_HALL_SPEED 1 + #define PTZ_HALL_SPEED_SL 1 + #define PTZ_HORI_PID_T 30u #define PTZ_HORI_PID_HORI_KP 20.0//ϵ #define PTZ_HORI_PID_HORI_TI 100.0 //ϵ diff --git a/APP/Device/device_Other/device_interrupt.c b/APP/Device/device_Other/device_interrupt.c index 33031d6..2d02825 100644 --- a/APP/Device/device_Other/device_interrupt.c +++ b/APP/Device/device_Other/device_interrupt.c @@ -8,6 +8,9 @@ #include "gd32f4xx_it.h" #include "ptz_type_select.h" #include "get_angle.h" + +#include "speed_to_hall.h" + /// @brief ⲿжϳʼ /// @param[in] usart_periph:EXTI_IRQ_init /// @return none @@ -604,6 +607,24 @@ void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex) #ifdef PTZ_BLDC_MOTOR #ifdef PTZ_HALL_FEEDBACK + if (g_speed_to_hall.hori_speed_t.flag == calculation_enable) { + g_speed_to_hall.hori_speed_t.startTime_us = TIMER_CNT(TIMER6); + g_speed_to_hall.hori_speed_t.startTime_60ms = g_speed_to_hall.time_60ms; + g_speed_to_hall.hori_speed_t.hallNum = 0; + g_speed_to_hall.hori_speed_t.flag = calculation_start; + } + + if (g_speed_to_hall.hori_speed_t.flag == calculation_start) { + g_speed_to_hall.hori_speed_t.hallNum++; + } + + if (g_speed_to_hall.hori_speed_t.flag == calculation_ok) { + // g_speed_to_hall.hori_speed_t.hallNum++; + g_speed_to_hall.hori_speed_t.endTime_us = TIMER_CNT(TIMER6); + g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.time_60ms; + g_speed_to_hall.hori_speed_t.flag = calculation_end; + } + switch(hall_linex) { case EXTI_10://ˮƽHALL-1 diff --git a/BSP/Driver/speed/speed_to_hall.c b/BSP/Driver/speed/speed_to_hall.c new file mode 100644 index 0000000..5250c5a --- /dev/null +++ b/BSP/Driver/speed/speed_to_hall.c @@ -0,0 +1,61 @@ + +#include "speed_to_hall.h" + + +speed_hall g_speed_to_hall = {0}; + +/** + * @brief 初始化定时器+霍尔计算速度 + * @param + * @retval + * + */ +void init_hall_speed_module(void) +{ + timer_parameter_struct timer_initpara; + + rcu_periph_clock_enable(RCU_TIMER6); + timer_struct_para_init(&timer_initpara); + timer_deinit(TIMER6); + + // TIMER1 configuration + timer_initpara.prescaler = 99; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE;//计数模式，边缘对齐 + timer_initpara.counterdirection = TIMER_COUNTER_UP;//计数方向 + timer_initpara.period = SPEED_TIMER_PERIOD;//计数重装载值，确定周期，，该值可在任意时刻进行修改以改变周期，TIMER_CAR(timer_periph) = (uint32_t)initpara->period; + timer_initpara.clockdivision = TIMER_CKDIV_DIV1;//时钟分频 + timer_initpara.repetitioncounter = 0;//计数重复值 + timer_init(TIMER6, &timer_initpara); + + timer_auto_reload_shadow_enable(TIMER6); + timer_enable(TIMER6); + + timer_interrupt_enable(TIMER6, TIMER_INT_UP);//定时器更新中断使能 + nvic_irq_enable(TIMER6_IRQn, 2U, 2U); + + g_speed_to_hall.hori_speed_t.flag = calculation_disable; + g_speed_to_hall.vert_speed_t.flag = calculation_disable; +} + +/** + * @brief 定时器1中断服务函数 + * @param + * @retval + * + */ +void TIMER6_IRQHandler(void) +{ + if(RESET != timer_interrupt_flag_get(TIMER6, TIMER_INT_FLAG_UP)) { + g_speed_to_hall.time_60ms++; + if (g_speed_to_hall.time_60ms >= TIME_60MS_MAX) { + g_speed_to_hall.time_60ms = 0; + } + timer_interrupt_flag_clear(TIMER6, TIMER_INT_FLAG_UP); + } +} + + + + + + diff --git a/BSP/Driver/speed/speed_to_hall.h b/BSP/Driver/speed/speed_to_hall.h new file mode 100644 index 0000000..5c52792 --- /dev/null +++ b/BSP/Driver/speed/speed_to_hall.h @@ -0,0 +1,40 @@ +#ifndef __BSP_DRIVER_SPEED_HALL_H_ +#define __BSP_DRIVER_SPEED_HALL_H_ +//#include "ptz_type_select.h" + +#include "gd32f4xx.h" + + +typedef struct _speed_t { + uint32_t hallNum; //霍尔编码器次数（运行方向的） + uint32_t startTime_60ms; //开始时间 + uint32_t startTime_us; + uint32_t endTime_60ms; //结束时间 + uint32_t endTime_us; + uint8_t flag; //标志位 +}speed_t; + +typedef struct _speed_to_hall { + speed_t hori_speed_t; + speed_t vert_speed_t; + uint32_t time_60ms; +}speed_hall; + +typedef enum _calculation_speed_type { + calculation_disable = 0, //未开始计算速度 + calculation_enable = 1, //使能计算速度 + calculation_ok = 2, //达到计算速度时间 + calculation_start = 3, //开始计算速度 + calculation_end = 4, //结束计算速度 +}calculation_speed_type; + +extern speed_hall g_speed_to_hall; + +#define TIME_60MS_MAX 4000000000 +#define SPEED_TIMER_PERIOD 60000 + +void init_hall_speed_module(void); + + + +#endif diff --git a/PROJECT/OS2.ewp b/PROJECT/OS2.ewp index c85f083..b296e1e 100644 --- a/PROJECT/OS2.ewp +++ b/PROJECT/OS2.ewp @@ -409,6 +409,7 @@ $PROJ_DIR$\..\BSP\Driver\timer $PROJ_DIR$\..\BSP\Driver\full_bridge $PROJ_DIR$\..\APP\Service + $PROJ_DIR$\..\BSP\Driver\speed