Merge remote-tracking branch 'origin/master'

energy/src/energy/clear/clear.cpp

@@ -8,11 +8,10 @@ using namespace std;
 using namespace cv;
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于清空各vector
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::clearAll(){
+void Energy::clearAll() {
     fans.clear();
     armors.clear();
     flow_strip_fans.clear();
@@ -24,10 +23,11 @@ void Energy::clearAll(){
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于图像预处理
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::initImage(cv::Mat &src){
+void Energy::initImage(cv::Mat &src) {
 //    imagePreprocess(src);
 //    if(show_process)imshow("img_preprocess", src);
     if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
     threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
     if (show_process)imshow("bin", src);
+    if (show_energy)waitKey(1);
 }

energy/src/energy/run.cpp

@@ -106,6 +106,8 @@ void Energy::runBig(cv::Mat &gimbal_src) {
     if (save_mark)writeDownSlightChange(gimbal_src);
     getPredictPoint(target_point);
     getAimPoint(predict_point);
+//    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl;
+//    waitKey(0);
     judgeShootInGimbal();
     sendEnergy();
 }

energy/src/energy/send/send.cpp

@@ -18,19 +18,32 @@ void Energy::sendEnergy() {
         if (camera_cnt == 1) {
             sum_yaw += yaw_rotation;
             sum_pitch += pitch_rotation;
-            float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
+            if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) {
-            float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
+                MINMAX(sum_yaw, -100, 100);
-            MINMAX(yaw_I_component, -2, 2);
+                MINMAX(sum_yaw, -100, 100);
-            MINMAX(pitch_I_component, -2, 2);
+            } else if (ROBOT_ID == 7) {
+                float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
+                float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
+                MINMAX(yaw_I_component, -3, 3);
+                MINMAX(pitch_I_component, -3, 3);
+//                MINMAX(yaw_I_component, -2, 2);
+//                MINMAX(pitch_I_component, -2, 2);
+            }
+
             double tmp_yaw = yaw_rotation;
             double tmp_pitch = pitch_rotation;
-            yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
+            yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
+                           BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
             pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
-                    BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
+                             BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
-            MINMAX(yaw_rotation, -6, 6);
+//            cout << BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
-            MINMAX(pitch_rotation, -6, 6);
+//                 << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
             last_yaw = tmp_yaw;
             last_pitch = tmp_pitch;
+            if (ROBOT_ID == 7) {
+                MINMAX(yaw_rotation, -6, 6);
+                MINMAX(pitch_rotation, -6, 6);
+            }
         } else if (is_chassis) {
 //            sum_yaw += yaw_rotation - mcu_data.curr_yaw;
 //            sum_pitch += pitch_rotation - mcu_data.curr_pitch;
@@ -41,13 +54,15 @@ void Energy::sendEnergy() {
 //            last_yaw = tmp_yaw;
 //            last_pitch = tmp_pitch;
         }
-    } else if (is_small){
+    } else if (is_small) {
         double tmp_yaw = yaw_rotation;
         double tmp_pitch = pitch_rotation;
         yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
         pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
-        MINMAX(yaw_rotation, -6, 6);
+        if (ROBOT_ID == 7) {
-        MINMAX(pitch_rotation, -6, 6);
+            MINMAX(yaw_rotation, -6, 6);
+            MINMAX(pitch_rotation, -6, 6);
+        }
         last_yaw = tmp_yaw;
         last_pitch = tmp_pitch;
     }

main.cpp

@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
             video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper("/home/sun/桌面/shiying/18.avi");
+            video_gimbal = new VideoWrapper("/home/sun/项目/RM_auto-aim/gimbal_video/57.avi");
-            video_chassis = new VideoWrapper("/home/sun/桌面/shiying/18.avi");
+            video_chassis = new VideoWrapper("/home/sun/项目/RM_auto-aim/gimbal_video/57.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");

others/include/config/setconfig.h

@@ -31,6 +31,11 @@
 #ifndef ENERGY_CAMERA_GAIN
     #define ENERGY_CAMERA_GAIN  (20)
 #endif
+
+#ifndef ROBOT_ID
+    #define ROBOT_ID (0)
+#endif
+
 #ifndef SMALL_YAW_AIM_KD
     #define SMALL_YAW_AIM_KD (0.35)
 #endif