diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 197b74c..a26afc6 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -163,6 +163,7 @@ private: bool guessTarget();//获得猜测击打点位 bool getOrigin();//获得云台对心所需角度 void changeTarget();//判断目标是否改变 + void getCenter();//对心 void multipleMode(cv::Mat &src);//多模式切换 void getTargetPolarAngle();//获得目标装甲板极坐标角度 void getPredictPoint(cv::Point target_point);//获取预测点位 diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 2123b82..b785a87 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -45,7 +45,7 @@ void Energy::initEnergy() { last_target_polar_angle = -1000; guess_polar_angle = -1000; last_base_angle = -1000; - predict_rad = 25; + predict_rad = 27; attack_distance = ATTACK_DISTANCE; center_delta_yaw = 1000; center_delta_pitch = 1000; diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index d53d1f5..543f2d0 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -17,31 +17,34 @@ 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_ > 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; - } - -// 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 { +// if (target_polar_angle_ > 40 && target_polar_angle_ < 140){ +// extra_delta_x = 0; +// extra_delta_y = -10; +// } +// 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; // } + if (target_polar_angle_ > 0 && target_polar_angle_ <= 90) { +// extra_delta_x = EXTRA_DELTA_X * (90 - target_polar_angle_) / 90; + extra_delta_y = - EXTRA_DELTA_Y * target_polar_angle_ / 90; + } else if (target_polar_angle_ > 90 && target_polar_angle_ < 180) { +// extra_delta_x = - EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90; + extra_delta_y = - EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90; + } else { +// extra_delta_x = 0; + 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; diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp index 54dd3b9..8359a1d 100644 --- a/energy/src/energy/get/origin_get.cpp +++ b/energy/src/energy/get/origin_get.cpp @@ -10,12 +10,27 @@ using namespace std; using namespace cv; + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于对心 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::getCenter(){ + double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW); + double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH); + yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; + pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; + if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) { + shoot = 4; + } else + shoot = 2; +} + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心） // --------------------------------------------------------------------------------------------------------------------- bool Energy::getOrigin() { if (!auto_mark && !manual_mark) { -//五号车 double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW); double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH); center_delta_yaw = static_cast(atan(dx / FOCUS_PIXAL) * 180 / PI); diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index baeed89..549d29b 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -3,6 +3,7 @@ // #include "energy/energy.h" #include "log.h" +#include "config/setconfig.h" #include "options/options.h" #include @@ -85,6 +86,11 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showFlowStripFan("strip", gimbal_src); if (!findCenterR(gimbal_src))return; if (show_energy)showCenterR("R", gimbal_src); + +// getCenter(); +// sendEnergy(); +// return; + changeTarget(); getTargetPolarAngle(); if (energy_rotation_init) { @@ -114,6 +120,10 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (!findTargetInFlowStripFan()) return; if (!findFlowStrip(gimbal_src))return; +// getCenter(); +// sendEnergy(); +// return; + changeTarget(); if (save_mark)writeDownSlightChange(gimbal_src); getPredictPoint(target_point); diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index 7399504..e23db56 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -43,6 +43,12 @@ #ifndef COMPENSATE_PITCH #define COMPENSATE_PITCH (74) #endif +#ifndef EXTRA_DELTA_X + #define EXTRA_DELTA_X (10) +#endif +#ifndef EXTRA_DELTA_Y + #define EXTRA_DELTA_Y (10) +#endif //#define GIMBAL_FLIP_MODE (-1) //#define CHASSIS_FLIP_MODE (-1)