energy change

energy/include/energy/energy.h

@@ -29,6 +29,7 @@ public:
 
     void run(cv::Mat &gimbal_src, cv::Mat &chassis_src);
     void run(cv::Mat &gimbal_src);
+    void runSmall(cv::Mat &gimbal_src);
 
     Serial &serial;//串口
     void setEnergyInit();//设置能量机关初始化
@@ -51,7 +52,6 @@ private:
     bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
     bool manual_mark;//若操作手进行过手动标定，则为true
     bool auto_mark;//云台完成自动对心，则置为true
-    bool shoot;//若为true，则要求主控板发弹
     bool start_guess;//进入猜测状态的标志
     bool change_target;//目标切换的标志
 
@@ -77,6 +77,7 @@ private:
     float center_delta_yaw, center_delta_pitch;//对心时相差的角度
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
     float origin_yaw, origin_pitch;//初始的云台角度设定值
+    float shoot;//若为2，则要求主控板发弹
 
     timeval time_start_guess;
 
@@ -158,8 +159,9 @@ private:
     void getAllTargetArmorCenters();//记录所有目标装甲板中心坐标
     void getRecentTargetArmorCenters();//记录近30帧目标装甲板中心坐标
 
-    void judgeShoot();//判断是否可以发弹
+    void judgeMode();//判断大符还是小符
-    void JudgeMode();//判断大符还是小符
+    void judgeShootInGimbal();//在云台坐标系中判断是否可以发弹
+    void judgeShootInWorld();//在世界坐标系中判断是否可以发弹
     bool isGuessingTimeout();//判断猜测模式是否超时（没打中）
 
     void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离

energy/src/energy/clear/energy_init.cpp

