energy changed

energy/src/energy/get/aim_point_get.cpp

@@ -13,11 +13,12 @@ using namespace cv;
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::getAimPoint(cv::Point target_point) {
 //五号车
-    double dx = -(target_point.x - 320 - 3);
+//    double dx = -(target_point.x - 320 - 10);
-    double dy = -(target_point.y - 240 - 74);
+//    double dy = -(target_point.y - 240 - 22);
 //四号车
-//    double dx = -(target_point.x - 320 + 16);
+    double dx = -(target_point.x - 320 - 0);
-//    double dy = -(target_point.y - 240 - 82);
+    double dy = -(target_point.y - 240 - 69);
     yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
     pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
+    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
 }

energy/src/energy/get/origin_get.cpp

@@ -17,8 +17,8 @@ bool Energy::getOrigin() {
 //        double dx = -(circle_center_point.x - 320 - 10);
 //        double dy = -(circle_center_point.y - 240 - 22);
 //四号车
-        double dx = -(circle_center_point.x - 320 - 3);
+        double dx = -(circle_center_point.x - 320 - 7);
-        double dy = -(circle_center_point.y - 240 - 74);
+        double dy = -(circle_center_point.y - 240 - 64);
         center_delta_yaw = static_cast<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);
         center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI);
         if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) {

energy/src/energy/judge/judge_shoot.cpp

@@ -29,7 +29,7 @@ void Energy::judgeShootInWorld(){
 // 此函数用于判断云台坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::judgeShootInGimbal(){
-    if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) {
+    if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) {
         shoot = 4;
         is_predicting = false;
         is_guessing = true;
@@ -43,6 +43,6 @@ void Energy::judgeShootInGimbal(){
     static float sum_pitch = 0;
     sum_yaw += yaw_rotation;
     sum_pitch += pitch_rotation;
-    yaw_rotation = 2.5 * yaw_rotation + 0.1 * sum_yaw;
+    yaw_rotation = 6 * yaw_rotation + 0.1 * sum_yaw;
-    pitch_rotation = 2.5 * pitch_rotation + 0.1 * sum_pitch;
+    pitch_rotation = 6 * pitch_rotation + 0.1 * sum_pitch;
 }

energy/src/energy/run.cpp

@@ -31,7 +31,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
         if (!findCenterR(gimbal_src))return;
         if (show_energy)showCenterR("R", gimbal_src);
 
-        if (!getOrigin())return;
+//        if (!getOrigin())return;
         startChassis();
         initEnergy();
         destroyAllWindows();
@@ -60,21 +60,21 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
             initRotation();
             return;
         }
-        if (is_predicting) {
+//        if (is_predicting) {
-            getPredictPoint(target_point);
+//            getPredictPoint(target_point);
-            gimbalRotation();
+//            gimbalRotation();
-            judgeShootInWorld();
+//            judgeShootInWorld();
-            sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+//            sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-        } else if (is_guessing && stayGuessing()) {
+//        } else if (is_guessing && stayGuessing()) {
-            findFans(chassis_src);
+//            findFans(chassis_src);
-            if (show_energy)showFans("fans", chassis_src);
+//            if (show_energy)showFans("fans", chassis_src);
-            if (save_mark)writeDownMark();
+//            if (save_mark)writeDownMark();
-            if (!guessTarget()) return;
+//            if (!guessTarget()) return;
-            if (show_energy)showGuessTarget("guess", chassis_src);
+//            if (show_energy)showGuessTarget("guess", chassis_src);
-            getPredictPoint(guess_point);
+//            getPredictPoint(guess_point);
-            gimbalRotation();
+//            gimbalRotation();
-            sendTarget(serial, yaw_rotation, pitch_rotation, 5);
+//            sendTarget(serial, yaw_rotation, pitch_rotation, 5);
-        }
+//        }
     }
 }
 
@@ -101,11 +101,12 @@ void Energy::runBig(cv::Mat &gimbal_src) {
     if (show_energy)showCenterR("R", gimbal_src);
     changeTarget();
     getTargetPolarAngle();
+//    cout << target_point << '\t' << target_polar_angle << endl;
 //    cout << circle_center_point<< endl;
 
 //    judgeMode();
 //    if (energy_mode_init)return;
-//    if (!getOrigin())return;
+    if (!getOrigin())return;
     if (energy_rotation_init) {
         initRotation();
         return;
@@ -127,23 +128,6 @@ void Energy::runBig(cv::Mat &gimbal_src) {
         getAimPoint(predict_point);
         sendTarget(serial, yaw_rotation, pitch_rotation, 5);
     }
-
-    if (is_predicting) {
-        getPredictPoint(target_point);
-        gimbalRotation();
-        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);
-        gimbalRotation();
-        sendTarget(serial, yaw_rotation, pitch_rotation, 5);
-    }
-
 }
 
 
@@ -163,7 +147,7 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
     if (show_energy)showFlowStripFan("strip", gimbal_src);
 
     changeTarget();
-    cout << "target point: " << target_point << endl;
+//    cout << "target point: " << target_point << endl;
 
     if (is_predicting) {
         getAimPoint(target_point);

others/src/additions/additions.cpp

@@ -37,8 +37,9 @@ 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);
+//            LOGM("Get yaw: %f, pitch: %f!ake
+//            ", mcuData.curr_yaw, mcuData.curr_pitch);
 //            static int t = time(nullptr);
 //            static int cnt = 0;
 //            if(time(nullptr) > t){