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

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

@@ -7,16 +7,15 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断目标是否切换
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeTarget(){
void Energy::changeTarget() {
if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) {
change_target = false;
} else {
change_target = true;
}
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_.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

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

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

@@ -19,20 +19,20 @@ void Energy::sendEnergy() {
sum_pitch += pitch_rotation;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
} else if (is_chassis){
} else if (is_chassis) {
sum_yaw += yaw_rotation - mcuData.curr_yaw;
sum_pitch += pitch_rotation - mcuData.curr_pitch;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch;
}
}
if (change_target) {
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
} else if (is_predicting) {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else {
} else if (is_guessing) {
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);
if (last_time != t) {
last_time = t;
cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
// cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
fps = 0;
}
fps += 1;