#include "ptz_header_file.h" #include #include "app_cfg.h" #include "gd32f4xx.h" #include "device_dac_out.h" #include "Timer.h" #include "full_bridge.h" #include "as5047d.h" #include "speed_to_hall.h" #ifdef PTZ_BLDC_MOTOR static volatile uint32_t hori_speed_task_time = 0; //发送云台实际转速 void ptz_send_speed(char dev, char speed) { unsigned char Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00}; unsigned char Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00}; unsigned short int HSpeed = 0; unsigned short int VSpeed = 0; Speed1[1] = g_ptz.address; Speed2[1] = g_ptz.address; HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5); Speed1[3] = 0X03;//水平电机 Speed1[4] = (unsigned char)(HSpeed >> 8); Speed1[5] = (unsigned char)(HSpeed & 0x00ff); Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1)); VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5); Speed2[3] = 0X04;//垂直电机 Speed2[4] = (unsigned char)(VSpeed >> 8); Speed2[5] = (unsigned char)(VSpeed & 0x00ff); Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2)); switch(speed) { case 1://只查询水平转速 ptz_send_data(dev,Speed1,sizeof(Speed1)); break; case 2://只查询垂直转速 ptz_send_data(dev,Speed2,sizeof(Speed2)); break; default://水平转速垂直转速都查询 ptz_send_data(dev,Speed1,sizeof(Speed1)); ptz_send_data(dev,Speed2,sizeof(Speed2)); break; } } void ptz_pid_init() { g_ptz.hori_pid.SumError = 0;//累计误差 g_ptz.hori_pid.LastError = 0;//上一次输入偏差 g_ptz.hori_pid.PrevError = 0;//上上次输入偏差 g_ptz.hori_pid.LastUT_float = 0;//上次PID输出值u(t) g_ptz.hori_pid.PidUT_float = 0;//PID当前输出值 g_ptz.hori_pid.KP = PTZ_HORI_PID_HORI_KP;//比例常数 g_ptz.hori_pid.TI = PTZ_HORI_PID_HORI_TI;//积分常数 g_ptz.hori_pid.TD = PTZ_HORI_PID_HORI_TD;//微分常数 g_ptz.hori_pid.T = PTZ_HORI_PID_T / 1000.0;//采样周期 //三个简化参数A，B，C g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD); g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T); g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T); g_ptz.vert_pid.SumError = 0;//累计误差 g_ptz.vert_pid.LastError = 0;//上一次输入偏差 g_ptz.vert_pid.PrevError = 0;//上上次输入偏差 g_ptz.vert_pid.LastUT_float = 0;//上次PID输出值u(t) g_ptz.vert_pid.PidUT_float = 0;//PID当前输出值 g_ptz.vert_pid.KP = PTZ_VERT_PID_VERT_KP;//比例常数 g_ptz.vert_pid.TI = PTZ_VERT_PID_VERT_TI;//积分常数 g_ptz.vert_pid.TD = PTZ_VERT_PID_VERT_TD;//微分常数 g_ptz.vert_pid.T = PTZ_VERT_PID_T / 1000.0;//采样周期 //三个简化参数A，B，C g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD); g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T); g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T); } void ptz_hori_pid_clear_zero() { g_ptz.hori_pid.SumError = 0;//累计误差 g_ptz.hori_pid.LastError = 0;//上一次输入偏差 g_ptz.hori_pid.PrevError = 0;//上上次输入偏差 g_ptz.hori_pid.LastUT_float = 0;//上次PID输出值u(t) g_ptz.hori_pid.PidUT_float = 0;//PID当前输出值 g_ptz.hori_pid.PidUT_uint = 0; // hori_dac0_data_out(0); } static float ptz_hori_pid_calculate(float H_SampSpeed, uint32_t H_SampTime) { float H_IError,H_IIncPid; H_IError = g_ptz.hori_speed_set - H_SampSpeed;//当前误差PID_INPUT_LIMIT /*控制PID输入参数*/ // if(H_IError > PTZ_HORI_PID_INPUT_LIMIT) // { // H_IError = PTZ_HORI_PID_INPUT_LIMIT; // } // if(H_IError < (-1 * PTZ_HORI_PID_INPUT_LIMIT)) // { // H_IError = PTZ_HORI_PID_INPUT_LIMIT * -1; // } //增量计算 // 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_IError - g_ptz.hori_pid.LastError) + PTZ_HORI_PID_HORI_TI * H_SampTime * H_IError; //存储误差，用于下次计算 g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; g_ptz.hori_pid.LastError = H_IError; //返回增量值 return H_IIncPid; } //新调速方式 static void ptz_hori_pid_task() { unsigned int time = 0; char i = 0; while(1) { if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { #ifdef PTZ_HORI_PID_as5047d_ANGLE_ASY_SPEED //磁编码器测速，数据清0 g_ptz.hori_as5047d.as5047d_speed_angle_a = 0; g_ptz.hori_as5047d.as5047d_speed_angle_b = 0; g_ptz.hori_as5047d.as5047d_speed_angle_c = 0; //磁编码器测速，测速起始角度 g_ptz.hori_as5047d.as5047d_speed_angle_a = (float)as5047d_vert_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual; //PID测速延时 OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T); //磁编码器测速，测速结束角度 g_ptz.hori_as5047d.as5047d_speed_angle_b = (float)as5047d_vert_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual; //根据磁编码器转动的方向 //由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态 //反转，云台角度减小，磁编码角度增大 if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_as5047d.as5047d_speed_angle_a <= g_ptz.hori_as5047d.as5047d_speed_angle_b) { g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_b - g_ptz.hori_as5047d.as5047d_speed_angle_a; } else { g_ptz.hori_as5047d.as5047d_speed_angle_c = (360.0 - g_ptz.hori_as5047d.as5047d_speed_angle_a) + (g_ptz.hori_as5047d.as5047d_speed_angle_b - 0); } } //正转，云台角度增大，磁编码角度减小 if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_as5047d.as5047d_speed_angle_a >= g_ptz.hori_as5047d.as5047d_speed_angle_b) { g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_a - g_ptz.hori_as5047d.as5047d_speed_angle_b; } else { g_ptz.hori_as5047d.as5047d_speed_angle_c = (360.0 - g_ptz.hori_as5047d.as5047d_speed_angle_b) + (g_ptz.hori_as5047d.as5047d_speed_angle_a - 0); } } //去除转角异常情况 if(g_ptz.hori_as5047d.as5047d_speed_angle_c >= 50.0 || g_ptz.hori_as5047d.as5047d_speed_angle_c < 0) { g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_c_last; } g_ptz.hori_as5047d.as5047d_speed_angle_c_last = g_ptz.hori_as5047d.as5047d_speed_angle_c; //计算磁编码器转速 g_ptz.hori_as5047d.as5047d_speed_actual = (g_ptz.hori_as5047d.as5047d_speed_angle_c * 1000.0) / (360.0 * PTZ_HORI_PID_T) * 60.0; //根据磁编码器计算电机转速 g_ptz.hori_motor_speed_as5047d_actual = g_ptz.hori_as5047d.as5047d_speed_actual * PTZ_HORI_MOTOR_RATIO; g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_as5047d_actual; //根据磁编码器计算云台转速 g_ptz.hori_speed_as5047d_actual = g_ptz.hori_as5047d.as5047d_speed_actual / PTZ_HORI_BIG_GEAR_RATIO; g_ptz.hori_speed_actual = g_ptz.hori_speed_as5047d_actual; #endif #ifdef PTZ_HORI_PID_as5047d_ANGLE_SYN_SPEED //磁编码器测速，数据清0 g_ptz.hori_as5047d.as5047d_speed_angle_a = 0; g_ptz.hori_as5047d.as5047d_speed_angle_b = 0; g_ptz.hori_as5047d.as5047d_speed_angle_c = 0; //磁编码器测速，测速起始角度 g_ptz.hori_as5047d.as5047d_speed_angle_a = (float)as5047d_hori_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual; //PID测速延时 OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T); //磁编码器测速，测速结束角度 g_ptz.hori_as5047d.as5047d_speed_angle_b = (float)as5047d_hori_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual; //根据磁编码器转动的方向 //由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态 //正转，云台角度增大，磁编码角度增大 if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_as5047d.as5047d_speed_angle_a <= g_ptz.hori_as5047d.as5047d_speed_angle_b) { g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_b - g_ptz.hori_as5047d.as5047d_speed_angle_a; } else { g_ptz.hori_as5047d.as5047d_speed_angle_c = (16384.0 - g_ptz.hori_as5047d.as5047d_speed_angle_a) + (g_ptz.hori_as5047d.as5047d_speed_angle_b - 0); } } //反转，云台角度减小，磁编码角度减小 if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_as5047d.as5047d_speed_angle_a >= g_ptz.hori_as5047d.as5047d_speed_angle_b) { g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_a - g_ptz.hori_as5047d.as5047d_speed_angle_b; } else { g_ptz.hori_as5047d.as5047d_speed_angle_c = (16384.0 - g_ptz.hori_as5047d.as5047d_speed_angle_b) + (g_ptz.hori_as5047d.as5047d_speed_angle_a - 0); } } //去除转角异常情况 if(g_ptz.hori_as5047d.as5047d_speed_angle_c >= 3000.0 || g_ptz.hori_as5047d.as5047d_speed_angle_c < 0) { g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_c_last; } g_ptz.hori_as5047d.as5047d_speed_angle_c_last = g_ptz.hori_as5047d.as5047d_speed_angle_c; //计算磁编码器转速 g_ptz.hori_as5047d.as5047d_speed_actual = ((g_ptz.hori_as5047d.as5047d_speed_angle_c / 16384.0) * 1000.0) / (PTZ_HORI_PID_T) * 60.0; // g_ptz.hori_as5047d.as5047d_speed_actual = (g_ptz.hori_as5047d.as5047d_speed_angle_c * 1000.0) / // (360.0 * PTZ_HORI_PID_T) * 60.0; //根据磁编码器计算电机转速 g_ptz.hori_motor_speed_as5047d_actual = g_ptz.hori_as5047d.as5047d_speed_actual * PTZ_HORI_MOTOR_RATIO; g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_as5047d_actual; //根据磁编码器计算云台转速 g_ptz.hori_speed_as5047d_actual = g_ptz.hori_as5047d.as5047d_speed_actual / PTZ_HORI_BIG_GEAR_RATIO; g_ptz.hori_speed_actual = g_ptz.hori_speed_as5047d_actual; #endif #ifdef PTZ_HORI_PID_JY02A_SPEED //电机驱动芯片测速,数据清0 g_ptz.hori_jy02a.jy02a_fg_num_last = g_ptz.hori_jy02a.jy02a_fg_num_actual; g_ptz.hori_jy02a.jy02a_fg_num_actual = 0; g_ptz.hori_motor_speed_actual = 0; //PID测速延时 OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T); //根据电机驱动芯片计算电机实际转速 g_ptz.hori_motor_speed_jy02a_actual = g_ptz.hori_jy02a.jy02a_fg_num_actual / PTZ_HORI_FG_PWM_NUM_CYCLE * 1000.0 * 60 / PTZ_HORI_PID_T; g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_jy02a_actual; //根据电机驱动芯片计算云台转速 g_ptz.hori_speed_jy02a_actual = g_ptz.hori_motor_speed_jy02a_actual / PTZ_HORI_RATIO; g_ptz.hori_speed_actual = g_ptz.hori_speed_jy02a_actual; #endif // #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 // OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T); #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; g_ptz.hori_pid.hall_h123_count = 0; g_ptz.hori_pid.hall_h1_count_last = 0; g_ptz.hori_pid.hall_h1_count_time = 0; g_ptz.hori_pid.hall_h1_count_time_s = 0; g_ptz.hori_pid.hall_h1_count_time_e = 0; g_ptz.hori_pid.hall_h2_count_last = 0; g_ptz.hori_pid.hall_h2_count_time = 0; g_ptz.hori_pid.hall_h2_count_time_s = 0; g_ptz.hori_pid.hall_h2_count_time_e = 0; g_ptz.hori_pid.hall_h3_count_last = 0; g_ptz.hori_pid.hall_h3_count_time = 0; g_ptz.hori_pid.hall_h3_count_time_s = 0; g_ptz.hori_pid.hall_h3_count_time_e = 0; g_ptz.hori_pid.hall_h123_count_last = 0; g_ptz.hori_pid.hall_h123_count_time = 0; g_ptz.hori_pid.hall_h123_count_time_s = 0; g_ptz.hori_pid.hall_h123_count_time_e = 0; g_ptz.hori_pid.hall_h1_start_flag = 0; g_ptz.hori_pid.hall_h2_start_flag = 0; g_ptz.hori_pid.hall_h3_start_flag = 0; g_ptz.hori_pid.hall_h123_start_flag = 0; /* for(time = 0; time < PTZ_HORI_PID_T_MAX; time ++) { if(g_ptz.hori_pid.hall_h1_count != g_ptz.hori_pid.hall_h1_count_last) { if(g_ptz.hori_pid.hall_h1_start_flag == 0) { g_ptz.hori_pid.hall_h1_count_last = g_ptz.hori_pid.hall_h1_count; g_ptz.hori_pid.hall_h1_count_time_s = time; g_ptz.hori_pid.hall_h1_start_flag = 1; } else { g_ptz.hori_pid.hall_h1_count_last = g_ptz.hori_pid.hall_h1_count; g_ptz.hori_pid.hall_h1_count_time_e = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.hori_pid.hall_h123_start_flag == 0) { g_ptz.hori_pid.hall_h123_count_time_s = time; g_ptz.hori_pid.hall_h123_start_flag = 1; } else { g_ptz.hori_pid.hall_h123_count_time_e = time; } } if(g_ptz.hori_pid.hall_h2_count != g_ptz.hori_pid.hall_h2_count_last) { if(g_ptz.hori_pid.hall_h2_start_flag == 0) { g_ptz.hori_pid.hall_h2_count_last = g_ptz.hori_pid.hall_h2_count; g_ptz.hori_pid.hall_h2_count_time_s = time; g_ptz.hori_pid.hall_h2_start_flag = 1; } else { g_ptz.hori_pid.hall_h2_count_last = g_ptz.hori_pid.hall_h2_count; g_ptz.hori_pid.hall_h2_count_time_e = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.hori_pid.hall_h123_start_flag == 0) { g_ptz.hori_pid.hall_h123_count_time_s = time; g_ptz.hori_pid.hall_h123_start_flag = 1; } else { g_ptz.hori_pid.hall_h123_count_time_e = time; } } if(g_ptz.hori_pid.hall_h3_count != g_ptz.hori_pid.hall_h3_count_last) { if(g_ptz.hori_pid.hall_h3_start_flag == 0) { g_ptz.hori_pid.hall_h3_count_last = g_ptz.hori_pid.hall_h3_count; g_ptz.hori_pid.hall_h3_count_time_s = time; g_ptz.hori_pid.hall_h3_start_flag = 1; } else { g_ptz.hori_pid.hall_h3_count_last = g_ptz.hori_pid.hall_h3_count; g_ptz.hori_pid.hall_h3_count_time_e = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.hori_pid.hall_h123_start_flag == 0) { g_ptz.hori_pid.hall_h123_count_time_s = time; g_ptz.hori_pid.hall_h123_start_flag = 1; } else { g_ptz.hori_pid.hall_h123_count_time_e = time; } } if(time >= PTZ_HORI_PID_T)//到达默认采集周期 { if(g_ptz.hori_pid.hall_h1_count >= 2 && g_ptz.hori_pid.hall_h2_count >= 2 && g_ptz.hori_pid.hall_h3_count >= 2) { break; } } OSTimeDlyHMSM(0u, 0u, 0u, 1u); } //计算单独时间 if(g_ptz.hori_pid.hall_h1_count_time_e > g_ptz.hori_pid.hall_h1_count_time_s) { g_ptz.hori_pid.hall_h1_count_time = g_ptz.hori_pid.hall_h1_count_time_e - g_ptz.hori_pid.hall_h1_count_time_s; } else { g_ptz.hori_pid.hall_h1_count_time = time; } if(g_ptz.hori_pid.hall_h2_count_time_e > g_ptz.hori_pid.hall_h2_count_time_s) { g_ptz.hori_pid.hall_h2_count_time = g_ptz.hori_pid.hall_h2_count_time_e - g_ptz.hori_pid.hall_h2_count_time_s; } else { g_ptz.hori_pid.hall_h2_count_time = time; } if(g_ptz.hori_pid.hall_h3_count_time_e > g_ptz.hori_pid.hall_h3_count_time_s) { g_ptz.hori_pid.hall_h3_count_time = g_ptz.hori_pid.hall_h3_count_time_e - g_ptz.hori_pid.hall_h3_count_time_s; } else { g_ptz.hori_pid.hall_h3_count_time = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.hori_pid.hall_h123_count_time_e > g_ptz.hori_pid.hall_h123_count_time_s) { g_ptz.hori_pid.hall_h123_count_time = g_ptz.hori_pid.hall_h123_count_time_e - g_ptz.hori_pid.hall_h123_count_time_s; } else { g_ptz.hori_pid.hall_h123_count_time = time; } //根据三个霍尔的反馈情况分别计算转速 i = 0; if(g_ptz.hori_pid.hall_h1_count >= 2 && g_ptz.hori_pid.hall_h1_count_time > 0) { g_ptz.hori_pid.hall_h1_motor_speed = 60000.0 * (float)(g_ptz.hori_pid.hall_h1_count - 1) / (float)g_ptz.hori_pid.hall_h1_count_time / PTZ_HORI_MOTOR_POLE_PAIRS; i++; } else { g_ptz.hori_pid.hall_h1_motor_speed = 0; } if(g_ptz.hori_pid.hall_h2_count >= 2 && g_ptz.hori_pid.hall_h2_count_time > 0) { g_ptz.hori_pid.hall_h2_motor_speed = 60000.0 * (float)(g_ptz.hori_pid.hall_h2_count - 1) / (float)g_ptz.hori_pid.hall_h2_count_time / PTZ_HORI_MOTOR_POLE_PAIRS; i++; } else { g_ptz.hori_pid.hall_h2_motor_speed = 0; } if(g_ptz.hori_pid.hall_h3_count >= 2 && g_ptz.hori_pid.hall_h3_count_time > 0) { g_ptz.hori_pid.hall_h3_motor_speed = 60000.0 * (float)(g_ptz.hori_pid.hall_h3_count - 1) / (float)g_ptz.hori_pid.hall_h3_count_time / PTZ_HORI_MOTOR_POLE_PAIRS; i++; } else { g_ptz.hori_pid.hall_h3_motor_speed = 0; } g_ptz.hori_pid.hall_h123_count = g_ptz.hori_pid.hall_h1_count + g_ptz.hori_pid.hall_h2_count + g_ptz.hori_pid.hall_h3_count; //根据霍尔总脉冲计算电机转速 if(g_ptz.hori_pid.hall_h123_count >= 2 && g_ptz.hori_pid.hall_h123_count_time > 0) { g_ptz.hori_pid.hall_h123_motor_speed = 60000.0 * (float)(g_ptz.hori_pid.hall_h123_count - 1) / (float)g_ptz.hori_pid.hall_h123_count_time / PTZ_HORI_ONE_CYCLE_HALL_NUM; i++; } else { g_ptz.hori_pid.hall_h123_motor_speed = 0; } //计算电机平均转速 if(i > 0) { g_ptz.hori_motor_speed_hall_actual = (g_ptz.hori_pid.hall_h1_motor_speed + g_ptz.hori_pid.hall_h2_motor_speed + g_ptz.hori_pid.hall_h3_motor_speed + g_ptz.hori_pid.hall_h123_motor_speed) / i; } else { g_ptz.hori_motor_speed_hall_actual = 0; } */ /* ********************************** */ OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T); g_ptz.hori_pid.hall_h123_count = g_ptz.hori_pid.hall_h1_count + g_ptz.hori_pid.hall_h2_count + g_ptz.hori_pid.hall_h3_count; if(g_ptz.hori_pid.hall_h123_count >= 2) { g_ptz.hori_pid.hall_h123_motor_speed = 60000.0 * (float)(g_ptz.hori_pid.hall_h123_count/* - 1*/) / (float)PTZ_HORI_PID_T / PTZ_HORI_ONE_CYCLE_HALL_NUM; } static volatile uint32_t lastTime = 0; static volatile uint32_t Time = 0; Time = OSTimeGet(); hori_speed_task_time = Time - lastTime; lastTime = Time; g_ptz.hori_motor_speed_hall_actual = g_ptz.hori_pid.hall_h123_motor_speed; /* ********************************** */ //判断电机转动是否抖动 if(g_ptz.hori_pid.hall_h1_count == 0 || g_ptz.hori_pid.hall_h2_count == 0 || g_ptz.hori_pid.hall_h3_count == 0) {//电机在抖动，速度直接置0 g_ptz.hori_motor_speed_hall_actual = 0; } g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_hall_actual; //根据电机转速计算云台转速 g_ptz.hori_speed_hall_actual = g_ptz.hori_motor_speed_hall_actual / PTZ_HORI_RATIO; g_ptz.hori_speed_actual = g_ptz.hori_speed_hall_actual; #endif /*由于系统实时调节能力不足将调速分为两个部分： 1.启动时首先进入直线比例快速提速； 2.速度达到设定速度的60％后转换为PID调速。*/ // if(g_ptz.hori_speed_actual < g_ptz.hori_speed_set * PTZ_HORI_PRO_ADD_RANGE_LESS) // { //// if(time > PTZ_HORI_PID_ADD) //// { //// time = PTZ_HORI_PID_ADD; //// } //// g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float + time/*PTZ_HORI_PID_ADD*/; //// //存储误差，用于PID计算 //// g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; //// g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual; // // g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * PTZ_HORI_PRO_ADD_MUL_LESS;//* 1.3/*PTZ_HORI_PID_ADD*/; // if(g_ptz.hori_pid.PidUT_float < PTZ_HORI_PID_ADD) // { // g_ptz.hori_pid.PidUT_float = PTZ_HORI_PID_ADD; // } // //存储误差，用于PID计算 // g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; // g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual; // /**考虑超调时的直线降速，否则超调后降速会很慢，可以比列减速**/ // } // else /* 霍尔计算速度 */ #if 1 g_speed_to_hall.hori_speed_t.flag = calculation_enable; if (g_ptz.hori_speed_actual == 0) { h_pwm_duty_change(220); } 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); } if (g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { continue; } OSTimeDlyHMSM(0u, 0u, 0u, 50u); 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); } if (g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { continue; } uint32_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 / 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;////新增直线减速 g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual; } else { //计算PID控制器输出值 g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual, time) + g_ptz.hori_pid.LastUT_float; //控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内 //即防止PID增量过大 if(fabs(g_ptz.hori_pid.PidUT_float - g_ptz.hori_pid.LastUT_float) > PTZ_HORI_PID_OUTPUT_LIMIT)//限定PID输出限定 { if(g_ptz.hori_pid.PidUT_float > g_ptz.hori_pid.LastUT_float) { g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float + PTZ_HORI_PID_OUTPUT_LIMIT; } else { g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float - PTZ_HORI_PID_OUTPUT_LIMIT; } } } if(g_ptz.hori_pid.PidUT_float < PTZ_HORI_VR_MIN)//限制输入模拟电压最小值 { g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MIN; } if(g_ptz.hori_pid.PidUT_float > PTZ_HORI_VR_MAX)//限制输入模拟电压最大值 { g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MAX; } g_ptz.hori_pid.PidUT_uint = (unsigned int)(g_ptz.hori_pid.PidUT_float + 0.5); //限制PID的输出值在某个指定的范围 if(g_ptz.hori_pid.PidUT_uint <= PTZ_HORI_VR_MIN)//限制输入模拟电压最小值 { g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MIN; } if(g_ptz.hori_pid.PidUT_uint > PTZ_HORI_VR_MAX)//限制输入模拟电压最大值 { g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MAX; } //将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(500); //将当前PID输出值保存 g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; } if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { g_ptz.hori_speed_actual = 0; OSTimeDlyHMSM(0u, 0u, 0u, 10u); } #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 //防止数据超限溢出 if(g_ptz.hori_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE) { g_ptz.hori_pid.hall_h1_count = 0; } if(g_ptz.hori_pid.hall_h2_count >= UNSIGNED_INT_MAX_VALUE) { g_ptz.hori_pid.hall_h2_count = 0; } if(g_ptz.hori_pid.hall_h3_count >= UNSIGNED_INT_MAX_VALUE) { g_ptz.hori_pid.hall_h3_count = 0; } #endif } } static OS_STK task_hori_pid_stk[TASK_HORI_PID_STK_SIZE]; static void creat_task_hori_pid(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_pid_task, (void *) 0, (OS_STK *)&task_hori_pid_stk[TASK_HORI_PID_STK_SIZE - 1], (INT8U ) TASK_HORI_PID_PRIO, (INT16U ) TASK_HORI_PID_PRIO, (OS_STK *)&task_hori_pid_stk[0], (INT32U ) TASK_HORI_PID_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_pid_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_pid_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_pid_task failed...\n\r"); } } /****************************************************************************/ void ptz_vert_pid_clear_zero() { g_ptz.vert_pid.SumError = 0;//累计误差 g_ptz.vert_pid.LastError = 0;//上一次输入偏差 g_ptz.vert_pid.PrevError = 0;//上上次输入偏差 g_ptz.vert_pid.LastUT_float = 0;//上次PID输出值u(t) g_ptz.vert_pid.PidUT_float = 0;//PID当前输出值 g_ptz.vert_pid.PidUT_uint = 0; g_ptz.vert_pid.mode = PID_ADJUST_SPEED;//首先默认PID调速 g_ptz.vert_pid.director_speed_state = 0; // vert_dac1_data_out(0); } static float ptz_vert_pid_calculate(float SampSpeed) { float IError,IIncPid; IError = g_ptz.vert_speed_set - SampSpeed;//当前误差PID_INPUT_LIMIT /*控制PID输入参数*/ // if(IError > PTZ_VERT_PID_INPUT_LIMIT) // { // IError = PTZ_VERT_PID_INPUT_LIMIT; // } // if(IError < (-1 * PTZ_VERT_PID_INPUT_LIMIT)) // { // IError = PTZ_VERT_PID_INPUT_LIMIT * -1; // } //增量计算 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_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; g_ptz.vert_pid.LastError = IError; //返回增量值 return IIncPid; } //新调速方式 static void ptz_vert_pid_task() { unsigned int time = 0; char i = 0; while(1) { //计算转速 if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { #ifdef PTZ_VERT_PID_as5047d_ANGLE_ASY_SPEED //磁编码器测速，数据清0 g_ptz.vert_as5047d.as5047d_speed_angle_a = 0; g_ptz.vert_as5047d.as5047d_speed_angle_b = 0; g_ptz.vert_as5047d.as5047d_speed_angle_c = 0; //磁编码器测速，测速起始角度 g_ptz.vert_as5047d.as5047d_speed_angle_a = (float)as5047d_vert_read_data_a();//as5047d_vert_get_angle_a();//g_ptz.vert_as5047d.as5047d_angle_actual; //PID测速延时 OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T); //磁编码器测速，测速结束角度 g_ptz.vert_as5047d.as5047d_speed_angle_b = (float)as5047d_vert_read_data_a();//as5047d_vert_get_angle_a();//g_ptz.vert_as5047d.as5047d_angle_actual; //根据磁编码器转动的方向 //由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态 //反转，云台角度减小，磁编码角度增大 if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN) { if(g_ptz.vert_as5047d.as5047d_speed_angle_a <= g_ptz.vert_as5047d.as5047d_speed_angle_b) { g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_b - g_ptz.vert_as5047d.as5047d_speed_angle_a; } else { g_ptz.vert_as5047d.as5047d_speed_angle_c = (16384.0 - g_ptz.vert_as5047d.as5047d_speed_angle_a) + //360.0 (g_ptz.vert_as5047d.as5047d_speed_angle_b - 0); } } //正转，云台角度增大，磁编码角度减小 if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP) { if(g_ptz.vert_as5047d.as5047d_speed_angle_a >= g_ptz.vert_as5047d.as5047d_speed_angle_b) { g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_a - g_ptz.vert_as5047d.as5047d_speed_angle_b; } else { g_ptz.vert_as5047d.as5047d_speed_angle_c = (16384.0 - g_ptz.vert_as5047d.as5047d_speed_angle_b) + //360.0 (g_ptz.vert_as5047d.as5047d_speed_angle_a - 0); } } //去除转角异常情况 if(g_ptz.vert_as5047d.as5047d_speed_angle_c < 0 || g_ptz.vert_as5047d.as5047d_speed_angle_c >= 3000.0)//50.0 { g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_c_last; } g_ptz.vert_as5047d.as5047d_speed_angle_c_last = g_ptz.vert_as5047d.as5047d_speed_angle_c; //计算磁编码器转速 g_ptz.vert_as5047d.as5047d_speed_actual = ((g_ptz.vert_as5047d.as5047d_speed_angle_c / 16384.0) * 1000) / PTZ_VERT_PID_T * 60.0; // g_ptz.vert_as5047d.as5047d_speed_actual = (g_ptz.vert_as5047d.as5047d_speed_angle_c * 1000) / // (360.0 * PTZ_VERT_PID_T) * 60.0; //根据磁编码器计算电机转速 g_ptz.vert_motor_speed_as5047d_actual = g_ptz.vert_as5047d.as5047d_speed_actual * PTZ_VERT_MOTOR_RATIO; g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_as5047d_actual; //根据磁编码器计算云台转速 g_ptz.vert_speed_as5047d_actual = g_ptz.vert_as5047d.as5047d_speed_actual / PTZ_VERT_BIG_GEAR_RATIO; g_ptz.vert_speed_actual = g_ptz.vert_speed_as5047d_actual; #endif #ifdef PTZ_VERT_PID_as5047d_ANGLE_SYN_SPEED //磁编码器测速，数据清0 g_ptz.vert_as5047d.as5047d_speed_angle_a = 0; g_ptz.vert_as5047d.as5047d_speed_angle_b = 0; g_ptz.vert_as5047d.as5047d_speed_angle_c = 0; //磁编码器测速，测速起始角度 g_ptz.vert_as5047d.as5047d_speed_angle_a = g_ptz.vert_as5047d.as5047d_angle_actual; //PID测速延时 OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T); //磁编码器测速，测速结束角度 g_ptz.vert_as5047d.as5047d_speed_angle_b = g_ptz.vert_as5047d.as5047d_angle_actual; //根据磁编码器转动的方向 //由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态 //正转，云台角度增大，磁编码角度增大 if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP) { if(g_ptz.vert_as5047d.as5047d_speed_angle_a <= g_ptz.vert_as5047d.as5047d_speed_angle_b) { g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_b - g_ptz.vert_as5047d.as5047d_speed_angle_a; } else { g_ptz.vert_as5047d.as5047d_speed_angle_c = (360.0 - g_ptz.vert_as5047d.as5047d_speed_angle_a) + (g_ptz.vert_as5047d.as5047d_speed_angle_b - 0); } } //反转，云台角度减小，磁编码角度减小 if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN) { if(g_ptz.vert_as5047d.as5047d_speed_angle_a >= g_ptz.vert_as5047d.as5047d_speed_angle_b) { g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_a - g_ptz.vert_as5047d.as5047d_speed_angle_b; } else { g_ptz.vert_as5047d.as5047d_speed_angle_c = (360.0 - g_ptz.vert_as5047d.as5047d_speed_angle_b) + (g_ptz.vert_as5047d.as5047d_speed_angle_a - 0); } } //去除转角异常情况 if(g_ptz.vert_as5047d.as5047d_speed_angle_c < 0 || g_ptz.vert_as5047d.as5047d_speed_angle_c >= 50.0) { g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_c_last; } g_ptz.vert_as5047d.as5047d_speed_angle_c_last = g_ptz.vert_as5047d.as5047d_speed_angle_c; //计算磁编码器转速 g_ptz.vert_as5047d.as5047d_speed_actual = (g_ptz.vert_as5047d.as5047d_speed_angle_c * 1000) / (360.0 * PTZ_VERT_PID_T) * 60.0; //根据磁编码器计算电机转速 g_ptz.vert_motor_speed_as5047d_actual = g_ptz.vert_as5047d.as5047d_speed_actual * PTZ_VERT_MOTOR_RATIO; g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_as5047d_actual; //根据磁编码器计算云台转速 g_ptz.vert_speed_as5047d_actual = g_ptz.vert_as5047d.as5047d_speed_actual / PTZ_VERT_BIG_GEAR_RATIO; g_ptz.vert_speed_actual = g_ptz.vert_speed_as5047d_actual; #endif #ifdef PTZ_VERT_PID_JY02A_SPEED //电机驱动芯片测速，数据清0 g_ptz.vert_jy02a.jy02a_fg_num_last = g_ptz.vert_jy02a.jy02a_fg_num_actual; g_ptz.vert_jy02a.jy02a_fg_num_actual = 0; g_ptz.vert_motor_speed_actual = 0; //PID测速延时 OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T); //根据电机驱动芯片计算电机实际转速 g_ptz.vert_motor_speed_jy02a_actual = g_ptz.vert_jy02a.jy02a_fg_num_actual / PTZ_VERT_FG_PWM_NUM_CYCLE * 1000.0 * 60 / PTZ_VERT_PID_T; g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_jy02a_actual; //根据电机驱动芯片计算云台转速 g_ptz.vert_speed_jy02a_actual = g_ptz.vert_motor_speed_jy02a_actual / PTZ_VERT_RATIO; g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual; #endif #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 g_ptz.vert_pid.hall_h1_count = 0; g_ptz.vert_pid.hall_h2_count = 0; g_ptz.vert_pid.hall_h3_count = 0; g_ptz.vert_pid.hall_h123_count = 0; g_ptz.vert_pid.hall_h1_count_last = 0; g_ptz.vert_pid.hall_h1_count_time = 0; g_ptz.vert_pid.hall_h1_count_time_s = 0; g_ptz.vert_pid.hall_h1_count_time_e = 0; g_ptz.vert_pid.hall_h2_count_last = 0; g_ptz.vert_pid.hall_h2_count_time = 0; g_ptz.vert_pid.hall_h2_count_time_s = 0; g_ptz.vert_pid.hall_h2_count_time_e = 0; g_ptz.vert_pid.hall_h3_count_last = 0; g_ptz.vert_pid.hall_h3_count_time = 0; g_ptz.vert_pid.hall_h3_count_time_s = 0; g_ptz.vert_pid.hall_h3_count_time_e = 0; g_ptz.vert_pid.hall_h123_count_last = 0; g_ptz.vert_pid.hall_h123_count_time = 0; g_ptz.vert_pid.hall_h123_count_time_s = 0; g_ptz.vert_pid.hall_h123_count_time_e = 0; g_ptz.vert_pid.hall_h1_start_flag = 0; g_ptz.vert_pid.hall_h2_start_flag = 0; g_ptz.vert_pid.hall_h3_start_flag = 0; g_ptz.vert_pid.hall_h123_start_flag = 0; /* for(time = 0; time < PTZ_VERT_PID_T_MAX; time ++) { if(g_ptz.vert_pid.hall_h1_count != g_ptz.vert_pid.hall_h1_count_last) { if(g_ptz.vert_pid.hall_h1_start_flag == 0) { g_ptz.vert_pid.hall_h1_count_last = g_ptz.vert_pid.hall_h1_count; g_ptz.vert_pid.hall_h1_count_time_s = time; g_ptz.vert_pid.hall_h1_start_flag = 1; } else { g_ptz.vert_pid.hall_h1_count_last = g_ptz.vert_pid.hall_h1_count; g_ptz.vert_pid.hall_h1_count_time_e = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.vert_pid.hall_h123_start_flag == 0) { g_ptz.vert_pid.hall_h123_count_time_s = time; g_ptz.vert_pid.hall_h123_start_flag = 1; } else { g_ptz.vert_pid.hall_h123_count_time_e = time; } } if(g_ptz.vert_pid.hall_h2_count != g_ptz.vert_pid.hall_h2_count_last) { if(g_ptz.vert_pid.hall_h2_start_flag == 0) { g_ptz.vert_pid.hall_h2_count_last = g_ptz.vert_pid.hall_h2_count; g_ptz.vert_pid.hall_h2_count_time_s = time; g_ptz.vert_pid.hall_h2_start_flag = 1; } else { g_ptz.vert_pid.hall_h2_count_last = g_ptz.vert_pid.hall_h2_count; g_ptz.vert_pid.hall_h2_count_time_e = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.vert_pid.hall_h123_start_flag == 0) { g_ptz.vert_pid.hall_h123_count_time_s = time; g_ptz.vert_pid.hall_h123_start_flag = 1; } else { g_ptz.vert_pid.hall_h123_count_time_e = time; } } if(g_ptz.vert_pid.hall_h3_count != g_ptz.vert_pid.hall_h3_count_last) { if(g_ptz.vert_pid.hall_h3_start_flag == 0) { g_ptz.vert_pid.hall_h3_count_last = g_ptz.vert_pid.hall_h3_count; g_ptz.vert_pid.hall_h3_count_time_s = time; g_ptz.vert_pid.hall_h3_start_flag = 1; } else { g_ptz.vert_pid.hall_h3_count_last = g_ptz.vert_pid.hall_h3_count; g_ptz.vert_pid.hall_h3_count_time_e = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.vert_pid.hall_h123_start_flag == 0) { g_ptz.vert_pid.hall_h123_count_time_s = time; g_ptz.vert_pid.hall_h123_start_flag = 1; } else { g_ptz.vert_pid.hall_h123_count_time_e = time; } } if(time >= PTZ_VERT_PID_T)//到达默认采集周期 { if(g_ptz.vert_pid.hall_h1_count >= 2 && g_ptz.vert_pid.hall_h2_count >= 2 && g_ptz.vert_pid.hall_h3_count >= 2) { break; } } OSTimeDlyHMSM(0u, 0u, 0u, 1u); } //计算时间 if(g_ptz.vert_pid.hall_h1_count_time_e > g_ptz.vert_pid.hall_h1_count_time_s) { g_ptz.vert_pid.hall_h1_count_time = g_ptz.vert_pid.hall_h1_count_time_e - g_ptz.vert_pid.hall_h1_count_time_s; } else { g_ptz.vert_pid.hall_h1_count_time = time; } if(g_ptz.vert_pid.hall_h2_count_time_e > g_ptz.vert_pid.hall_h2_count_time_s) { g_ptz.vert_pid.hall_h2_count_time = g_ptz.vert_pid.hall_h2_count_time_e - g_ptz.vert_pid.hall_h2_count_time_s; } else { g_ptz.vert_pid.hall_h2_count_time = time; } if(g_ptz.vert_pid.hall_h3_count_time_e > g_ptz.vert_pid.hall_h3_count_time_s) { g_ptz.vert_pid.hall_h3_count_time = g_ptz.vert_pid.hall_h3_count_time_e - g_ptz.vert_pid.hall_h3_count_time_s; } else { g_ptz.vert_pid.hall_h3_count_time = time; } //计算接收所有霍尔脉冲的时间 if(g_ptz.vert_pid.hall_h123_count_time_e > g_ptz.vert_pid.hall_h123_count_time_s) { g_ptz.vert_pid.hall_h123_count_time = g_ptz.vert_pid.hall_h123_count_time_e - g_ptz.vert_pid.hall_h123_count_time_s; } else { g_ptz.vert_pid.hall_h123_count_time = time; } //根据三个霍尔的反馈情况分别计算转速 i = 0; if(g_ptz.vert_pid.hall_h1_count >= 2 && g_ptz.vert_pid.hall_h1_count_time > 0) { g_ptz.vert_pid.hall_h1_motor_speed = 60000.0 * (float)(g_ptz.vert_pid.hall_h1_count - 1) / (float)g_ptz.vert_pid.hall_h1_count_time / PTZ_VERT_MOTOR_POLE_PAIRS; i++; } else { g_ptz.vert_pid.hall_h1_motor_speed = 0; } if(g_ptz.vert_pid.hall_h2_count >= 2 && g_ptz.vert_pid.hall_h2_count_time > 0) { g_ptz.vert_pid.hall_h2_motor_speed = 60000.0 * (float)(g_ptz.vert_pid.hall_h2_count - 1) / (float)g_ptz.vert_pid.hall_h2_count_time / PTZ_VERT_MOTOR_POLE_PAIRS; i++; } else { g_ptz.vert_pid.hall_h2_motor_speed = 0; } if(g_ptz.vert_pid.hall_h3_count >= 2 && g_ptz.vert_pid.hall_h3_count_time > 0) { g_ptz.vert_pid.hall_h3_motor_speed = 60000.0 * (float)(g_ptz.vert_pid.hall_h3_count - 1) / (float)g_ptz.vert_pid.hall_h3_count_time / PTZ_VERT_MOTOR_POLE_PAIRS; i++; } else { g_ptz.vert_pid.hall_h3_motor_speed = 0; } g_ptz.vert_pid.hall_h123_count = g_ptz.vert_pid.hall_h1_count + g_ptz.vert_pid.hall_h2_count + g_ptz.vert_pid.hall_h3_count; //根据霍尔总脉冲计算电机转速 if(g_ptz.vert_pid.hall_h123_count >= 2 && g_ptz.vert_pid.hall_h123_count_time > 0) { g_ptz.vert_pid.hall_h123_motor_speed = 60000.0 * (float)(g_ptz.vert_pid.hall_h123_count - 1) / (float)g_ptz.vert_pid.hall_h123_count_time / PTZ_VERT_ONE_CYCLE_HALL_NUM; i++; } else { g_ptz.vert_pid.hall_h123_motor_speed = 0; } //计算电机平均转速 if(i > 0) { g_ptz.vert_motor_speed_hall_actual = (g_ptz.vert_pid.hall_h1_motor_speed + g_ptz.vert_pid.hall_h2_motor_speed + g_ptz.vert_pid.hall_h3_motor_speed + g_ptz.vert_pid.hall_h123_motor_speed) / i; } else { g_ptz.vert_motor_speed_hall_actual = 0; } */ /* ********************************** */ OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T); g_ptz.vert_pid.hall_h123_count = g_ptz.vert_pid.hall_h1_count + g_ptz.vert_pid.hall_h2_count + g_ptz.vert_pid.hall_h3_count; if(g_ptz.vert_pid.hall_h123_count >= 2) { g_ptz.vert_pid.hall_h123_motor_speed = 60000.0 * (float)(g_ptz.vert_pid.hall_h123_count /*-1*/) / (float)PTZ_VERT_PID_T / PTZ_VERT_ONE_CYCLE_HALL_NUM; } g_ptz.vert_motor_speed_hall_actual = g_ptz.vert_pid.hall_h123_motor_speed; /* ********************************** */ //判断电机转动是否抖动 if(g_ptz.vert_pid.hall_h1_count == 0 || g_ptz.vert_pid.hall_h2_count == 0 || g_ptz.vert_pid.hall_h3_count == 0) {//电机在抖动，速度直接置0 g_ptz.vert_motor_speed_hall_actual = 0; } g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_hall_actual; //根据电机转速计算云台转速 g_ptz.vert_speed_hall_actual = g_ptz.vert_motor_speed_hall_actual / PTZ_VERT_RATIO; g_ptz.vert_speed_actual = g_ptz.vert_speed_hall_actual; #endif } //PID调速 if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_pid.mode == PID_ADJUST_SPEED) { #ifdef PTZ_CONTROL_SPEED_L6235D //判断是否需要启动方向调速 if(g_ptz.vert_pid.PidUT_float <= 0 && g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2) { v_pwm_duty_change(0); g_ptz.vert_pid.director_speed_state = 0; g_ptz.vert_pid.mode = DIRECTOR_ADJUST_SPEED; } #endif /*由于系统实时调节能力不足将调速分为两个部分： 1.启动时首先进入直线比例快速提速； 2.速度达到设定速度的70％后转换为PID调速。*/ // if(g_ptz.vert_speed_actual < g_ptz.vert_speed_set * PTZ_VERT_PRO_ADD_RANGE_LESS) // { //// if(time > PTZ_VERT_PID_ADD) //// { //// time = PTZ_VERT_PID_ADD; //// } //// g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float + time/*PTZ_VERT_PID_ADD*/; //// //存储误差，用于PID计算 //// g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; //// g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; // // g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float * PTZ_VERT_PRO_ADD_MUL_LESS; // if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_PID_ADD) // { // g_ptz.vert_pid.PidUT_float = PTZ_VERT_PID_ADD; // } // //存储误差，用于PID计算 // g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; // g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; // } // else if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2) {//速度大于设定速度的3倍 g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float - 100.0;//* 1.3/*PTZ_HORI_PID_ADD*/; g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; } else { //计算PID控制器输出值 g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float; //控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内 //即防止PID增量过大 if(fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT)//限定PID输出限定 { if(g_ptz.vert_pid.PidUT_float > g_ptz.vert_pid.LastUT_float) { g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float + PTZ_VERT_PID_OUTPUT_LIMIT; } else { g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float - PTZ_VERT_PID_OUTPUT_LIMIT; } } } if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_VR_MIN)//限制输入模拟电压最小值 { g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MIN; } if(g_ptz.vert_pid.PidUT_float > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值 { g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MAX; } g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5); //限制PID的输出值在某个指定的范围 if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//限制输入模拟电压最小值 { g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN; } if(g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值 { g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX; } //将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片 // vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint); v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint); //将当前PID输出值保存 g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float; } #ifdef PTZ_CONTROL_SPEED_L6235D //方向pwm波反向力矩调速 if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_pid.mode == DIRECTOR_ADJUST_SPEED) { /******/ //判断是否需要恢复PID调速 if(g_ptz.vert_pid.director_speed_start_t <= 0 && g_ptz.vert_speed_actual < (g_ptz.vert_speed_set / 2.0)) { vert_dir_speed_stop(); // adc7311_vert_vr(0); g_ptz.vert_pid.director_speed_state = 0; g_ptz.vert_pid.SumError = 0;//累计误差 g_ptz.vert_pid.LastError = 0;//上一次输入偏差 g_ptz.vert_pid.PrevError = 0;//上上次输入偏差 g_ptz.vert_pid.LastUT_float = 0;//上次PID输出值u(t) g_ptz.vert_pid.PidUT_float = 0;//PID当前输出值 // l6235d_vert_set_direction(g_ptz.vert_direction_set); g_ptz.vert_pid.mode = PID_ADJUST_SPEED; continue; } /*****/ switch(g_ptz.vert_pid.director_speed_state) { case 0://方向调速初始化 if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN) { g_ptz.vert_pid.director_speed_direction = PTZ_VERT_DIR_UP; } if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP) { g_ptz.vert_pid.director_speed_direction = PTZ_VERT_DIR_DOWN; } g_ptz.vert_pid.director_speed_t = PTZ_DIRECTOR_SPEED_T;//20ms // g_ptz.vert_pid.director_speed_vref = 0; // adc7311_vert_vr(g_ptz.vert_pid.director_speed_vref); g_ptz.vert_pid.director_speed_start_t = 0; g_ptz.vert_pid.director_speed_stop_t = PTZ_DIRECTOR_SPEED_T - g_ptz.vert_pid.director_speed_start_t; g_ptz.vert_pid.director_speed_state ++; break; case 1://调节方向占空比 if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set && g_ptz.vert_pid.director_speed_start_t < g_ptz.vert_pid.director_speed_t) { g_ptz.vert_pid.director_speed_start_t ++; } if(g_ptz.vert_speed_actual < g_ptz.vert_speed_set && g_ptz.vert_pid.director_speed_start_t > 0) { g_ptz.vert_pid.director_speed_start_t --; } g_ptz.vert_pid.director_speed_stop_t = g_ptz.vert_pid.director_speed_t - g_ptz.vert_pid.director_speed_start_t; break; } } #endif if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) { g_ptz.vert_pid.roll_start = 0; g_ptz.vert_speed_actual = 0; OSTimeDlyHMSM(0u, 0u, 0u, 10u); } #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 //防止数据超限溢出 if(g_ptz.vert_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE) { g_ptz.vert_pid.hall_h1_count = 0; } if(g_ptz.vert_pid.hall_h2_count >= UNSIGNED_INT_MAX_VALUE) { g_ptz.vert_pid.hall_h2_count = 0; } if(g_ptz.vert_pid.hall_h3_count >= UNSIGNED_INT_MAX_VALUE) { g_ptz.vert_pid.hall_h3_count = 0; } #endif } } //方向调速任务,方向PWM波产生任务,,,给定电机一个保持力矩，使电机自锁，达到减速目的 static void ptz_vert_director_speed_pwm_task() { while(1) {//方向pwm波 if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_pid.mode == DIRECTOR_ADJUST_SPEED) { switch(g_ptz.vert_pid.director_speed_state) { case 1: if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_pid.director_speed_start_t > 0) {//减速 // l6235d_vert_set_direction(g_ptz.vert_pid.director_speed_direction); vert_dir_speed_start(80); OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_pid.director_speed_start_t); } if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_pid.director_speed_stop_t > 0) { // l6235d_vert_set_direction(g_ptz.vert_direction_actual); vert_dir_speed_stop(); OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_pid.director_speed_stop_t); } break; default: OSTimeDlyHMSM(0u, 0u, 0u, 10u); break; } } else { OSTimeDlyHMSM(0u, 0u, 0u, 10u); } } } static OS_STK task_vert_pid_stk[TASK_VERT_PID_STK_SIZE]; static void creat_task_vert_pid(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_pid_task, (void *) 0, (OS_STK *)&task_vert_pid_stk[TASK_VERT_PID_STK_SIZE - 1], (INT8U ) TASK_VERT_PID_PRIO, (INT16U ) TASK_VERT_PID_PRIO, (OS_STK *)&task_vert_pid_stk[0], (INT32U ) TASK_VERT_PID_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_pid_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_pid_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_pid_task failed...\n\r"); } } static OS_STK task_vert_director_speed_pwm_stk[TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE]; static void creat_task_director_speed_pwm(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_director_speed_pwm_task, (void *) 0, (OS_STK *)&task_vert_director_speed_pwm_stk[TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE - 1], (INT8U ) TASK_VERT_DIRECTOR_SPEED_PWM_PRIO, (INT16U ) TASK_VERT_DIRECTOR_SPEED_PWM_PRIO, (OS_STK *)&task_vert_director_speed_pwm_stk[0], (INT32U ) TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_DIRECTOR_SPEED_PWM_PRIO, "ptz_vert_director_speed_pwm_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_director_speed_pwm_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_director_speed_pwm_task failed...\n\r"); } } /******************************************************************************/ void init_speed_module(void) { bldc_pwm_init();//初始化定时器部分 ptz_pid_init(); creat_task_hori_pid(); creat_task_vert_pid(); #ifdef PTZ_CONTROL_SPEED_L6235D creat_task_director_speed_pwm(); #endif } #endif static volatile float angleDifference = 0; void as5047d_hori_get_speed(void) { #if 0 static float lastAngle = 0; // static uint32_t lastTime = 0; static float angle = 0; // static uint32_t time = 0; static int num = 0; if (num++ > 100) { if (g_ptz.hori_start_stop_set == PTZ_HORI_START) { // time = OSTimeGet(); // angle = as5047d_hori_get_angle(); for(int m = 0; m < 5; m++) { angle = as5047d_hori_get_angle_a(); if(angle < 0 || //防止读出的角度为负值 angle > 360.0 ||//防止读出的数值超过360 isnan(angle) == 1 ||//防止读出的不是一个数 ((fabs(angle - lastAngle) > PTZ_HORI_ANGLE_DIFF_A) && (fabs(angle - lastAngle) < PTZ_HORI_ANGLE_DIFF_B))) {//以上错误的数据都该舍弃,从新读取 asm("nop");asm("nop");asm("nop");asm("nop");asm("nop"); } else { break; } } if (angle < 0 || angle > 360.0 || isnan(angle)) { return; } angleDifference = fabs(angle - lastAngle); if (angleDifference > 180) { angleDifference = 360 - angleDifference; } // g_ptz.hori_speed_actual = angleDifference / (time - lastTime) * 1000.0; g_ptz.hori_speed_actual = angleDifference * 0.03082; lastAngle = angle; } } #endif }