From acfc1e8701e0e8931dc9b077f4694070f4091b6c Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 19:33:00 +0800 Subject: [PATCH] energy changed --- energy/src/energy/run.cpp | 2 ++ energy/src/energy/send/send.cpp | 31 ++++++++++++++++--------------- main.cpp | 2 +- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 9cf3006..0e606f7 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -107,6 +107,8 @@ void Energy::runBig(cv::Mat &gimbal_src) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); + cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl; + waitKey(0); sendEnergy(); } diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index f310129..8da11de 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -18,35 +18,36 @@ void Energy::sendEnergy() { if (camera_cnt == 1) { sum_yaw += yaw_rotation; sum_pitch += pitch_rotation; - MINMAX(sum_yaw, -100, 100); - MINMAX(sum_pitch, -100, 100); + float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw; + float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch; + 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); pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch + BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); - MINMAX(yaw_rotation, -10, 10); + MINMAX(yaw_rotation, -6, 6); + MINMAX(pitch_rotation, -6, 6); last_yaw = tmp_yaw; last_pitch = tmp_pitch; } else if (is_chassis) { - sum_yaw += yaw_rotation - mcu_data.curr_yaw; - sum_pitch += pitch_rotation - mcu_data.curr_pitch; - double tmp_yaw = yaw_rotation; - double tmp_pitch = pitch_rotation; - yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw; - pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch; - last_yaw = tmp_yaw; - last_pitch = tmp_pitch; +// sum_yaw += yaw_rotation - mcu_data.curr_yaw; +// sum_pitch += pitch_rotation - mcu_data.curr_pitch; +// double tmp_yaw = yaw_rotation; +// double tmp_pitch = pitch_rotation; +// yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw; +// pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch; +// last_yaw = tmp_yaw; +// last_pitch = tmp_pitch; } } else if (is_small){ - sum_yaw += yaw_rotation; - sum_pitch += pitch_rotation; - MINMAX(sum_yaw, -100, 100); - MINMAX(sum_pitch, -100, 100); 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); + MINMAX(pitch_rotation, -6, 6); last_yaw = tmp_yaw; last_pitch = tmp_pitch; } diff --git a/main.cpp b/main.cpp index a5e31d2..bef8e67 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色