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-17 23:56:45 +08:00
parent 3258c85888
commit c2a92fc755
9 changed files with 64 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
energy/include/energy/constant.h
View File

@@ -11,7 +11,8 @@ const int SRC_WIDTH = 320;
const int SRC_HEIGHT = 240; const int SRC_HEIGHT = 240;
const int CLOCKWISE = 1; const int CLOCKWISE = 1;
const int ANTICLOCKWISE = -1; const int ANTICLOCKWISE = -1;
const float ATTACK_DISTANCE = 718.0;//cm //const float ATTACK_DISTANCE = 718.0;//cm
const float ATTACK_DISTANCE = 750.0;//cm
const double ARMOR_CENTER_TO_CYCLE_CENTER = 75.0;//cm const double ARMOR_CENTER_TO_CYCLE_CENTER = 75.0;//cm

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

@@ -40,7 +40,7 @@ void Energy::initEnergy() {
last_target_polar_angle = -1000; last_target_polar_angle = -1000;
guess_polar_angle = -1000; guess_polar_angle = -1000;
last_base_angle = -1000; last_base_angle = -1000;
predict_rad = 30; predict_rad = 25;
attack_distance = ATTACK_DISTANCE; attack_distance = ATTACK_DISTANCE;
center_delta_yaw = 1000; center_delta_yaw = 1000;
center_delta_pitch = 1000; center_delta_pitch = 1000;
@@ -67,7 +67,7 @@ void Energy::initEnergy() {
// 此函数对能量机关参数进行初始化 // 此函数对能量机关参数进行初始化
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() { void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.GRAY_THRESH = 120;//home gimbal_energy_part_param_.GRAY_THRESH = 140;//home
// gimbal_energy_part_param_.GRAY_THRESH = 200;//official // gimbal_energy_part_param_.GRAY_THRESH = 200;//official
// gimbal_energy_part_param_.GRAY_THRESH = 225; // gimbal_energy_part_param_.GRAY_THRESH = 225;
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230;
@@ -108,17 +108,17 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7;
gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 17000; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 0; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 1000;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 50; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 50;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.65;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34;
gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.58; gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.65;
gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34; gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34;
// gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2;
// gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08;
@@ -131,7 +131,7 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12;
// gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; // gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 117; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 117;

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

@@ -12,8 +12,12 @@ using namespace cv;
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::getAimPoint(cv::Point target_point) { void Energy::getAimPoint(cv::Point target_point) {
//五号车
double dx = -(target_point.x - 320 - 10); double dx = -(target_point.x - 320 - 10);
double dy = -(target_point.y - 240 - 22); double dy = -(target_point.y - 240 - 22);
//四号车
// double dx = -(target_point.x - 320 + 16);
// double dy = -(target_point.y - 240 - 82);
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;
} }

5
energy/src/energy/judge/judge_contour.cpp
View File

@@ -143,6 +143,7 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &f
return false; return false;
//选区面积大小不合适 //选区面积大小不合适
} }
// cout << "area: " << cur_contour_area << endl;
RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour); RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour);
Size2f cur_size = cur_rect.size; Size2f cur_size = cur_rect.size;
float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长 float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长
@@ -152,10 +153,10 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &f
|| length > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX || length > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX
|| width > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX) { || width > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX) {
// cout<<"length width fail."<<endl; // cout<<"length width fail."<<endl;
// cout << "length: " << length << '\t' << "width: " << width << endl;
return false; return false;
//矩形边长不合适 //矩形边长不合适
} }
float length_width_ratio = length / width;//计算矩形长宽比 float length_width_ratio = length / width;//计算矩形长宽比
if (length_width_ratio > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX || if (length_width_ratio > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN) { length_width_ratio < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN) {
@@ -166,6 +167,7 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &f
// cout << cur_contour_area / cur_size.area() << endl; // cout << cur_contour_area / cur_size.area() << endl;
if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN
|| cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) { || cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) {
// cout << "rate: " << cur_contour_area / cur_size.area() << endl;
return false; return false;
} }
// 轮廓对矩形的面积占有率不合适 // 轮廓对矩形的面积占有率不合适
@@ -205,6 +207,7 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
return false; return false;
//矩形边长不合适 //矩形边长不合适
} }
// cout << length << '\t' << width << endl;
float length_width_ratio = length / width;//计算矩形长宽比 float length_width_ratio = length / width;//计算矩形长宽比
if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX || if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) { length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) {

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

@@ -10,10 +10,10 @@ using namespace cv;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断云台坐标系下是否可以发弹 // 此函数用于判断世界坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInGimbal(){ void Energy::judgeShootInWorld(){
if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) { if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.8) {
shoot = 4; shoot = 4;
is_predicting = false; is_predicting = false;
is_guessing = true; is_guessing = true;
@@ -26,9 +26,9 @@ void Energy::judgeShootInGimbal(){
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断世界坐标系下是否可以发弹 // 此函数用于判断云台坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInWorld(){ void Energy::judgeShootInGimbal(){
if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) { if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) {
shoot = 4; shoot = 4;
is_predicting = false; is_predicting = false;
@@ -38,4 +38,11 @@ void Energy::judgeShootInWorld(){
LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else } else
shoot = 2; shoot = 2;
static float sum_yaw = 0;
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;
} }

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

