energy changed(8/7game PID test)

energy/include/energy/energy.h

@@ -52,6 +52,7 @@ private:
     bool is_chassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
     bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
     bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
+    bool is_find_target;//判断当前是否找到了目标
     bool energy_mode_init;//正在进行大小符判断
     bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
     bool manual_mark;//若操作手进行过手动标定，则为true
@@ -91,6 +92,7 @@ private:
     float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
 
     systime time_start_guess;
+    systime time_start_energy;
 
     cv::RotatedRect centerR;//风车中心字母R的可能候选区
     cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
@@ -116,8 +118,6 @@ private:
     std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
 
 
-
-
     void initEnergy();//能量机关初始化
     void initEnergyPartParam();//能量机关参数初始化
     void initRotation();//顺逆时针初始化

energy/include/energy/param_struct_define.h

@@ -17,6 +17,7 @@ using std::vector;
 // ---------------------------------------------------------------------------------------------------------------------
 struct EnergyPartParam {
 	int GRAY_THRESH;//二值化阈值
+	int SUB_GRAY_THRESH;//备用二值化阈值
 	int SPLIT_GRAY_THRESH;//通道分离二值化阈值
 	int FAN_GRAY_THRESH;//扇叶识别二值化阈值
 	int ARMOR_GRAY_THRESH;//装甲板识别二值化阈值

energy/src/energy/clear/clear.cpp

@@ -26,8 +26,16 @@ void Energy::clearAll() {
 void Energy::initImage(cv::Mat &src) {
 //    imagePreprocess(src);
 //    if(show_process)imshow("img_preprocess", src);
+    systime cur_time;
+    getsystime(cur_time);
+    float interval = getTimeIntervalms(cur_time, time_start_energy);
+
     if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
+    if(interval > 3000 && !is_find_target){
+        threshold(src, src, energy_part_param_.SUB_GRAY_THRESH, 255, THRESH_BINARY);
+    }else{
         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/clear/energy_init.cpp

@@ -17,6 +17,7 @@ void Energy::initEnergy() {
     is_mark = false;
     is_guessing = false;
     is_predicting = true;
+    is_find_target = false;
     energy_mode_init = true;
     energy_rotation_init = true;
     manual_mark = false;
@@ -71,7 +72,7 @@ void Energy::initEnergy() {
     target_armors.clear();
     flow_strips.clear();
     all_target_armor_centers.clear();
-    while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
+    while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
 
 }
 
@@ -82,7 +83,8 @@ void Energy::initEnergy() {
 void Energy::initEnergyPartParam() {
 //    gimbal_energy_part_param_.GRAY_THRESH = 120;//home
 //    gimbal_energy_part_param_.GRAY_THRESH = 200;//official
-    gimbal_energy_part_param_.GRAY_THRESH =180;//game
+    gimbal_energy_part_param_.GRAY_THRESH = 180;//game
+    gimbal_energy_part_param_.SUB_GRAY_THRESH = 100;
     gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180;
     gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
     gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80;
@@ -157,7 +159,6 @@ void Energy::initEnergyPartParam() {
     gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
 
 
-
     chassis_energy_part_param_.GRAY_THRESH = 120;//home
 //    chassis_energy_part_param_.GRAY_THRESH = 200;//official
 //    chassis_energy_part_param_.GRAY_THRESH = 225;

energy/src/energy/energy.cpp

@@ -53,6 +53,8 @@ void Energy::setBigEnergyInit() {
     initEnergy();
     initEnergyPartParam();
 
+    getsystime(time_start_energy);
+
     is_big = true;
     is_small = false;
     is_gimbal = true;

energy/src/energy/run.cpp

@@ -85,6 +85,7 @@ void Energy::runBig(cv::Mat &gimbal_src) {
         if (!findTargetInFlowStripFan()) return;
         if (!findFlowStrip(gimbal_src))return;
     }
+    is_find_target = true;
     if (!findCenterROI(gimbal_src))return;
     if (show_energy)showFlowStrip("strip", gimbal_src);
     if (!findCenterR(gimbal_src))return;

energy/src/energy/send/send.cpp

@@ -24,20 +24,30 @@ void Energy::sendEnergy() {
             } 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(yaw_I_component, -3, 3);
-                MINMAX(pitch_I_component, -3, 3);
+//                MINMAX(pitch_I_component, -3, 3);
-//                MINMAX(yaw_I_component, -2, 2);
+                MINMAX(yaw_I_component, -2, 2);
-//                MINMAX(pitch_I_component, -2, 2);
+                MINMAX(pitch_I_component, -2, 2);
             }
 
             double tmp_yaw = yaw_rotation;
             double tmp_pitch = pitch_rotation;
+            if(mcu_data.mark == 1){
+                yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
+                        TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
+                pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
+                        TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
+            } else {
                 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);
-//            cout << BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
+            }
+
+//            cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
 //                 << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
+//            cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
+//                 << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
             last_yaw = tmp_yaw;
             last_pitch = tmp_pitch;
             if (ROBOT_ID == 7) {

main.cpp

@@ -30,7 +30,7 @@ using namespace std;
 McuData mcu_data = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        ARMOR_STATE,    // 当前状态，自瞄-大符-小符
+        SMALL_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         1,              // 是否启用数字识别
         ENEMY_BLUE,      // 敌方颜色
@@ -42,7 +42,7 @@ WrapperHead *video_gimbal = nullptr;    // 云台摄像头视频源
 WrapperHead *video_chassis = nullptr;   // 底盘摄像头视频源
 
 Serial serial(115200);                  // 串口对象
-uint8_t last_state = INIT_STATE;     // 上次状态，用于初始化
+uint8_t last_state = INIT_STATE;     // 上次状态，用于初始化y
 // 自瞄主程序对象
 ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/");
 // 能量机关主程序对象
@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
             video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper(PROJECT_DIR"/5.avi");
+            video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-small.avi");
-            video_chassis = new VideoWrapper(PROJECT_DIR"/5.avi");
+            video_chassis = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-small.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");
@@ -123,8 +123,8 @@ int main(int argc, char *argv[]) {
                 if (!from_camera) extract(gimbal_src, chassis_src);
                 if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
                 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
-                energy.runBig(gimbal_src, chassis_src);
+//                energy.runBig(gimbal_src, chassis_src);
-//                energy.runBig(gimbal_src);
+                energy.runBig(gimbal_src);
             } else if (curr_state == SMALL_ENERGY_STATE) {
                 if (last_state != SMALL_ENERGY_STATE) {
                     LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!"));

tools/create-startup.sh

@@ -5,5 +5,5 @@ echo "echo sjturm | sudo -S cpufreq-set -g performance" >> $2/startup-run
 echo "mkdir $1/Mark" >> $2/startup-run
 echo "mkdir $1/gimbal_video" >> $2/startup-run
 echo "mkdir $1/armor_box_photo" >> $2/startup-run
-echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --save-video --wait-uart --save-labelled-boxes --show-armor-box\\\"\"" >> $2/startup-run
+echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --save-video --save-mark --wait-uart --save-labelled-boxes\\\"\"" >> $2/startup-run
 chmod +x $2/startup-run