VSAG/vision_sjtu_19
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 10:17:51 +08:00
parent 0814522e46
commit 6e87cfe4db
10 changed files with 139 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
energy/include/energy/energy.h
View File

@@ -29,6 +29,7 @@ public:
void run(cv::Mat &gimbal_src, cv::Mat &chassis_src);
void run(cv::Mat &gimbal_src);
void runSmall(cv::Mat &gimbal_src);
Serial &serial;//串口
void setEnergyInit();//设置能量机关初始化
@@ -51,7 +52,6 @@ private:
bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
bool manual_mark;//若操作手进行过手动标定，则为true
bool auto_mark;//云台完成自动对心，则置为true
bool shoot;//若为true，则要求主控板发弹
bool start_guess;//进入猜测状态的标志
bool change_target;//目标切换的标志
@@ -77,6 +77,7 @@ private:
float center_delta_yaw, center_delta_pitch;//对心时相差的角度
float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
float origin_yaw, origin_pitch;//初始的云台角度设定值
float shoot;//若为2，则要求主控板发弹
timeval time_start_guess;
@@ -158,8 +159,9 @@ private:
void getAllTargetArmorCenters();//记录所有目标装甲板中心坐标
void getRecentTargetArmorCenters();//记录近30帧目标装甲板中心坐标
void judgeShoot();//判断是否可以发弹
void JudgeMode();//判断大符还是小符
void judgeMode();//判断大符还是小符
void judgeShootInGimbal();//在云台坐标系中判断是否可以发弹
void judgeShootInWorld();//在世界坐标系中判断是否可以发弹
bool isGuessingTimeout();//判断猜测模式是否超时（没打中）
void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离

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

