diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index ea5d92c..197b74c 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -73,7 +73,7 @@ private: int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 int last_mode;//上一帧的能量机关状态 int manual_delta_x, manual_delta_y;//手动微调量 - int extra_delta_y;//在风车运动到最高点附近的额外补偿量 + int extra_delta_x, extra_delta_y;//在风车运动到最高点附近的额外补偿量 float target_polar_angle;//待击打装甲板的极坐标角度 float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 41123ce..2123b82 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -39,6 +39,7 @@ void Energy::initEnergy() { manual_delta_x = 0; manual_delta_y = 0; extra_delta_y = 0; + extra_delta_x = 0; target_polar_angle = -1000; last_target_polar_angle = -1000; @@ -76,7 +77,7 @@ void Energy::initEnergy() { // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimbal_energy_part_param_.GRAY_THRESH = 90;//home + gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official // gimbal_energy_part_param_.GRAY_THRESH = 225; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 8d7c6d2..d53d1f5 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -17,10 +17,32 @@ extern mcu_data mcuData; void Energy::getAimPoint(cv::Point target_point_) { float target_polar_angle_ = static_cast(180 / PI * atan2(-1 * (target_point_.y - circle_center_point.y), (target_point_.x - circle_center_point.x))); - if (target_polar_angle_ > 50 && target_polar_angle_ < 130)extra_delta_y = -16; - else extra_delta_y = 0; + if (target_polar_angle_ > 40 && target_polar_angle_ < 140){ + extra_delta_x = 0; + extra_delta_y = -14; + } + else if (target_polar_angle_ > 30 && target_polar_angle_ <= 40){ + extra_delta_x = 8; + extra_delta_y = -8; + } + else if (target_polar_angle_ >= 140 && target_polar_angle_ < 150){ + extra_delta_x = -8; + extra_delta_y = -8; + } + else { + extra_delta_x = 0; + extra_delta_y = 0; + } - double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x); +// if (target_polar_angle_ > 20 && target_polar_angle_ <= 90) { +// extra_delta_y = -17 * (target_polar_angle_ - 20) / 70; +// } else if (target_polar_angle_ > 90 && target_polar_angle_ < 160) { +// extra_delta_y = -17 * (160 - target_polar_angle_) / 70; +// } else { +// extra_delta_y = 0; +// } + + double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x - extra_delta_x); double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcuData.delta_y - manual_delta_y - extra_delta_y); yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;