From 4a012f0fc42a279880e38c4b8f77e53affe73bbe Mon Sep 17 00:00:00 2001 From: sun Date: Tue, 23 Jul 2019 22:26:21 +0800 Subject: [PATCH] energy change --- energy/src/energy/change/target_change.cpp | 5 ++--- energy/src/energy/clear/energy_init.cpp | 3 +++ energy/src/energy/get/aim_point_get.cpp | 2 +- energy/src/energy/judge/judge_shoot.cpp | 22 +++++++++++----------- energy/src/energy/send/send.cpp | 14 +++++++------- 5 files changed, 24 insertions(+), 22 deletions(-) diff --git a/energy/src/energy/change/target_change.cpp b/energy/src/energy/change/target_change.cpp index 46d95e6..8825b84 100644 --- a/energy/src/energy/change/target_change.cpp +++ b/energy/src/energy/change/target_change.cpp @@ -7,16 +7,15 @@ using namespace std; using namespace cv; - - //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断目标是否切换 // --------------------------------------------------------------------------------------------------------------------- -void Energy::changeTarget(){ +void Energy::changeTarget() { if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) { change_target = false; } else { change_target = true; } last_target_point = target_point; +// cout << change_target << endl; } \ No newline at end of file diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 1db2106..753aeff 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -141,6 +141,9 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10; gimbal_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40; + gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20; + gimbal_energy_part_param_.TWIN_POINT_MAX = 20; + chassis_energy_part_param_.GRAY_THRESH = 120;//home diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 32eeffa..e62bbbb 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -20,6 +20,6 @@ void Energy::getAimPoint(cv::Point target_point) { 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; -// cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl; + cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl; } diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index 10c765a..cbc2660 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -16,10 +16,10 @@ using namespace cv; void Energy::judgeShootInWorld() { if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) { shoot = 4; - is_predicting = false; - is_guessing = true; - start_guess = true; - gettimeofday(&time_start_guess, NULL); +// 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; @@ -30,15 +30,15 @@ void Energy::judgeShootInWorld() { // 此函数用于判断云台坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- void Energy::judgeShootInGimbal() { - if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) { - shoot = 3; - is_predicting = false; - is_guessing = true; - start_guess = true; - gettimeofday(&time_start_guess, NULL); + if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) { + 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 = 1; + shoot = 2; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; } diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index c395477..9d2a609 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -19,20 +19,20 @@ void Energy::sendEnergy() { sum_pitch += pitch_rotation; yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; - } else if (is_chassis){ + } else if (is_chassis) { sum_yaw += yaw_rotation - mcuData.curr_yaw; sum_pitch += pitch_rotation - mcuData.curr_pitch; - yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; - pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; + yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw; + pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch; } } if (change_target) { sendTarget(serial, yaw_rotation, pitch_rotation, 5); - } else if (is_predicting) { - sendTarget(serial, yaw_rotation, pitch_rotation, shoot); - } else { + } else if (is_guessing) { sendTarget(serial, yaw_rotation, pitch_rotation, 6); + } else { + sendTarget(serial, yaw_rotation, pitch_rotation, shoot); } } @@ -51,7 +51,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) { time_t t = time(nullptr); if (last_time != t) { last_time = t; - cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; +// cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; fps = 0; } fps += 1;