energy changed

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);//获取预测点位

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;

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<float>(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){
+//    if (target_polar_angle_ > 40 && target_polar_angle_ < 140){
-        extra_delta_x = 0;
+//        extra_delta_x = 0;
-        extra_delta_y = -14;
+//        extra_delta_y = -10;
-    }
+//    }
-    else if (target_polar_angle_ > 30 && target_polar_angle_ <= 40){
+//    else if (target_polar_angle_ > 30 && target_polar_angle_ <= 40){
-        extra_delta_x = 8;
+//        extra_delta_x = 8;
-        extra_delta_y = -8;
+//        extra_delta_y = -8;
-    }
+//    }
-    else if (target_polar_angle_ >= 140 && target_polar_angle_ < 150){
+//    else if (target_polar_angle_ >= 140 && target_polar_angle_ < 150){
-        extra_delta_x = -8;
+//        extra_delta_x = -8;
-        extra_delta_y = -8;
+//        extra_delta_y = -8;
-    }
+//    }
-    else {
+//    else {
-        extra_delta_x = 0;
+//        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 {
 //        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;

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<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);

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 <sys/time.h>
 
@@ -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);

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)