@@ -21,7 +21,6 @@ void Energy::initEnergy() {
energy_rotation_init = true;
manual_mark = false;
auto_mark = false;
shoot = false;
start_guess = false;
change_target = false;
@@ -41,14 +40,13 @@ void Energy::initEnergy() {
last_target_polar_angle = -1000;
guess_polar_angle = -1000;
last_base_angle = -1000;
predict_rad = 20;
predict_rad = 30;
attack_distance = ATTACK_DISTANCE;
center_delta_yaw = 1000;
center_delta_pitch = 1000;
yaw_rotation = 0;
pitch_rotation = 0;
origin_yaw = 0;
origin_pitch = 0;
shoot = 0;
circle_center_point = Point(0, 0);
target_point = Point(0, 0);
@@ -69,8 +67,8 @@ void Energy::initEnergy() {
// 此函数对能量机关参数进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() {
// gimbal_energy_part_param_.GRAY_THRESH = 120;//home
gimbal_energy_part_param_.GRAY_THRESH = 200;//official
gimbal_energy_part_param_.GRAY_THRESH = 120;//home
// gimbal_energy_part_param_.GRAY_THRESH = 200;//official
// gimbal_energy_part_param_.GRAY_THRESH = 225;
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230;
gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
@@ -142,8 +140,8 @@ void Energy::initEnergyPartParam() {
// chassis_energy_part_param_.GRAY_THRESH = 120;//home
chassis_energy_part_param_.GRAY_THRESH = 200;//official
chassis_energy_part_param_.GRAY_THRESH = 120;//home
// chassis_energy_part_param_.GRAY_THRESH = 200;//official
// chassis_energy_part_param_.GRAY_THRESH = 225;
chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
chassis_energy_part_param_.FAN_GRAY_THRESH = 75;
@@ -204,7 +202,7 @@ void Energy::initEnergyPartParam() {
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 55;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 95;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 25;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12;
// chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7;

1
energy/src/energy/find/energy_finder.cpp
View File

@@ -69,6 +69,7 @@ int Energy::findArmors(const cv::Mat src) {
ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
imshow("armor struct", src_bin);
// findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
// for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓

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

@@ -13,18 +13,20 @@ using namespace cv;
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::getOrigin() {
if (!auto_mark && !manual_mark) {
double dx = -(circle_center_point.x - 320);
double dy = -(circle_center_point.y - 240);
center_delta_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
center_delta_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
if (abs(center_delta_pitch) > 0.3 || abs(center_delta_pitch) > 0.3) {
sendTarget(serial, center_delta_yaw, center_delta_pitch, false);
double dx = -(circle_center_point.x - 320 - 10);
double dy = -(circle_center_point.y - 240 - 22);
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) {
// cout << "origin not get!" << endl;
// cout << center_delta_yaw << '\t' << center_delta_pitch << endl;
sendTarget(serial, center_delta_yaw, center_delta_pitch, 0);
return false;
} else {
origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch;
auto_mark = true;
sendTarget(serial, center_delta_yaw, center_delta_pitch, true);
sendTarget(serial, center_delta_yaw, center_delta_pitch, 1);
LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
return true;
}

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

@@ -12,7 +12,7 @@ using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断大小符
// ---------------------------------------------------------------------------------------------------------------------
void Energy::JudgeMode() {
void Energy::judgeMode() {
getRecentTargetArmorCenters();
if (recent_target_armor_centers.size() < 30) {
return;

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

@@ -9,18 +9,33 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断是否可以发弹
// 此函数用于判断云台坐标系下是否可以发弹
// ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShoot(){
void Energy::judgeShootInGimbal(){
if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
shoot = true;
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 = false;
shoot = 2;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断世界坐标系下是否可以发弹
// ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInWorld(){
if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) {
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 = 2;
}

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

@@ -16,12 +16,11 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (chassis_src.empty())
run(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if (is_gimbal) {
// energy_part_param_ = chassis_energy_part_param_;
energy_part_param_ = chassis_energy_part_param_;
energy_part_param_ = gimbal_energy_part_param_;
clearAll();
initImage(gimbal_src);
findFans(gimbal_src);
showFans("fan",gimbal_src);
// findFans(gimbal_src);
// showFans("fan",gimbal_src);
if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src);
@@ -37,7 +36,6 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
initEnergy();
destroyAllWindows();
} else if (is_chassis) {
// energy_part_param_ = chassis_energy_part_param_;
energy_part_param_ = chassis_energy_part_param_;
clearAll();
initImage(chassis_src);
@@ -47,16 +45,17 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (findArmors(chassis_src) < 1)return;
if (show_energy)showArmors("armor", chassis_src);
if (!findFlowStripFan(chassis_src)) return;
if (!findFlowStripFan(chassis_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findCenterROI(chassis_src))return;
if (show_energy)showFlowStripFan("strip", chassis_src);
if (!findCenterR(chassis_src))return;
if (show_energy)showCenterR("R", chassis_src);
getTargetPolarAngle();
changeTarget();
JudgeMode();
if (energy_mode_init)return;
// judgeMode();
// if (energy_mode_init)return;
if (is_big && energy_rotation_init) {
initRotation();
return;
@@ -64,7 +63,7 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (is_predicting) {
getPredictPoint(target_point);
gimbalRotation();
judgeShoot();
judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (is_guessing && stayGuessing()) {
findFans(chassis_src);
@@ -74,7 +73,7 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (show_energy)showGuessTarget("guess", chassis_src);
getPredictPoint(guess_point);
gimbalRotation();
sendTarget(serial, yaw_rotation, pitch_rotation, false);
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
}
}
}
@@ -102,8 +101,8 @@ void Energy::run(cv::Mat &gimbal_src) {
if (show_energy)showCenterR("R", gimbal_src);
changeTarget();
getTargetPolarAngle();
JudgeMode();
if (energy_mode_init)return;
// judgeMode();
// if (energy_mode_init)return;
if (!getOrigin())return;
if (is_big && energy_rotation_init) {
initRotation();
@@ -112,7 +111,7 @@ void Energy::run(cv::Mat &gimbal_src) {
if (is_predicting) {
getPredictPoint(target_point);
gimbalRotation();
judgeShoot();
judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (is_guessing && stayGuessing()) {
findFans(gimbal_src);
@@ -122,6 +121,71 @@ void Energy::run(cv::Mat &gimbal_src) {
if (show_energy)showGuessTarget("guess", gimbal_src);
getPredictPoint(guess_point);
gimbalRotation();
sendTarget(serial, yaw_rotation, pitch_rotation, false);
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
void Energy::runSmall(cv::Mat &gimbal_src){
energy_part_param_ = gimbal_energy_part_param_;
clearAll();
initImage(gimbal_src);
if (show_process)imshow("bin", gimbal_src);
if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
if (show_energy)showFlowStripFan("strip", gimbal_src);
changeTarget();
getTargetPolarAngle();
if (!getOrigin())return;
getPredictPoint(target_point);
gimbalRotation();
getAimPoint(target_point);
if (is_predicting) {
getPredictPoint(target_point);
getAimPoint(target_point);
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);
getAimPoint(target_point);
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
}
}
//getPredictPoint(target_point);
//gimbalRotation();
//static bool k = false;
//cout<<"delta yaw: "<<abs(yaw_rotation - mcuData.curr_yaw)<<endl;
//cout<<"delta pitch: "<<abs(pitch_rotation - mcuData.curr_pitch)<<endl;
//cout << "origin_yaw: " << origin_yaw << '\t' << "origin_pitch: " << origin_pitch << endl;
//cout << "predict point: " << predict_point << endl;
//
//if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && abs(pitch_rotation - mcuData.curr_pitch) < 0.6) {
//shoot = 4;
//if (!k) {
//sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
//cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
//k = false;
//}
//waitKey(400);
//} else {
//shoot = 2;
//sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
//}

1
energy/src/energy/tool/tool.cpp
View File

@@ -43,7 +43,6 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2) {
double x_0 = point_1.x - circle_center_point.x;
double y_0 = point_1.y - circle_center_point.y;
double r_0 = sqrt(pow(x_0, 2) + pow(y_0, 2));
point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
}