#include "ptz_header_file.h" #include "Timer.h" #include "ptz_default_value.h" #include "ptz_type_select.h" #include "rotate_step.h" #include "speed_to_step.h" #ifdef PTZ_STEP_MOTOR static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁 static BSP_OS_SEM ptz_vert_stop_mutex; static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时 static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时 void ptz_sem_post_stop_mutex()//释放云台启停共享资源锁 { BSP_OS_SemPost(&ptz_hori_stop_mutex); BSP_OS_SemPost(&ptz_vert_stop_mutex); } //转速r/min转化为步进电机步进频率,step 细分程度1,2,4,8,16,32,64,128,256 float ptz_hori_step_v_to_f(float v, unsigned short int microstep_alue) { float f; f = v * 6.0 * (float)microstep_alue / PTZ_HORI_MOTOR_STEP; return f; } //步进电机步进频率转化为转速r/min float ptz_hori_step_f_to_v(float f, unsigned short int microstep_alue) { float v; v = f * PTZ_HORI_MOTOR_STEP / ((float)microstep_alue * 6.0); return v; } //转速r/min转化为步进电机步进频率,step 细分程度1,2,4,8,16,32,64,128,256 float ptz_vert_step_v_to_f(float v, unsigned short int microstep_alue) { float f; f = v * 6.0 * (float)microstep_alue / PTZ_VERT_MOTOR_STEP; return f; } //步进电机步进频率转化为转速r/min float ptz_vert_step_f_to_v(float f, unsigned short int microstep_alue) { float v; v = f * PTZ_VERT_MOTOR_STEP / ((float)microstep_alue * 6.0); return v; } #ifdef TMC2160 extern TMC2160Parameter tmc2160_parameter; float ptz_hori_break_angle() { float s,s1,s2,v2; s1 = 3.0 * PTZ_HORI_BREAK_SPEED * PTZ_HORI_BREAK_SPEED * (float)tmc2160_parameter.hori_dec_time_max / PTZ_HORI_MAX_SPEED / 1000.0; v2 = ptz_hori_step_f_to_v(/*g_ptz.hori_drv8711.set_f*/g_ptz.hori_tmc2160.f, g_ptz.hori_tmc2160.microstep_vlue) / PTZ_HORI_RATIO; s2 = 3.0 * v2 * v2 * (float)tmc2160_parameter.hori_dec_time_max / PTZ_HORI_MAX_SPEED / 1000.0; s = s2 -s1; g_ptz.hori_tmc2160.stop_angle = s; return s; } float ptz_vert_break_angle() { float s,s1,s2,v2; s1 = 3.0 * PTZ_VERT_BREAK_SPEED * PTZ_VERT_BREAK_SPEED * (float)tmc2160_parameter.vert_dec_time_max / PTZ_VERT_MAX_SPEED / 1000.0; v2 = ptz_vert_step_f_to_v(/*g_ptz.vert_drv8711.set_f*/g_ptz.vert_tmc2160.f, g_ptz.vert_tmc2160.microstep_vlue) / PTZ_VERT_RATIO; s2 = 3.0 * v2 * v2 * (float)tmc2160_parameter.vert_dec_time_max / PTZ_VERT_MAX_SPEED / 1000.0; s = s2 -s1; g_ptz.vert_tmc2160.stop_angle = s; return s; } //根据转速判断采用的细分程度,输入速度v是云台的转速 TMC2160Control ptz_hori_choice_microstep(float ptz_v) { float motor_v; TMC2160Control data_tmc2160; motor_v = ptz_v * PTZ_HORI_RATIO; if(motor_v > PTZ_HORI_MOTOR_MAX_SPEED) { motor_v = PTZ_HORI_MOTOR_MAX_SPEED; } if(motor_v < PTZ_HORI_MOTOR_MIN_SPEED) { motor_v = PTZ_HORI_MOTOR_MIN_SPEED; } data_tmc2160.set_v = motor_v; //给一个预设的默认值 data_tmc2160.microstep = TMC2160_1_16_STEP; data_tmc2160.microstep_vlue = 16; if(tmc2160_parameter.hori_step_1_256_en == 1 && motor_v >= tmc2160_parameter.hori_step_1_256_v_min && motor_v <= tmc2160_parameter.hori_step_1_256_v_max) { data_tmc2160.microstep = TMC2160_1_256_STEP; data_tmc2160.microstep_vlue = 256; } if(tmc2160_parameter.hori_step_1_128_en == 1 && motor_v >= tmc2160_parameter.hori_step_1_128_v_min && motor_v <= tmc2160_parameter.hori_step_1_128_v_max) { data_tmc2160.microstep = TMC2160_1_128_STEP; data_tmc2160.microstep_vlue = 128; } if(tmc2160_parameter.hori_step_1_64_en == 1 && motor_v > tmc2160_parameter.hori_step_1_64_v_min && motor_v <= tmc2160_parameter.hori_step_1_64_v_max) { data_tmc2160.microstep = TMC2160_1_64_STEP; data_tmc2160.microstep_vlue = 64; } if(tmc2160_parameter.hori_step_1_32_en == 1 && motor_v > tmc2160_parameter.hori_step_1_32_v_min && motor_v <= tmc2160_parameter.hori_step_1_32_v_max) { data_tmc2160.microstep = TMC2160_1_32_STEP; data_tmc2160.microstep_vlue = 32; } if(tmc2160_parameter.hori_step_1_16_en == 1 && motor_v > tmc2160_parameter.hori_step_1_16_v_min && motor_v <= tmc2160_parameter.hori_step_1_16_v_max) { data_tmc2160.microstep = TMC2160_1_16_STEP; data_tmc2160.microstep_vlue = 16; } if(tmc2160_parameter.hori_step_1_8_en == 1 && motor_v > tmc2160_parameter.hori_step_1_8_v_min && motor_v <= tmc2160_parameter.hori_step_1_8_v_max) { data_tmc2160.microstep = TMC2160_1_8_STEP; data_tmc2160.microstep_vlue = 8; } if(tmc2160_parameter.hori_step_1_4_en == 1 && motor_v > tmc2160_parameter.hori_step_1_4_v_min && motor_v <= tmc2160_parameter.hori_step_1_4_v_max) { data_tmc2160.microstep = TMC2160_1_4_STEP; data_tmc2160.microstep_vlue = 4; } if(tmc2160_parameter.hori_step_1_2_en == 1 && motor_v > tmc2160_parameter.hori_step_1_2_v_min && motor_v <= tmc2160_parameter.hori_step_1_2_v_max) { data_tmc2160.microstep = TMC2160_1_2_STEP; data_tmc2160.microstep_vlue = 2; } if(tmc2160_parameter.hori_step_1_1_en == 1 && motor_v > tmc2160_parameter.hori_step_1_1_v_min && motor_v <= tmc2160_parameter.hori_step_1_1_v_max) { data_tmc2160.microstep = TMC2160_1_1_STEP; data_tmc2160.microstep_vlue = 1; } switch(data_tmc2160.microstep) { case TMC2160_1_1_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 1); break; case TMC2160_1_2_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 2); break; case TMC2160_1_4_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 4); break; case TMC2160_1_8_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 8); break; case TMC2160_1_16_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 16); break; case TMC2160_1_32_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 32); break; case TMC2160_1_64_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 64); break; case TMC2160_1_128_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 128); break; case TMC2160_1_256_STEP: data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 256); break; } return data_tmc2160; } TMC2160Control ptz_vert_choice_microstep(float ptz_v) { float motor_v; TMC2160Control data_tmc2160; motor_v = ptz_v * PTZ_VERT_RATIO; if(motor_v > PTZ_VERT_MOTOR_MAX_SPEED) { motor_v = PTZ_VERT_MOTOR_MAX_SPEED; } if(motor_v < PTZ_VERT_MOTOR_MIN_SPEED) { motor_v = PTZ_VERT_MOTOR_MIN_SPEED; } data_tmc2160.set_v = motor_v; //给一个预设的默认值 data_tmc2160.microstep = TMC2160_1_16_STEP; data_tmc2160.microstep_vlue = 16; if(tmc2160_parameter.vert_step_1_256_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_256_v_min && motor_v <= tmc2160_parameter.vert_step_1_256_v_max) { data_tmc2160.microstep = TMC2160_1_256_STEP; data_tmc2160.microstep_vlue = 256; } if(tmc2160_parameter.vert_step_1_128_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_128_v_min && motor_v <= tmc2160_parameter.vert_step_1_128_v_max) { data_tmc2160.microstep = TMC2160_1_128_STEP; data_tmc2160.microstep_vlue = 128; } if(tmc2160_parameter.vert_step_1_64_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_64_v_min && motor_v <= tmc2160_parameter.vert_step_1_64_v_max) { data_tmc2160.microstep = TMC2160_1_64_STEP; data_tmc2160.microstep_vlue = 64; } if(tmc2160_parameter.vert_step_1_32_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_32_v_min && motor_v <= tmc2160_parameter.vert_step_1_32_v_max) { data_tmc2160.microstep = TMC2160_1_32_STEP; data_tmc2160.microstep_vlue = 32; } if(tmc2160_parameter.vert_step_1_16_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_16_v_min && motor_v <= tmc2160_parameter.vert_step_1_16_v_max) { data_tmc2160.microstep = TMC2160_1_16_STEP; data_tmc2160.microstep_vlue = 16; } if(tmc2160_parameter.vert_step_1_8_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_8_v_min && motor_v <= tmc2160_parameter.vert_step_1_8_v_max) { data_tmc2160.microstep = TMC2160_1_8_STEP; data_tmc2160.microstep_vlue = 8; } if(tmc2160_parameter.vert_step_1_4_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_4_v_min && motor_v <= tmc2160_parameter.vert_step_1_4_v_max) { data_tmc2160.microstep = TMC2160_1_4_STEP; data_tmc2160.microstep_vlue = 4; } if(tmc2160_parameter.vert_step_1_2_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_2_v_min && motor_v <= tmc2160_parameter.vert_step_1_2_v_max) { data_tmc2160.microstep = TMC2160_1_2_STEP; data_tmc2160.microstep_vlue = 2; } if(tmc2160_parameter.vert_step_1_1_en == 1 && motor_v >= tmc2160_parameter.vert_step_1_1_v_min && motor_v <= tmc2160_parameter.vert_step_1_1_v_max) { data_tmc2160.microstep = TMC2160_1_1_STEP; data_tmc2160.microstep_vlue = 1; } switch(data_tmc2160.microstep) { case TMC2160_1_1_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 1); break; case TMC2160_1_2_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 2); break; case TMC2160_1_4_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 4); break; case TMC2160_1_8_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 8); break; case TMC2160_1_16_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 16); break; case TMC2160_1_32_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 32); break; case TMC2160_1_64_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 64); break; case TMC2160_1_128_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 128); break; case TMC2160_1_256_STEP: data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 256); break; } return data_tmc2160; } void ptz_hori_start(char direction, float speed) { BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); g_ptz.hori_dec_mode = PTZ_DEC_MAX; //设定转动速度 g_ptz.hori_speed_set = speed; //设定转动方向 g_ptz.hori_direction_last = g_ptz.hori_direction_set; g_ptz.hori_direction_set = direction; g_ptz.hori_direction_actual = g_ptz.hori_direction_set; tmc2160_set_hori_dir(direction); //驱动芯片寄存器写数据 tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep); g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep; g_ptz.hori_tmc2160.f = 0; tmc2160_hori_motor_enable(TMC2160_MOTOR_ENABLE); g_ptz.hori_tmc2160.off_on =1; OSTimeDlyHMSM(0u, 0u, 0u, 10u); g_ptz.hori_start_stop_set = PTZ_HORI_START; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; ptz_hori_stop_count = 0; BSP_OS_SemPost(&ptz_hori_stop_mutex); } void ptz_hori_stop(unsigned short int time) { BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); g_ptz.hori_dec_mode = PTZ_DEC_MAX; //设定转动速度 g_ptz.hori_speed_set = 0; g_ptz.hori_speed_actual = 0; tmc2160_hori_motor_enable(TMC2160_MOTOR_DISABLE); g_ptz.hori_tmc2160.off_on = 0; ptz_hori_timer_stop(); g_ptz.hori_tmc2160.f = 0; g_ptz.hori_speed_actual = 0; g_ptz.hori_tmc2160.off_on = 0; //停止电机 g_ptz.hori_start_stop_set = PTZ_HORI_STOP; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP; //#ifdef PTZ_ELECTRIC_STABLE_ON//是否打开电子自锁 // drv8711_hori_change_register(DRV8711_TORQUE_TORQUE, drv8711_parameter.hori_self_lock_torque); // drv8711_hori_change_register(DRV8711_CTRL_MODE, drv8711_parameter.hori_self_lock_micr); // g_ptz.hori_drv8711.actual_microstep = drv8711_parameter.hori_self_lock_micr; // drv8711_hori_enbl(DRV8711_MOTOR_ENABLE); //#endif if(ptz_hori_stop_count <= 0) { OSTimeDlyHMSM(0u, 0u, 0u, time); ptz_hori_stop_count = 0; } ptz_hori_stop_count ++; BSP_OS_SemPost(&ptz_hori_stop_mutex); } /// 云台水平转动监控 static char ptz_hori_rotate_monitor_task() { static float BNP_D;//刹车最近点与当前位置的距离 static float BFP_D;//刹车最远点与当前位置的距离 static unsigned int k; TMC2160Control data; switch(g_ptz.hori_rotate_monitor_switch) { case 0://关闭 break; case PTZ_HORI_RIGHT_KEEP://向右转动，不刹车 data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed); //关闭转动监控 g_ptz.hori_rotate_monitor_switch = 0; //首先判断云台是否在转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { // if(g_ptz.hori_tmc2160.actual_microstep == data.microstep) // { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同，直接赋值速度 // }else{ // // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度，，，解决转动过程中改变速度引起的停顿 // g_ptz.hori_tmc2160 = data; // tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep); // g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep; // } }else{ //如果转向不同 //则先刹车 if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; g_ptz.hori_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) { break; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set); } // //判断转动方向是否相同 // //如果转向相同且细分程度相同，则直接修改设置的转速 // if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT && // g_ptz.hori_tmc2160.actual_microstep == data.microstep) // { // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; // g_ptz.hori_tmc2160 = data; // } // else//如果转向不同 // { // //则先刹车 // // if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) // { // g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; // g_ptz.hori_dec_mode = PTZ_DEC_DEF; // } // for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms // { // if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) // { // break; // } // OSTimeDlyHMSM(0u, 0u, 0u, 2u); // } // // ptz_hori_stop(PTZ_HORI_STOP_TIME); // //再启动 // g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT; // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; // g_ptz.hori_tmc2160 = data; // ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set); // } } else//没有转动 { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set); } break; case PTZ_HORI_LEFT_KEEP://向左转动，不刹车 data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed); //关闭转动监控 g_ptz.hori_rotate_monitor_switch = 0; //首先判断云台是否在转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转动方向是否相同 //如果转向相同且细分程度相同，则直接修改设置的转速 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { // if(g_ptz.hori_tmc2160.actual_microstep == data.microstep) // { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同，直接赋值速度 // }else{ // g_ptz.hori_tmc2160 = data; // tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep); // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度 // g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep; // } } else//如果转向不同 { //则先刹车 if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; g_ptz.hori_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) { break; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set); } // //判断转动方向是否相同 // //如果转向相同且细分程度相同，则直接修改设置的转速 // if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT && // g_ptz.hori_tmc2160.actual_microstep == data.microstep) // { // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; //// g_ptz.hori_tmc2160 = data; // } // else//如果转向不同 // { // //则先刹车 // if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) // { // g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; // g_ptz.hori_dec_mode = PTZ_DEC_DEF; // } // for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms // { // if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) // { // break; // } // OSTimeDlyHMSM(0u, 0u, 0u, 2u); // } // // ptz_hori_stop(PTZ_HORI_STOP_TIME); // //再启动 // g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT; // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; // g_ptz.hori_tmc2160 = data; // ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set); // } } else//没有转动 { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set); } break; case PTZ_HORI_GO_TO_ANGLE_A://角度定位 case PTZ_HORI_RIGHT_ANGLE: case PTZ_HORI_LEFT_ANGLE: case PTZ_HORI_MIN_DISTANCE: case PTZ_HORI_MAX_DISTANCE: data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed); //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动 if(g_ptz.hori_rotate_plan.rotate_switch == 1) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B; //如果不是处于同一个位置 //判断是否转动 //处于转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { // if(g_ptz.hori_tmc2160.actual_microstep == data.microstep) // {//如果转向相同且细分程度相同 g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同，直接赋值速度 // }else{ // g_ptz.hori_tmc2160 = data; // tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep); // g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度 // g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep; // } } else//转向不同，则直接修改设置的转速 { //则先刹车 if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; g_ptz.hori_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) { break; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED); if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } } else//g_ptz.hori_rotate_plan.rotate_switch == 0; { g_ptz.hori_rotate_monitor_switch = 0; //不需要转动则刹车停止，关闭监控 g_ptz.hori_arrive_flag = 0;//转动到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { ptz_hori_stop(PTZ_HORI_STOP_TIME); } } break; case PTZ_HORI_RIGHT_CYCLE: case PTZ_HORI_LEFT_CYCLE: if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } g_ptz.hori_rotate_monitor_switch = PTZ_HORI_CYCLE; break; case PTZ_HORI_CYCLE: BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle); if(BNP_D > 360.0 / 2.0) { BNP_D = 360.0 - BNP_D; } BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle); if(BFP_D > 360.0 / 2.0) { BFP_D = 360.0 - BFP_D; } /*定位*/ if(BNP_D + BFP_D > (g_ptz.hori_rotate_plan.stop_angle_range + 2.0)) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B; } break; case PTZ_HORI_GO_TO_ANGLE_B: BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle); if(BNP_D > 360.0 / 2.0) { BNP_D = 360.0 - BNP_D; } BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle); if(BFP_D > 360.0 / 2.0) { BFP_D = 360.0 - BFP_D; } /*定位*/ if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01)) { if(g_ptz.hori_repeat_locate_switch == 0)//重复定位关闭 { g_ptz.hori_arrive_flag = 0;//转到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 } g_ptz.hori_rotate_monitor_switch = 0; if(g_ptz.hori_repeat_locate_switch == 1)//重复定位打开 { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A; } ptz_hori_stop(PTZ_HORI_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED); if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } break; case PTZ_HORI_BRAKE://停止转动 g_ptz.hori_rotate_monitor_switch = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); break; case PTZ_HORI_DEC_BRAKE_A://减速刹车 //g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; data = ptz_hori_choice_microstep(PTZ_HORI_BREAK_SPEED); if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; g_ptz.hori_dec_mode = PTZ_DEC_MAX; } g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B; k = 0; break; case PTZ_HORI_DEC_BRAKE_B: k++;//K增加1，时间增加1ms if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1 || k >= (tmc2160_parameter.hori_dec_time_max / 1.0)) { k = 0; g_ptz.hori_rotate_monitor_switch = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); } break; //重复定位路径规划 case PTZ_HORI_REPEAT_LOCATE_A: ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_B; break; //重复定位启动控制 case PTZ_HORI_REPEAT_LOCATE_B: //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动 data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed); if(g_ptz.hori_rotate_plan.rotate_switch == 1) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C; //如果不是处于同一个位置 //判断是否转动 //处于转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同且细分程度相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction && g_ptz.hori_tmc2160.actual_microstep == data.microstep) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; // g_ptz.hori_tmc2160 = data; } else//转向不同，则直接修改设置的转速 { //则先刹车 if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; g_ptz.hori_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) { break; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED); if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } } else//g_ptz.hori_rotate_plan.rotate_switch == 0; { g_ptz.hori_rotate_monitor_switch = 0; //不需要转动则刹车停止，关闭监控 g_ptz.hori_arrive_flag = 0;//转到指定预位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { ptz_hori_stop(PTZ_HORI_STOP_TIME); } } break; //重复定位判断是否达到定位位置 case PTZ_HORI_REPEAT_LOCATE_C: BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle); if(BNP_D > 360.0 / 2.0) { BNP_D = 360.0 - BNP_D; } BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle); if(BFP_D > 360.0 / 2.0) { BFP_D = 360.0 - BFP_D; } /*定位*/ if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01)) { g_ptz.hori_arrive_flag = 0;//转到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.hori_rotate_monitor_switch = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED); if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } break; case PTZ_HORI_RIGHT_ANGLE_INC: data = ptz_hori_choice_microstep(PTZ_HORI_BREAK_SPEED); //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动 if(g_ptz.hori_rotate_plan.rotate_switch == 1) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B; //如果不是处于同一个位置 //判断是否转动 //处于转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction && g_ptz.hori_tmc2160.actual_microstep == data.microstep) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; // g_ptz.hori_tmc2160 = data; } else//转向不同，则直接修改设置的转速 { //则先刹车 if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; g_ptz.hori_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1) { break; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_tmc2160 = data; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED); //如果离最近刹车距离小于一定程度，则以最小转速转动 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_BREAK_SPEED_ANGLE) { if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED) { g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED; } } } } } else//g_ptz.hori_rotate_plan.rotate_switch == 0; { g_ptz.hori_rotate_monitor_switch = 0; //不需要转动则刹车停止，关闭监控 g_ptz.hori_arrive_flag = 0;//转动到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { ptz_hori_stop(PTZ_HORI_STOP_TIME); } } break; case PTZ_HORI_LEFT_ANGLE_INC: break; } return 1; } void ptz_vert_start(char direction, float speed) { BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); g_ptz.vert_dec_mode = PTZ_DEC_MAX; //设定转动速度 g_ptz.vert_speed_set = speed; //设定转动方向 g_ptz.vert_direction_last = g_ptz.vert_direction_set; g_ptz.vert_direction_set = direction; g_ptz.vert_direction_actual = g_ptz.vert_direction_set; tmc2160_set_vert_dir(direction); //写寄存器数据 tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, g_ptz.vert_tmc2160.microstep); g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep; g_ptz.vert_tmc2160.f = 0; tmc2160_vert_motor_enable(TMC2160_MOTOR_ENABLE); g_ptz.vert_tmc2160.off_on = 1; OSTimeDlyHMSM(0u, 0u, 0u, 10u); g_ptz.vert_start_stop_set = PTZ_VERT_START; g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set; ptz_vert_stop_count = 0; BSP_OS_SemPost(&ptz_vert_stop_mutex); } void ptz_vert_stop(unsigned short int time) { BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); g_ptz.vert_dec_mode = PTZ_DEC_MAX; if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { //设定转动速度 g_ptz.vert_speed_set = 0; g_ptz.vert_speed_actual = 0; //若需要自锁，则驱动器要保持使能状态 tmc2160_vert_motor_enable(TMC2160_MOTOR_DISABLE); g_ptz.vert_tmc2160.off_on = 0; ptz_vert_timer_stop(); g_ptz.vert_tmc2160.f = 0; g_ptz.vert_speed_actual = 0; g_ptz.vert_tmc2160.off_on = 0; //停止电机 g_ptz.vert_start_stop_set = PTZ_VERT_STOP; g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set; g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP; //#ifdef PTZ_ELECTRIC_STABLE_ON//是否打开电子自锁 // drv8711_vert_change_register(DRV8711_TORQUE_TORQUE, drv8711_parameter.vert_self_lock_torque); // drv8711_vert_change_register(DRV8711_CTRL_MODE, drv8711_parameter.vert_self_lock_micr); // g_ptz.vert_drv8711.actual_microstep = drv8711_parameter.vert_self_lock_micr; // drv8711_vert_enbl(DRV8711_MOTOR_ENABLE); //#endif if(ptz_vert_stop_count <= 0) { OSTimeDlyHMSM(0u, 0u, 0u, time); ptz_vert_stop_count = 0; } ptz_vert_stop_count ++; } BSP_OS_SemPost(&ptz_vert_stop_mutex); } /// 云台垂直转动监控 static char ptz_vert_rotate_monitor_task() { static float BNP_D;//刹车最近点与当前位置的距离 static float BFP_D;//刹车最远点与当前位置的距离 static char cmd_type; static unsigned int k; TMC2160Control data; switch(g_ptz.vert_rotate_monitor_switch) { case 0://关闭 break; case PTZ_VERT_GO_TO_ANGLE_A://转动到指定角度启动 case PTZ_VERT_ANGLE: case PTZ_VERT_UP_KEEP: case PTZ_VERT_DOWN_KEEP: data = ptz_vert_choice_microstep(g_ptz.vert_rotate_plan.max_speed); cmd_type = g_ptz.vert_rotate_monitor_switch; g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_B; if(g_ptz.vert_rotate_plan.rotate_switch == 1) { //首先判断是否转动 //处于转动 if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { //判断转向是否相同 //如果转向相同且步进电机细分程度相同 if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction) { // g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; // if(g_ptz.vert_tmc2160.actual_microstep == data.microstep) // {//如果转向相同且细分程度相同 g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//细分相同，直接赋值速度 // }else{ // g_ptz.vert_tmc2160 = data; // tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, g_ptz.vert_tmc2160.microstep); // g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//先改细分再赋值速度 // g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep; // } } else//转向不同 { //则先刹车 if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; g_ptz.vert_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_VERT_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1) { break; } #ifdef PTZ_PHOTOELECTRIC_SWITCH if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置 { if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN) { break; } } if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置 { if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP) { break; } } #endif OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_vert_stop(PTZ_VERT_STOP_TIME); //再启动 g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; g_ptz.vert_tmc2160 = data; //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_BREAK_SPEED_ANGLE) { if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED); g_ptz.vert_tmc2160 = data; } } ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } } else//没有转动 { g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; g_ptz.vert_tmc2160 = data; //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_BREAK_SPEED_ANGLE) { if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED); g_ptz.vert_tmc2160 = data; } } ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } } else//g_ptz.vert_rotate_plan.rotate_switch == 0 { //刹车关闭转动 g_ptz.vert_arrive_flag = 0;//转到指定预位置 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.vert_rotate_monitor_switch = 0; if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { ptz_vert_stop(PTZ_VERT_STOP_TIME); } } break; case PTZ_VERT_GO_TO_ANGLE_B://转动到指定角度定位 BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle); BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle); /*定位*/ if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01)) { if(g_ptz.vert_repeat_locate_switch == 0 && cmd_type != PTZ_VERT_UP_KEEP && cmd_type != PTZ_VERT_DOWN_KEEP) { g_ptz.vert_arrive_flag = 0;//转到指定预位置 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 } g_ptz.vert_rotate_monitor_switch = 0; if(g_ptz.vert_repeat_locate_switch == 1 && cmd_type != PTZ_VERT_UP_KEEP && cmd_type != PTZ_VERT_DOWN_KEEP) { g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_A; } ptz_vert_stop(PTZ_VERT_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 //data = ptz_choice_microstep(PTZ_VERT_BREAK_SPEED); if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_BREAK_SPEED_ANGLE) { if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; //g_ptz.vert_drv8711.set_f = data.set_f; } } break; case PTZ_VERT_BRAKE://刹车 g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); break; case PTZ_VERT_DEC_BRAKE_A://垂直减速刹车 g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED); if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; g_ptz.vert_dec_mode = PTZ_DEC_MAX; } g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B; k = 0; break; case PTZ_VERT_DEC_BRAKE_B: k ++;//K增加1,时间增加1ms if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1 || k >= (tmc2160_parameter.vert_dec_time_max / 1.0)) { k = 0; g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } break; //重复定位路径规划 case PTZ_VERT_REPEAT_LOCATE_A: ptz_vert_rotate_plan(PTZ_VERT_ANGLE); g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_B; break; //重复定位转动启动 case PTZ_VERT_REPEAT_LOCATE_B: data = ptz_vert_choice_microstep(g_ptz.vert_rotate_plan.max_speed); if(g_ptz.vert_rotate_plan.rotate_switch == 1) { g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C; //首先判断是否转动 //处于转动 if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction && g_ptz.vert_tmc2160.actual_microstep == data.microstep) { g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; g_ptz.vert_dec_mode = PTZ_DEC_DEF; } for(k = 0; k <= (PTZ_VERT_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms { if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1) { break; } #ifdef PTZ_PHOTOELECTRIC_SWITCH if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置 { if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN) { break; } } if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置 { if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP) { break; } } #endif OSTimeDlyHMSM(0u, 0u, 0u, 2u); } ptz_vert_stop(PTZ_VERT_STOP_TIME); //再启动 g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; g_ptz.vert_tmc2160 = data; //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_BREAK_SPEED_ANGLE) { if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED); g_ptz.vert_tmc2160 = data; } } ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } } else//没有转动 { g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; g_ptz.vert_tmc2160 = data; //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_BREAK_SPEED_ANGLE) { if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED); g_ptz.vert_tmc2160 = data; } } ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } } else//g_ptz.vert_rotate_plan.rotate_switch == 0 { //刹车关闭转动 g_ptz.vert_arrive_flag = 0;//转到指定位置 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.vert_rotate_monitor_switch = 0; if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { ptz_vert_stop(PTZ_VERT_STOP_TIME); } } break; case PTZ_VERT_REPEAT_LOCATE_C: BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle); BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle); /*定位*/ if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01)) { g_ptz.vert_arrive_flag = 0;//预置位扫描，转到指定预置位 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_BREAK_SPEED_ANGLE) { if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED) { g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED; } } break; } //当云台垂直方向处于极限位置时，防止垂直方向角度越界 #ifdef PTZ_PHOTOELECTRIC_SWITCH if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置 { if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN) { g_ptz.vert_arrive_flag = 0; g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } } if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置 { if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_direction_set == PTZ_VERT_DIR_UP) { g_ptz.vert_arrive_flag = 0; g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } } #endif return 1; } #endif static void ptz_hori_rotate_task() { while(1) { ptz_hori_rotate_monitor_task(); OSTimeDlyHMSM(0u, 0u, 0u, 1u); } } static void ptz_vert_rotate_task() { while(1) { ptz_vert_rotate_monitor_task(); OSTimeDlyHMSM(0u, 0u, 0u, 1u); } } static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE]; static void creat_task_hori_rotate(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task, (void *) 0, (OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1], (INT8U ) TASK_HORI_ROATE_MONITOR_PRIO, (INT16U ) TASK_HORI_ROATE_MONITOR_PRIO, (OS_STK *)&task_hori_rotate_stk[0], (INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_rotate_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_rotate_task failed...\n\r"); } } static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE]; static void creat_task_vert_rotate(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task, (void *) 0, (OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1], (INT8U ) TASK_VERT_ROATE_MONITOR_PRIO, (INT16U ) TASK_VERT_ROATE_MONITOR_PRIO, (OS_STK *)&task_vert_rotate_stk[0], (INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_rotate_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_rotate_task failed...\n\r"); } } void init_rotate_monitor_module(void) { BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex"); BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex"); #ifdef TMC2160 tmc2160_init(); #endif creat_task_hori_rotate(); creat_task_vert_rotate(); } #endif