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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
JiatongSun
2019-08-07 23:36:11 +08:00
parent 40b442983e 86b37b3f5e
commit b38c6177ab
4 changed files with 67 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -91,9 +91,9 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 55; gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2;
@@ -105,10 +105,10 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500; gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500;
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180; gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 16; gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 32; gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5; gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 20; gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;

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

@@ -29,6 +29,7 @@ void Energy::getAimPoint(cv::Point target_point_) {
extra_delta_y = 0; extra_delta_y = 0;
} }
double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcu_data.delta_x - manual_delta_x - extra_delta_x); 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 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; yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;

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

@@ -30,7 +30,7 @@ void Energy::judgeShootInWorld() {
// 此函数用于判断云台坐标系下是否可以发弹 // 此函数用于判断云台坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInGimbal() { void Energy::judgeShootInGimbal() {
if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) { if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) {
shoot = 4; shoot = 4;
// is_predicting = false; // is_predicting = false;
// is_guessing = true; // is_guessing = true;

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

@@ -15,67 +15,70 @@ using namespace std;
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::sendEnergy() { void Energy::sendEnergy() {
if (is_big) { if (is_big) {
if (camera_cnt == 1) { sum_yaw += yaw_rotation;
sum_yaw += yaw_rotation; sum_pitch += pitch_rotation;
sum_pitch += pitch_rotation; if (ROBOT_ID == 4) {
if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) { MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100); MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100); } else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
} else if (ROBOT_ID == 7) { float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw; float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch; MINMAX(yaw_I_component, -2, 2);
// MINMAX(yaw_I_component, -3, 3); MINMAX(pitch_I_component, -2, 2);
// MINMAX(pitch_I_component, -3, 3);
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) {
yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
// cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
// cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
if (ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
} else if (is_chassis) {
// sum_yaw += yaw_rotation - mcu_data.curr_yaw;
// sum_pitch += pitch_rotation - mcu_data.curr_pitch;
// double tmp_yaw = yaw_rotation;
// double tmp_pitch = pitch_rotation;
// yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw;
// pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch;
// last_yaw = tmp_yaw;
// last_pitch = tmp_pitch;
} }
} else if (is_small) {
double tmp_yaw = yaw_rotation; double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation; double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) { if (mcu_data.mark == 1) {
yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
pitch_rotation = TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else { } else {
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} }
if (ROBOT_ID == 7) {
// cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
// cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
} else if (is_small) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 4) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
float yaw_I_component = SMALL_YAW_AIM_KI * sum_yaw;
float pitch_I_component = SMALL_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) {
yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KI * sum_yaw +
TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation =
TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KI * sum_pitch +
TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KI * sum_yaw +
SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KI * sum_pitch +
SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
MINMAX(yaw_rotation, -6, 6); MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6); MINMAX(pitch_rotation, -6, 6);
} }
@@ -88,9 +91,7 @@ void Energy::sendEnergy() {
sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0); sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0);
} else if (is_guessing) { } else if (is_guessing) {
sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0); sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0);
} /*else if (fans_cnt >= 4) { } else {
sendTarget(serial, yaw_rotation, pitch_rotation, 7, 0);
}*/ else {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0); sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);
} }