diff --git a/APP/Appcfg/app.c b/APP/Appcfg/app.c index 537d728..c6fc5ad 100644 --- a/APP/Appcfg/app.c +++ b/APP/Appcfg/app.c @@ -197,7 +197,7 @@ static void task_start (void *p_arg) //角度计算模块初始化 init_angle_module(); // term_printf("\n angle init \r\n\r\n"); - + task_printf_init(); //自检模块初始化 diff --git a/APP/Common/ptz_default_value.h b/APP/Common/ptz_default_value.h index 9a6fce1..b977b07 100644 --- a/APP/Common/ptz_default_value.h +++ b/APP/Common/ptz_default_value.h @@ -363,11 +363,11 @@ #endif #ifdef NEW_MOTOR ///水平电机减速比 - #define PTZ_HORI_MOTOR_RATIO 3.0 + #define PTZ_HORI_MOTOR_RATIO 5.0 ///水平大齿轮减速比 #define PTZ_HORI_BIG_GEAR_RATIO 50.0 ///水平总减速比 - #define PTZ_HORI_RATIO 150.0 + #define PTZ_HORI_RATIO 250.0 ///水平电机调速模拟电压最大值 #define PTZ_HORI_VR_MAX 1999 ///水平电机调速模拟电压最小值 @@ -379,11 +379,11 @@ ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 405.0 ///水平云台最大转速 - #define PTZ_HORI_MAX_SPEED 20.0 + #define PTZ_HORI_MAX_SPEED 12.0 ///水平云台最小转速 - #define PTZ_HORI_MIN_SPEED 2.7 + #define PTZ_HORI_MIN_SPEED 1.63 ///水平云台默认最佳速度 - #define PTZ_HORI_BEST_SPEED 14.4 + #define PTZ_HORI_BEST_SPEED 9.0 #endif /**********************************************************/ diff --git a/APP/Device/Device_rotate/rotate_bldc.h b/APP/Device/Device_rotate/rotate_bldc.h index 8ccea85..622f908 100644 --- a/APP/Device/Device_rotate/rotate_bldc.h +++ b/APP/Device/Device_rotate/rotate_bldc.h @@ -230,7 +230,7 @@ ///ɲ #define PTZ_HORI_STOP_NEAR_DISTANCE 0.1 ///ɲԶ - #define PTZ_HORI_STOP_FAR_DISTANCE 4.0 + #define PTZ_HORI_STOP_FAR_DISTANCE 4.5 ///ɲ #define PTZ_VERT_STOP_NEAR_DISTANCE 0.1 diff --git a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c index 980bf7c..6c4f395 100644 --- a/APP/Device/Device_speed/speed_to_bldc.c +++ b/APP/Device/Device_speed/speed_to_bldc.c @@ -86,12 +86,12 @@ void ptz_pid_init() g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T); - g_speed_to_hall.hori_speed_pid.kp = PTZ_HORI_PID_HORI_KP; + // g_speed_to_hall.hori_speed_pid.kp = PTZ_HORI_PID_HORI_KP; // g_speed_to_hall.hori_speed_pid.ki = PTZ_HORI_PID_HORI_KP * (20.0 / PTZ_HORI_PID_HORI_TI); - g_speed_to_hall.hori_speed_pid.ki = PTZ_HORI_PID_HORI_TI; - g_speed_to_hall.vert_speed_pid.kp = PTZ_VERT_PID_VERT_KP; + // g_speed_to_hall.hori_speed_pid.ki = PTZ_HORI_PID_HORI_TI; + g_speed_to_hall.hori_speed_pid.k = PTZ_HORI_PID_HORI_KP + PTZ_HORI_PID_HORI_TI; // g_speed_to_hall.vert_speed_pid.ki = PTZ_VERT_PID_VERT_KP * (20.0 / PTZ_VERT_PID_VERT_TI); - g_speed_to_hall.vert_speed_pid.ki = PTZ_VERT_PID_VERT_TI; + g_speed_to_hall.vert_speed_pid.k = PTZ_VERT_PID_VERT_KP + PTZ_VERT_PID_VERT_TI; } void ptz_hori_pid_clear_zero() @@ -123,15 +123,18 @@ static float ptz_hori_pid_calculate(float H_SampSpeed) // H_IIncPid = g_speed_to_hall.hori_speed_pid.kp * (H_IError - g_ptz.hori_pid.LastError) // + g_speed_to_hall.hori_speed_pid.ki * H_IError; // H_IIncPid = g_speed_to_hall.hori_speed_pid.kp * (H_IError - g_ptz.hori_pid.LastError); - g_speed_to_hall.hori_speed_pid.sum_error += H_IError; - if (g_speed_to_hall.hori_speed_pid.sum_error > 20000) { - g_speed_to_hall.hori_speed_pid.sum_error = 20000; - } - else if (g_speed_to_hall.hori_speed_pid.sum_error < -20000) { - g_speed_to_hall.hori_speed_pid.sum_error = -20000; - } + // g_speed_to_hall.hori_speed_pid.sum_error += H_IError; + // if (g_speed_to_hall.hori_speed_pid.sum_error > 20000) { + // g_speed_to_hall.hori_speed_pid.sum_error = 20000; + // } + // else if (g_speed_to_hall.hori_speed_pid.sum_error < -20000) { + // g_speed_to_hall.hori_speed_pid.sum_error = -20000; + // } + + // H_IIncPid = g_speed_to_hall.hori_speed_pid.kp * H_IError + g_speed_to_hall.hori_speed_pid.ki * g_speed_to_hall.hori_speed_pid.sum_error; + + H_IIncPid = g_speed_to_hall.hori_speed_pid.k * H_IError; - H_IIncPid = g_speed_to_hall.hori_speed_pid.kp * H_IError + g_speed_to_hall.hori_speed_pid.ki * g_speed_to_hall.hori_speed_pid.sum_error; // H_IIncPid = g_speed_to_hall.hori_speed_pid.ki * g_speed_to_hall.hori_speed_pid.sum_error; // H_IIncPid = PTZ_HORI_PID_HORI_KP; @@ -890,19 +893,21 @@ static float ptz_vert_pid_calculate(float SampSpeed) // IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError; - g_speed_to_hall.vert_speed_pid.sum_error += IError; - if (g_speed_to_hall.vert_speed_pid.sum_error > 20000) { - g_speed_to_hall.vert_speed_pid.sum_error = 20000; - } - else if (g_speed_to_hall.vert_speed_pid.sum_error < -20000) { - g_speed_to_hall.vert_speed_pid.sum_error = -20000; - } + // g_speed_to_hall.vert_speed_pid.sum_error += IError; + // if (g_speed_to_hall.vert_speed_pid.sum_error > 20000) { + // g_speed_to_hall.vert_speed_pid.sum_error = 20000; + // } + // else if (g_speed_to_hall.vert_speed_pid.sum_error < -20000) { + // g_speed_to_hall.vert_speed_pid.sum_error = -20000; + // } - IIncPid = g_speed_to_hall.vert_speed_pid.kp * IError + g_speed_to_hall.vert_speed_pid.ki * g_speed_to_hall.vert_speed_pid.sum_error; + // IIncPid = g_speed_to_hall.vert_speed_pid.kp * IError + g_speed_to_hall.vert_speed_pid.ki * g_speed_to_hall.vert_speed_pid.sum_error; // IIncPid = PTZ_VERT_PID_VERT_KP * (IError - g_ptz.hori_pid.LastError) + PTZ_VERT_PID_VERT_TI * IError * H_SampTime; // IIncPid = g_speed_to_hall.vert_speed_pid.kp * (IError - g_ptz.hori_pid.LastError) + g_speed_to_hall.vert_speed_pid.ki * IError; + IIncPid = g_speed_to_hall.vert_speed_pid.k * IError; + //洢´μ g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; g_ptz.vert_pid.LastError = IError; @@ -1752,8 +1757,8 @@ void get_hori_speed(void) hallLastNum = hallNum; //PIDֵ - // g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float; - g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual); + g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float; + // g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual); //PIDֵǰֵһֵIJֵijΧ //ֹPID //޶PID޶ @@ -1779,8 +1784,8 @@ void get_hori_speed(void) } //PIDĵתģѹȼ뵽ģѹоƬ - // h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); - h_pwm_duty_change(450); + h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); + // h_pwm_duty_change(450); //ǰPIDֵ g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; @@ -1791,7 +1796,7 @@ void get_hori_speed(void) g_speed_to_hall.hori_speed_t.startTime_us = g_speed_to_hall.time_60ms; g_speed_to_hall.hori_speed_t.speed = 0; g_ptz.hori_pid.LastError = 0; - g_speed_to_hall.hori_speed_pid.sum_error = 0; + // g_speed_to_hall.hori_speed_pid.sum_error = 0; g_ptz.hori_speed_actual = 0; } } @@ -1812,8 +1817,8 @@ void get_vert_speed(void) hallLastNum = hallNum; //PIDֵ - // g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float; - g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual); + g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float; + // g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual); //PIDֵǰֵһֵIJֵijΧ //ֹPID @@ -1838,8 +1843,7 @@ void get_vert_speed(void) if (g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX) { g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX; } - - + //PIDĵתģѹȼ뵽ģѹоƬ v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint); @@ -1852,7 +1856,7 @@ void get_vert_speed(void) g_speed_to_hall.vert_speed_t.startTime_us = g_speed_to_hall.time_60ms; g_speed_to_hall.vert_speed_t.speed = 0; - g_speed_to_hall.vert_speed_pid.sum_error = 0; + // g_speed_to_hall.vert_speed_pid.sum_error = 0; g_ptz.vert_pid.LastError = 0; g_ptz.vert_pid.roll_start = 0; diff --git a/APP/Device/Device_speed/speed_to_bldc.c.rej b/APP/Device/Device_speed/speed_to_bldc.c.rej new file mode 100644 index 0000000..5f8b45a --- /dev/null +++ b/APP/Device/Device_speed/speed_to_bldc.c.rej @@ -0,0 +1,10 @@ +diff a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c (rejected hunks) +@@ -112,7 +112,7 @@ + //�������� + 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; ++ // H_IIncPid = PTZ_HORI_PID_HORI_KP * (H_IError - g_ptz.hori_pid.LastError) + PTZ_HORI_PID_HORI_TI * H_IError; + + + //�洢�������´μ��� diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h index e952049..65d4da1 100644 --- a/APP/Device/Device_speed/speed_to_bldc.h +++ b/APP/Device/Device_speed/speed_to_bldc.h @@ -233,16 +233,16 @@ // #define PTZ_HALL_SPEED_SL 1 #define PTZ_HORI_PID_T 30u - #define PTZ_HORI_PID_HORI_KP 25.0 //ϵ - #define PTZ_HORI_PID_HORI_TI 0.5 //ϵ - #define PTZ_HORI_PID_HORI_TD 0.0 //΢ϵ + #define PTZ_HORI_PID_HORI_KP 5.0 //ϵ + #define PTZ_HORI_PID_HORI_TI 1.0 //ϵ + #define PTZ_HORI_PID_HORI_TD 0.0 //΢ϵ #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PIDֵ޶ #define PTZ_HORI_PID_OUTPUT_LIMIT 400.0//PIDֵ޶ǰֵһֵ֮IJ #define PTZ_VERT_PID_T 30u - #define PTZ_VERT_PID_VERT_KP 80.0 //ϵ - #define PTZ_VERT_PID_VERT_TI 7.0 //ϵ + #define PTZ_VERT_PID_VERT_KP 5.0 //ϵ + #define PTZ_VERT_PID_VERT_TI 1.0 //ϵ #define PTZ_VERT_PID_VERT_TD 0.0 //΢ϵ #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PIDֵ޶ diff --git a/BSP/Driver/speed/speed_to_hall.h b/BSP/Driver/speed/speed_to_hall.h index d9ffdfc..58ceee7 100644 --- a/BSP/Driver/speed/speed_to_hall.h +++ b/BSP/Driver/speed/speed_to_hall.h @@ -24,10 +24,7 @@ typedef struct _speed_t { }speed_t; typedef struct _speed_pid_t { - float kp; - float ki; - float kd; - float sum_error; //积分误差 + float k; }speed_pid_t; typedef struct _speed_to_hall {