diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index d382300..68aea4b 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -29,6 +29,7 @@ void Energy::getAimPoint(cv::Point target_point_) { extra_delta_y = 0; } + double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcu_data.delta_x - manual_delta_x - extra_delta_x); double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcu_data.delta_y - manual_delta_y - extra_delta_y); yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 9f132e0..a0bc804 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -15,56 +15,55 @@ using namespace std; // --------------------------------------------------------------------------------------------------------------------- void Energy::sendEnergy() { if (is_big) { - if (camera_cnt == 1) { - sum_yaw += yaw_rotation; - sum_pitch += pitch_rotation; - if (ROBOT_ID == 4 || ROBOT_ID == 8) { - MINMAX(sum_yaw, -100, 100); - MINMAX(sum_yaw, -100, 100); - } else if (ROBOT_ID == 3 || 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(pitch_I_component, -3, 3); - MINMAX(yaw_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 << "yaw: "<= 4) { - sendTarget(serial, yaw_rotation, pitch_rotation, 7, 0); - }*/ else { + } else { sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0); } diff --git a/main.cpp b/main.cpp index 6675d6a..e008b3c 100644 --- a/main.cpp +++ b/main.cpp @@ -33,7 +33,7 @@ McuData mcu_data = { // 单片机端回传结构体 BIG_ENERGY_STATE, // 当前状态,自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 - ENEMY_BLUE, // 敌方颜色 + ENEMY_RED, // 敌方颜色 0, // 能量机关x轴补偿量 0, // 能量机关y轴补偿量 }; @@ -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("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi"); - video_chassis = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi"); + video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi"); + video_chassis = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully.");