@@ -17,5 +17,5 @@ bool Energy::isGuessingTimeout() {
timeval cur_time; timeval cur_time;
gettimeofday(&cur_time, NULL); gettimeofday(&cur_time, NULL);
return (cur_time.tv_sec - time_start_guess.tv_sec) * 1000.0 + return (cur_time.tv_sec - time_start_guess.tv_sec) * 1000.0 +
(cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 500; (cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 1000;
}; };

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

@@ -63,7 +63,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (is_predicting) { if (is_predicting) {
getPredictPoint(target_point); getPredictPoint(target_point);
gimbalRotation(); gimbalRotation();
judgeShootInGimbal(); judgeShootInWorld();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot); sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (is_guessing && stayGuessing()) { } else if (is_guessing && stayGuessing()) {
findFans(chassis_src); findFans(chassis_src);
@@ -104,14 +104,17 @@ void Energy::runBig(cv::Mat &gimbal_src) {
// judgeMode(); // judgeMode();
// if (energy_mode_init)return; // if (energy_mode_init)return;
if (!getOrigin())return; if (!getOrigin())return;
if (is_big && energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
return; return;
} }
if (is_predicting) { if (is_predicting) {
getPredictPoint(target_point); getPredictPoint(target_point);
gimbalRotation(); getAimPoint(predict_point);
cout << yaw_rotation << '\t' << pitch_rotation << endl;
judgeShootInGimbal(); judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot); sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (is_guessing && stayGuessing()) { } else if (is_guessing && stayGuessing()) {
findFans(gimbal_src); findFans(gimbal_src);
@@ -120,10 +123,26 @@ void Energy::runBig(cv::Mat &gimbal_src) {
guessTarget(); guessTarget();
if (show_energy)showGuessTarget("guess", gimbal_src); if (show_energy)showGuessTarget("guess", gimbal_src);
getPredictPoint(guess_point); getPredictPoint(guess_point);
gimbalRotation(); getAimPoint(predict_point);
sendTarget(serial, yaw_rotation, pitch_rotation, 5); 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);
// }
} }
@@ -143,17 +162,11 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (show_energy)showFlowStripFan("strip", gimbal_src); if (show_energy)showFlowStripFan("strip", gimbal_src);
changeTarget(); changeTarget();
getTargetPolarAngle(); cout << "target point: " << target_point << endl;
if (!getOrigin())return;
getPredictPoint(target_point);
gimbalRotation();
getAimPoint(target_point);
if (is_predicting) { if (is_predicting) {
getPredictPoint(target_point);
getAimPoint(target_point); getAimPoint(target_point);
judgeShootInWorld(); judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot); sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (is_guessing && stayGuessing()) { } else if (is_guessing && stayGuessing()) {
findFans(gimbal_src); findFans(gimbal_src);
@@ -161,8 +174,7 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (save_mark)writeDownMark(); if (save_mark)writeDownMark();
guessTarget(); guessTarget();
if (show_energy)showGuessTarget("guess", gimbal_src); if (show_energy)showGuessTarget("guess", gimbal_src);
getPredictPoint(guess_point); getAimPoint(guess_point);
getAimPoint(target_point);
sendTarget(serial, yaw_rotation, pitch_rotation, 5); sendTarget(serial, yaw_rotation, pitch_rotation, 5);
} }

8
main.cpp
View File

@@ -64,8 +64,8 @@ int main(int argc, char *argv[]) {
video_gimbal = new CameraWrapper(0/*, "armor"*/); video_gimbal = new CameraWrapper(0/*, "armor"*/);
video_chassis = new CameraWrapper(1/*, "energy"*/); video_chassis = new CameraWrapper(1/*, "energy"*/);
} else { } else {
video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/148.avi"); video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi");
video_chassis = new VideoWrapper("/home/sun/项目/energy_video/148.avi"); video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi");
} }
if (video_gimbal->init()) { if (video_gimbal->init()) {
LOGM("video_gimbal source initialization successfully."); LOGM("video_gimbal source initialization successfully.");
@@ -91,7 +91,7 @@ int main(int argc, char *argv[]) {
bool ok = true; bool ok = true;
cout << "start running" << endl; cout << "start running" << endl;
do { do {
CNT_TIME("Total", { // CNT_TIME("Total", {
if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
destroyAllWindows(); destroyAllWindows();
@@ -145,7 +145,7 @@ int main(int argc, char *argv[]) {
}); });
} }
// cv::waitKey(1); // cv::waitKey(1);
}); // });
} while (ok); } while (ok);
delete video_gimbal; delete video_gimbal;
video_gimbal = nullptr; video_gimbal = nullptr;

6
others/src/additions/additions.cpp
View File

@@ -76,12 +76,12 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix) {
bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) { bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) {
if (!is_camera_0_connect) { if (!is_camera_0_connect) {
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(0, "armor"); video_gimbal = new CameraWrapper(0/*, "armor"*/);
is_camera_0_connect = video_gimbal->init(); is_camera_0_connect = video_gimbal->init();
} }
if (!is_camera_1_connect) { if (!is_camera_1_connect) {
delete video_chassis; delete video_chassis;
video_chassis = new CameraWrapper(1, "energy"); video_chassis = new CameraWrapper(1/*, "energy"*/);
is_camera_1_connect = video_chassis->init(); is_camera_1_connect = video_chassis->init();
} }
return is_camera_0_connect && is_camera_1_connect; return is_camera_0_connect && is_camera_1_connect;
@@ -90,7 +90,7 @@ bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) {
bool checkReconnect(bool is_camera_connect) { bool checkReconnect(bool is_camera_connect) {
if (!is_camera_connect) { if (!is_camera_connect) {
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(0, "armor"); video_gimbal = new CameraWrapper(0/*, "armor"*/);
is_camera_connect = video_gimbal->init(); is_camera_connect = video_gimbal->init();
} }
return is_camera_connect; return is_camera_connect;