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

energy change

Browse Source
This commit is contained in:
sun
2019-07-23 22:26:21 +08:00
parent d04a45cdef
commit 4a012f0fc4
5 changed files with 24 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
energy/src/energy/change/target_change.cpp
View File

@@ -7,8 +7,6 @@ using namespace std;
using namespace cv; using namespace cv;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断目标是否切换 // 此函数用于判断目标是否切换
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -19,4 +17,5 @@ void Energy::changeTarget(){
change_target = true; change_target = true;
} }
last_target_point = target_point; last_target_point = target_point;
// cout << change_target << endl;
} }

3
energy/src/energy/clear/energy_init.cpp
View File

@@ -141,6 +141,9 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10; gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10;
gimbal_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40; gimbal_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40;
gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
gimbal_energy_part_param_.TWIN_POINT_MAX = 20;
chassis_energy_part_param_.GRAY_THRESH = 120;//home chassis_energy_part_param_.GRAY_THRESH = 120;//home

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

@@ -20,6 +20,6 @@ void Energy::getAimPoint(cv::Point target_point) {
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
// cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl; cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl;
} }

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

@@ -16,10 +16,10 @@ using namespace cv;
void Energy::judgeShootInWorld() { void Energy::judgeShootInWorld() {
if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) { if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) {
shoot = 4; shoot = 4;
is_predicting = false; // is_predicting = false;
is_guessing = true; // is_guessing = true;
start_guess = true; // start_guess = true;
gettimeofday(&time_start_guess, NULL); // gettimeofday(&time_start_guess, NULL);
LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else } else
shoot = 2; shoot = 2;
@@ -30,15 +30,15 @@ void Energy::judgeShootInWorld() {
// 此函数用于判断云台坐标系下是否可以发弹 // 此函数用于判断云台坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInGimbal() { void Energy::judgeShootInGimbal() {
if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) { if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) {
shoot = 3; shoot = 4;
is_predicting = false; // is_predicting = false;
is_guessing = true; // is_guessing = true;
start_guess = true; // start_guess = true;
gettimeofday(&time_start_guess, NULL); // gettimeofday(&time_start_guess, NULL);
// LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); // LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else } else
shoot = 1; shoot = 2;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
} }

12
energy/src/energy/send/send.cpp
View File

@@ -22,17 +22,17 @@ void Energy::sendEnergy() {
} else if (is_chassis) { } else if (is_chassis) {
sum_yaw += yaw_rotation - mcuData.curr_yaw; sum_yaw += yaw_rotation - mcuData.curr_yaw;
sum_pitch += pitch_rotation - mcuData.curr_pitch; sum_pitch += pitch_rotation - mcuData.curr_pitch;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch;
} }
} }
if (change_target) { if (change_target) {
sendTarget(serial, yaw_rotation, pitch_rotation, 5); sendTarget(serial, yaw_rotation, pitch_rotation, 5);
} else if (is_predicting) { } else if (is_guessing) {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else {
sendTarget(serial, yaw_rotation, pitch_rotation, 6); sendTarget(serial, yaw_rotation, pitch_rotation, 6);
} else {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} }
} }
@@ -51,7 +51,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
time_t t = time(nullptr); time_t t = time(nullptr);
if (last_time != t) { if (last_time != t) {
last_time = t; last_time = t;
cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; // cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
fps = 0; fps = 0;
} }
fps += 1; fps += 1;