diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 60e7b2d..5825cff 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -80,7 +80,7 @@ void Energy::setSmallEnergyInit() { FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r"); if (fp) { - fscanf(fp, "%d %d", &manual_delta_x, &manual_delta_y); + fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y); fclose(fp); } } \ No newline at end of file diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 68aea4b..cb551cc 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -20,18 +20,26 @@ void Energy::getAimPoint(cv::Point target_point_) { if (target_polar_angle_ > 0 && target_polar_angle_ <= 90) { extra_delta_x = EXTRA_DELTA_X * (90 - target_polar_angle_) / 90; - extra_delta_y = - EXTRA_DELTA_Y * target_polar_angle_ / 90; + extra_delta_y = -EXTRA_DELTA_Y * target_polar_angle_ / 90; } else if (target_polar_angle_ > 90 && target_polar_angle_ < 180) { - extra_delta_x = - EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90; - extra_delta_y = - EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90; + extra_delta_x = -EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90; + extra_delta_y = -EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90; } else { extra_delta_x = 0; extra_delta_y = 0; } + int compensate_yaw = 0, compensate_pitch = 0; + if (mcu_data.enemy_color == ENEMY_BLUE) { + compensate_yaw = RED_COMPENSATE_YAW; + compensate_pitch = RED_COMPENSATE_PITCH; + } else if (mcu_data.enemy_color == ENEMY_RED) { + compensate_yaw = BLUE_COMPENSATE_YAW; + compensate_pitch = BLUE_COMPENSATE_PITCH; + } - 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); + 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; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; diff --git a/energy/src/energy/mark/mark.cpp b/energy/src/energy/mark/mark.cpp index 355cf2f..b851fe2 100644 --- a/energy/src/energy/mark/mark.cpp +++ b/energy/src/energy/mark/mark.cpp @@ -22,27 +22,25 @@ void Energy::writeDownSlightChange(cv::Mat &src) { fclose(fp_delta); } } - if(fans_cnt>=1){ - FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a"); - if (fp_data) { - if(mcu_data.mark == 1){ - fprintf(fp_data, "PID: %s\t", "new"); - } else { - fprintf(fp_data, "PID: %s\t", "default"); - } - if(is_big){ - fprintf(fp_data, "state: %s\t", "big"); - } else if(is_small){ - fprintf(fp_data, "state: %s\t", "small"); - } - fprintf(fp_data, "fps: %d\t", curr_fps); - fprintf(fp_data, "fans_cnt: %d\t", fans_cnt); - fprintf(fp_data, "shoot: %d\t", int(shoot)); - fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation); - fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, - mcu_data.delta_y + manual_delta_y); - fclose(fp_data); + FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a"); + if (fp_data) { + if(mcu_data.mark == 1){ + fprintf(fp_data, "PID: %s\t", "new"); + } else { + fprintf(fp_data, "PID: %s\t", "default"); } + if(is_big){ + fprintf(fp_data, "state: %s\t", "big"); + } else if(is_small){ + fprintf(fp_data, "state: %s\t", "small"); + } + fprintf(fp_data, "fps: %d\t", curr_fps); + fprintf(fp_data, "fans_cnt: %d\t", fans_cnt); + fprintf(fp_data, "shoot: %d\t", int(shoot)); + fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation); + fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, + mcu_data.delta_y + manual_delta_y); + fclose(fp_data); } } diff --git a/main.cpp b/main.cpp index bcead53..24afb72 100644 --- a/main.cpp +++ b/main.cpp @@ -30,10 +30,10 @@ using namespace std; McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态,自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态,自瞄-大符-小符 0, // 云台角度标记位 0, // 是否为反陀螺模式 - ENEMY_BLUE, // 敌方颜色 + ENEMY_RED, // 敌方颜色 0, // 能量机关x轴补偿量 0, // 能量机关y轴补偿量 }; @@ -66,7 +66,7 @@ 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(PROJECT_DIR"/test_video/red_small.avi"); + video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/blue_small.avi"); video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/red_small.avi"); } if (video_gimbal->init()) {