diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 4364048..c15d61d 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/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 dy = -(target_point.y - 240 - 74); +// double dx = -(target_point.x - 320 - 10); +// double dy = -(target_point.y - 240 - 22); //四号车 -// double dx = -(target_point.x - 320 + 16); -// double dy = -(target_point.y - 240 - 82); + double dx = -(target_point.x - 320 - 0); + 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; } diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp index 06f1840..415fbc9 100644 --- a/energy/src/energy/get/origin_get.cpp +++ b/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 dy = -(circle_center_point.y - 240 - 74); + double dx = -(circle_center_point.x - 320 - 7); + double dy = -(circle_center_point.y - 240 - 64); center_delta_yaw = static_cast(atan(dx / FOCUS_PIXAL) * 180 / PI); center_delta_pitch = static_cast(atan(dy / FOCUS_PIXAL) * 180 / PI); if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) { diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index c4a50bd..1407479 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/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; - pitch_rotation = 2.5 * pitch_rotation + 0.1 * sum_pitch; + yaw_rotation = 6 * yaw_rotation + 0.1 * sum_yaw; + pitch_rotation = 6 * pitch_rotation + 0.1 * sum_pitch; } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 7195b43..355db0a 100644 --- a/energy/src/energy/run.cpp +++ b/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) { - getPredictPoint(target_point); - gimbalRotation(); - judgeShootInWorld(); - sendTarget(serial, yaw_rotation, pitch_rotation, shoot); - } else if (is_guessing && stayGuessing()) { - findFans(chassis_src); - if (show_energy)showFans("fans", chassis_src); - if (save_mark)writeDownMark(); - if (!guessTarget()) return; - if (show_energy)showGuessTarget("guess", chassis_src); - getPredictPoint(guess_point); - gimbalRotation(); - 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(chassis_src); +// if (show_energy)showFans("fans", chassis_src); +// if (save_mark)writeDownMark(); +// if (!guessTarget()) return; +// if (show_energy)showGuessTarget("guess", chassis_src); +// getPredictPoint(guess_point); +// gimbalRotation(); +// 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);