VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

energy changed

Browse Source
This commit is contained in:
xinyang
2019-07-18 23:34:29 +08:00
parent e1c7e04138
commit 282d69c220
5 changed files with 32 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
energy/src/energy/get/aim_point_get.cpp
View File

@@ -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;
}

4
energy/src/energy/get/origin_get.cpp
View File

@@ -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<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);
center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI);
if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) {

6
energy/src/energy/judge/judge_shoot.cpp
View File

@@ -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;
}

54
energy/src/energy/run.cpp
View File

@@ -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);