diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 8509d32..ea5d92c 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -73,6 +73,7 @@ private: int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 int last_mode;//上一帧的能量机关状态 int manual_delta_x, manual_delta_y;//手动微调量 + int 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 d86cc50..41123ce 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -38,6 +38,7 @@ void Energy::initEnergy() { last_mode = -1;//既不是大符也不是小符 manual_delta_x = 0; manual_delta_y = 0; + extra_delta_y = 0; target_polar_angle = -1000; last_target_polar_angle = -1000; @@ -75,7 +76,7 @@ void Energy::initEnergy() { // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimbal_energy_part_param_.GRAY_THRESH = 100;//home + gimbal_energy_part_param_.GRAY_THRESH = 90;//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 2c83150..8d7c6d2 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -14,9 +14,14 @@ extern mcu_data mcuData; //---------------------------------------------------------------------------------------------------------------------- // 此函数通过自瞄逻辑击打目标点,用于大符的自动对心和小符直接打击 // --------------------------------------------------------------------------------------------------------------------- -void Energy::getAimPoint(cv::Point target_point) { - double dx = -(target_point.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x); - double dy = -(target_point.y - 240 - COMPENSATE_PITCH - mcuData.delta_y - manual_delta_y); +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; + + double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_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; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; diff --git a/main.cpp b/main.cpp index a10e8ec..c5b60bb 100644 --- a/main.cpp +++ b/main.cpp @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal255.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal255.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully.");