@@ -21,7 +21,6 @@ void Energy::initEnergy() {
     energy_rotation_init = true;
     manual_mark = false;
     auto_mark = false;
-    shoot = false;
     start_guess = false;
     change_target = false;
 
@@ -41,14 +40,13 @@ void Energy::initEnergy() {
     last_target_polar_angle = -1000;
     guess_polar_angle = -1000;
     last_base_angle = -1000;
-    predict_rad = 20;
+    predict_rad = 30;
     attack_distance = ATTACK_DISTANCE;
     center_delta_yaw = 1000;
     center_delta_pitch = 1000;
     yaw_rotation = 0;
     pitch_rotation = 0;
-    origin_yaw = 0;
+    shoot = 0;
-    origin_pitch = 0;
 
     circle_center_point = Point(0, 0);
     target_point = Point(0, 0);
@@ -69,8 +67,8 @@ void Energy::initEnergy() {
 // 此函数对能量机关参数进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergyPartParam() {
-//    gimbal_energy_part_param_.GRAY_THRESH = 120;//home
+    gimbal_energy_part_param_.GRAY_THRESH = 120;//home
-    gimbal_energy_part_param_.GRAY_THRESH = 200;//official
+//    gimbal_energy_part_param_.GRAY_THRESH = 200;//official
 //    gimbal_energy_part_param_.GRAY_THRESH = 225;
     gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230;
     gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
@@ -142,8 +140,8 @@ void Energy::initEnergyPartParam() {
 
 
 
-//    chassis_energy_part_param_.GRAY_THRESH = 120;//home
+    chassis_energy_part_param_.GRAY_THRESH = 120;//home
-    chassis_energy_part_param_.GRAY_THRESH = 200;//official
+//    chassis_energy_part_param_.GRAY_THRESH = 200;//official
 //    chassis_energy_part_param_.GRAY_THRESH = 225;
     chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
     chassis_energy_part_param_.FAN_GRAY_THRESH = 75;
@@ -204,7 +202,7 @@ void Energy::initEnergyPartParam() {
     chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 55;
     chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 95;
     chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
+    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 25;
     chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12;
 //    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4;
     chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7;

energy/src/energy/find/energy_finder.cpp

@@ -69,6 +69,7 @@ int Energy::findArmors(const cv::Mat src) {
 
     ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
+    imshow("armor struct", src_bin);
 //    findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 
 //    for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓

energy/src/energy/get/origin_get.cpp

@@ -13,18 +13,20 @@ using namespace cv;
 // ---------------------------------------------------------------------------------------------------------------------
 bool Energy::getOrigin() {
     if (!auto_mark && !manual_mark) {
-        double dx = -(circle_center_point.x - 320);
+        double dx = -(circle_center_point.x - 320 - 10);
-        double dy = -(circle_center_point.y - 240);
+        double dy = -(circle_center_point.y - 240 - 22);
-        center_delta_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        center_delta_yaw = static_cast<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);
-        center_delta_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
+        center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI);
-        if (abs(center_delta_pitch) > 0.3 || abs(center_delta_pitch) > 0.3) {
+        if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) {
-            sendTarget(serial, center_delta_yaw, center_delta_pitch, false);
+//            cout << "origin not get!" << endl;
+//            cout << center_delta_yaw << '\t' << center_delta_pitch << endl;
+            sendTarget(serial, center_delta_yaw, center_delta_pitch, 0);
             return false;
         } else {
             origin_yaw = mcuData.curr_yaw;
             origin_pitch = mcuData.curr_pitch;
             auto_mark = true;
-            sendTarget(serial, center_delta_yaw, center_delta_pitch, true);
+            sendTarget(serial, center_delta_yaw, center_delta_pitch, 1);
             LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
             return true;
         }

energy/src/energy/judge/judge_mode.cpp

@@ -12,7 +12,7 @@ using namespace cv;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断大小符
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::JudgeMode() {
+void Energy::judgeMode() {
     getRecentTargetArmorCenters();
     if (recent_target_armor_centers.size() < 30) {
         return;

energy/src/energy/judge/judge_shoot.cpp

@@ -9,18 +9,33 @@ using namespace std;
 using namespace cv;
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断是否可以发弹
+// 此函数用于判断云台坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeShoot(){
+void Energy::judgeShootInGimbal(){
     if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
-        shoot = true;
+        shoot = 4;
         is_predicting = false;
         is_guessing = true;
         start_guess = true;
         gettimeofday(&time_start_guess, NULL);
         LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
     } else
-        shoot = false;
+        shoot = 2;
+}
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于判断世界坐标系下是否可以发弹
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::judgeShootInWorld(){
+    if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) {
+        shoot = 4;
+        is_predicting = false;
+        is_guessing = true;
+        start_guess = true;
+        gettimeofday(&time_start_guess, NULL);
+        LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
+    } else
+        shoot = 2;
 }

energy/src/energy/run.cpp

@@ -16,12 +16,11 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
     if (chassis_src.empty())
         run(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数
     else if (is_gimbal) {
-//        energy_part_param_ = chassis_energy_part_param_;
+        energy_part_param_ = gimbal_energy_part_param_;
-        energy_part_param_ = chassis_energy_part_param_;
         clearAll();
         initImage(gimbal_src);
-        findFans(gimbal_src);
+//        findFans(gimbal_src);
-        showFans("fan",gimbal_src);
+//        showFans("fan",gimbal_src);
 
         if (findArmors(gimbal_src) < 1)return;
         if (show_energy)showArmors("armor", gimbal_src);
@@ -37,7 +36,6 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
         initEnergy();
         destroyAllWindows();
     } else if (is_chassis) {
-//        energy_part_param_ = chassis_energy_part_param_;
         energy_part_param_ = chassis_energy_part_param_;
         clearAll();
         initImage(chassis_src);
@@ -54,9 +52,10 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
         if (!findCenterR(chassis_src))return;
         if (show_energy)showCenterR("R", chassis_src);
         getTargetPolarAngle();
+
         changeTarget();
-        JudgeMode();
+//        judgeMode();
-        if (energy_mode_init)return;
+//        if (energy_mode_init)return;
         if (is_big && energy_rotation_init) {
             initRotation();
             return;
@@ -64,7 +63,7 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
         if (is_predicting) {
             getPredictPoint(target_point);
             gimbalRotation();
-            judgeShoot();
+            judgeShootInGimbal();
             sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
         } else if (is_guessing && stayGuessing()) {
             findFans(chassis_src);
@@ -74,7 +73,7 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
             if (show_energy)showGuessTarget("guess", chassis_src);
             getPredictPoint(guess_point);
             gimbalRotation();
-            sendTarget(serial, yaw_rotation, pitch_rotation, false);
+            sendTarget(serial, yaw_rotation, pitch_rotation, 5);
         }
     }
 }
@@ -102,8 +101,8 @@ void Energy::run(cv::Mat &gimbal_src) {
     if (show_energy)showCenterR("R", gimbal_src);
     changeTarget();
     getTargetPolarAngle();
-    JudgeMode();
+//    judgeMode();
-    if (energy_mode_init)return;
+//    if (energy_mode_init)return;
     if (!getOrigin())return;
     if (is_big && energy_rotation_init) {
         initRotation();
@@ -112,7 +111,7 @@ void Energy::run(cv::Mat &gimbal_src) {
     if (is_predicting) {
         getPredictPoint(target_point);
         gimbalRotation();
-        judgeShoot();
+        judgeShootInGimbal();
         sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
     } else if (is_guessing && stayGuessing()) {
         findFans(gimbal_src);
@@ -122,6 +121,71 @@ void Energy::run(cv::Mat &gimbal_src) {
         if (show_energy)showGuessTarget("guess", gimbal_src);
         getPredictPoint(guess_point);
         gimbalRotation();
-        sendTarget(serial, yaw_rotation, pitch_rotation, false);
+        sendTarget(serial, yaw_rotation, pitch_rotation, 5);
     }
+
 }
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::runSmall(cv::Mat &gimbal_src){
+    energy_part_param_ = gimbal_energy_part_param_;
+    clearAll();
+    initImage(gimbal_src);
+
+    if (show_process)imshow("bin", gimbal_src);
+    if (findArmors(gimbal_src) < 1)return;
+    if (show_energy)showArmors("armor", gimbal_src);
+    if (!findFlowStripFan(gimbal_src))return;
+    if (!findTargetInFlowStripFan()) return;
+    if (show_energy)showFlowStripFan("strip", gimbal_src);
+
+    changeTarget();
+    getTargetPolarAngle();
+    if (!getOrigin())return;
+
+    getPredictPoint(target_point);
+    gimbalRotation();
+    getAimPoint(target_point);
+
+    if (is_predicting) {
+        getPredictPoint(target_point);
+        getAimPoint(target_point);
+        judgeShootInWorld();
+        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+    } else if (is_guessing && stayGuessing()) {
+        findFans(gimbal_src);
+        if (show_energy)showFans("fans", gimbal_src);
+        if (save_mark)writeDownMark();
+        guessTarget();
+        if (show_energy)showGuessTarget("guess", gimbal_src);
+        getPredictPoint(guess_point);
+        getAimPoint(target_point);
+        sendTarget(serial, yaw_rotation, pitch_rotation, 5);
+    }
+
+}
+
+//getPredictPoint(target_point);
+//gimbalRotation();
+//static bool k = false;
+//cout<<"delta yaw: "<<abs(yaw_rotation - mcuData.curr_yaw)<<endl;
+//cout<<"delta pitch: "<<abs(pitch_rotation - mcuData.curr_pitch)<<endl;
+//cout << "origin_yaw: " << origin_yaw << '\t' << "origin_pitch: " << origin_pitch << endl;
+//cout << "predict point: " << predict_point << endl;
+//
+//if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && abs(pitch_rotation - mcuData.curr_pitch) < 0.6) {
+//shoot = 4;
+//if (!k) {
+//sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+//cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
+//k = false;
+//}
+//waitKey(400);
+//} else {
+//shoot = 2;
+//sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+//}

energy/src/energy/tool/tool.cpp

@@ -43,7 +43,6 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2) {
     double x_0 = point_1.x - circle_center_point.x;
     double y_0 = point_1.y - circle_center_point.y;
     double r_0 = sqrt(pow(x_0, 2) + pow(y_0, 2));
-
     point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
     point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
 }

main.cpp

@@ -30,7 +30,7 @@ using namespace std;
 mcu_data mcuData = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        ARMOR_STATE,    // 当前状态，自瞄-大符-小符
+        BIG_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         1,              // 是否启用数字识别
         ENEMY_RED,      // 敌方颜色
@@ -64,10 +64,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(0/*, "armor"*/);
             video_chassis = new CameraWrapper(1/*, "energy"*/);
         } else {
-//            video_gimbal = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi");
+            video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/148.avi");
-//            video_chassis = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi");
+            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/148.avi");
-            video_gimbal = new VideoWrapper("/home/sjturm/Desktop/RM2019 能量机关视频/大能量机关（蓝+开灯）.mov");
-            video_chassis = new VideoWrapper("/home/sjturm/Desktop/RM2019 能量机关视频/大能量机关（蓝+开灯）.mov");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");
@@ -105,7 +103,6 @@ int main(int argc, char *argv[]) {
 #endif
                     }
                     ok = checkReconnect(video_gimbal->read(gimbal_src));
-                    video_gimbal->read(gimbal_src);
                     video_chassis->read(chassis_src);
 #ifdef GIMBAL_FLIP_MODE
                     flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
@@ -113,8 +110,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.run(gimbal_src, chassis_src);
+//                    energy.run(gimbal_src, chassis_src);
-//                    energy.run(gimbal_src);
+                    energy.run(gimbal_src);
                     last_state = mcuData.state;//更新上一帧状态
                 } else {                                         // 自瞄模式
                     if (last_state != ARMOR_STATE) {
@@ -133,7 +130,7 @@ int main(int argc, char *argv[]) {
                             armorFinder.run(gimbal_src);
                     });
                 }
-                cv::waitKey(1);
+//                cv::waitKey(1);
             });
         } while (ok);
         delete video_gimbal;

others/src/additions/additions.cpp

@@ -37,9 +37,19 @@ void uartReceive(Serial *pSerial) {
         pSerial->ReadData((uint8_t *) buffer, sizeof(mcuData)+1);
         if (buffer[sizeof(mcuData)] == '\n') {
             memcpy(&mcuData, buffer, sizeof(mcuData));
-            LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark);
+//            LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark);
+//            LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch);
+//            static int t = time(nullptr);
+//            static int cnt = 0;
+//            if(time(nullptr) > t){
+//                t = time(nullptr);
+//                LOGM("fps:%d", cnt);
+//                cnt = 0;
+//            }else{
+//                cnt++;
+//            }
         }else{
-            LOGW("corrupt data!");
+//            LOGW("corrupt data!");
         }
     }
 }