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

energy changed

Browse Source
This commit is contained in:
sun
2019-07-15 15:43:44 +08:00
parent f8c3eb19fd
commit 39f2cd6818
17 changed files with 162 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
energy/include/energy/energy.h
View File

@@ -28,7 +28,6 @@ public:
~Energy();//默认析构函数
void run(cv::Mat &gimbal_src, cv::Mat &chassis_src);
void run(cv::Mat &gimbal_src);
Serial &serial;//串口
@@ -41,19 +40,20 @@ private:
EnergyPartParam gimbal_energy_part_param_;//云台摄像头能量机关的参数设置
EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置
bool isMark;//若操作手正在手动标定，则为true
bool isBig;//大符模式为true
bool isSmall;//小符模式为true
bool isGimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
bool is_mark;//若操作手正在手动标定，则为true
bool is_big;//大符模式为true
bool is_small;//小符模式为true
bool is_gimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
bool is_chassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
bool energy_mode_init;//正在进行大小符判断
bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
bool manual_mark;//若操作手进行过手动标定，则为true
bool auto_mark;//云台完成自动对心，则置为true
bool shoot;//若为true，则要求主控板发弹
bool startguessing;//进入猜测状态的标志
bool start_guess;//进入猜测状态的标志
bool change_target;//目标切换的标志
uint8_t last_mark;//用于记录上一帧操作手是否进行标定
uint8_t &ally_color;//我方颜色
@@ -80,13 +80,6 @@ private:
timeval time_start_guess;
std::vector<cv::RotatedRect> fans;//图像中所有扇叶
std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别)
std::vector<cv::Point> all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
cv::RotatedRect centerR;//风车中心字母R的可能候选区
cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
cv::RotatedRect flow_strip_fan;//图像中所有流动条所在扇叶（理论上只有一个）
@@ -101,6 +94,15 @@ private:
cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道
std::vector<cv::RotatedRect> fans;//图像中所有扇叶
std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别)
std::vector<cv::Point> all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
void initEnergy();//能量机关初始化
void initEnergyPartParam();//能量机关参数初始化
void initRotation();//顺逆时针初始化
@@ -146,7 +148,7 @@ private:
void writeDownMark();//记录操作手标定的云台初始角度
bool guessTarget();//获得猜测击打点位
bool changeTarget();//判断目标是否改变
void changeTarget();//判断目标是否改变
bool getOrigin();//获得云台对心所需角度
void getTargetPolarAngle();//获得目标装甲板极坐标角度
void getPredictPoint(cv::Point target_point);//获取预测点位

27
energy/src/energy/calibrate/structing.cpp
View File

@@ -34,8 +34,6 @@ void Energy::FanStruct(cv::Mat &src) {
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
}
@@ -44,19 +42,26 @@ void Energy::FanStruct(cv::Mat &src) {
// 此函数对图像进行腐蚀与膨胀操作
// ---------------------------------------------------------------------------------------------------------------------
void Energy::ArmorStruct(cv::Mat &src) {
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(8, 6));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1));
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2));
Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
erode(src,src, element_erode_1);
// imshow("erode_1", src);
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
erode(src,src, element_erode_1);
// imshow("erode_1", src);
erode(src,src, element_erode_2);
// imshow("erode_2", src);
erode(src,src, element_erode_3);
// imshow("erode_3", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
}
@@ -84,8 +89,6 @@ void Energy::FlowStripFanStruct(cv::Mat &src) {
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
}
@@ -113,8 +116,6 @@ void Energy::FlowStripStruct(cv::Mat &src) {
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
}

11
energy/src/energy/change/mode_change.cpp
View File

@@ -13,8 +13,9 @@ using namespace cv;
// 此函数用于从云台摄像头对心状态切换到底盘摄像头击打状态
// ---------------------------------------------------------------------------------------------------------------------
void Energy::startChassis(){
isChassis = true;
isGimbal = false;
is_chassis = true;
is_gimbal = false;
LOGM(STR_CTR(WORD_YELLOW, "Start Chassis!"));
}
@@ -23,9 +24,9 @@ void Energy::startChassis(){
// 此函数用于判断是否应当继续保持猜测模式
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::stayGuessing(){
if (changeTarget() || isGuessingTimeout()) {
isPredicting = true;
isGuessing = false;
if (change_target || isGuessingTimeout()) {
is_predicting = true;
is_guessing = false;
LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
return false;
}

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

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

2
energy/src/energy/init/clear.cpp → energy/src/energy/clear/clear.cpp
View File

@@ -28,4 +28,4 @@ void Energy::initImage(cv::Mat &src){
if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
if (show_process)imshow("bin", src);
}
}

82
energy/src/energy/init/energy_init.cpp → energy/src/energy/clear/energy_init.cpp
View File

@@ -14,43 +14,54 @@ using std::vector;
// 此函数对能量机关成员变量进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergy() {
isMark = false;
isPredicting = true;
isGuessing = false;
is_mark = false;
is_guessing = false;
is_predicting = true;
energy_mode_init = true;
energy_rotation_init = true;
manual_mark = false;
auto_mark = false;
energy_mode_init = true;
shoot = false;
start_guess = false;
change_target = false;
last_mark = 0;
radius = 0;
send_cnt = 0;
last_fans_cnt = 0;
guess_devide = 0;
energy_rotation_direction = ANTICLOCKWISE;
clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0;
last_mode = -1;//既不是大符也不是小符
target_polar_angle = -1000;
last_target_polar_angle = -1000;
guess_polar_angle = -1000;
last_base_angle = -1000;
predict_rad = 20;
attack_distance = ATTACK_DISTANCE;
center_delta_yaw = 1000;
center_delta_pitch = 1000;
yaw_rotation = 0;
pitch_rotation = 0;
origin_yaw = 0;
origin_pitch = 0;
circle_center_point = Point(0, 0);
target_point = Point(0, 0);
last_target_point = Point(0, 0);
guess_point = Point(0, 0);
predict_point = Point(0, 0);
target_polar_angle = -1000;
guess_polar_angle = -1000;
last_target_polar_angle = -1000;
radius = 0;
energy_rotation_direction = ANTICLOCKWISE;
attack_distance = ATTACK_DISTANCE;
last_mode = -1;//既不是大符也不是小符
last_fans_cnt = 0;
send_cnt = 0;
yaw_rotation = 0;
pitch_rotation = 0;
last_mark = 0;
shoot = false;
guess_devide = 0;
startguessing = false;
predict_rad = 20;
fans.clear();
armors.clear();
all_target_armor_centers.clear();
while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0;
}
@@ -58,19 +69,21 @@ void Energy::initEnergy() {
// 此函数对能量机关参数进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.GRAY_THRESH = 80;
// gimbal_energy_part_param_.GRAY_THRESH = 80;
gimbal_energy_part_param_.GRAY_THRESH = 225;
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230;
gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 6600;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 56;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 88;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 21;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 40;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 80;
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_MAX = 50;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.65;
gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8;
gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48;
// gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3;
@@ -80,8 +93,8 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 16;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 32;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 7;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 25;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 20;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
@@ -128,7 +141,8 @@ void Energy::initEnergyPartParam() {
chassis_energy_part_param_.GRAY_THRESH = 80;
// chassis_energy_part_param_.GRAY_THRESH = 80;
chassis_energy_part_param_.GRAY_THRESH = 225;
chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
chassis_energy_part_param_.FAN_GRAY_THRESH = 75;
chassis_energy_part_param_.ARMOR_GRAY_THRESH = 80;
@@ -151,8 +165,8 @@ void Energy::initEnergyPartParam() {
chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15;
chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45;
chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 13;
chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 33;
chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
chassis_energy_part_param_.ARMOR_CONTOUR_AREA_RATIO_MIN = 0.7;

18
energy/src/energy/energy.cpp
View File

@@ -19,11 +19,10 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
initEnergy();
initEnergyPartParam();
energy_rotation_init = false;
isBig = false;
isSmall = false;
isGimbal = true;
isChassis = false;
is_big = false;
is_small = false;
is_gimbal = true;
is_chassis = false;
}
@@ -41,9 +40,8 @@ void Energy::setEnergyInit() {
initEnergy();
initEnergyPartParam();
energy_rotation_init = true;
isBig = false;
isSmall = false;
isGimbal = true;
isChassis = false;
is_big = false;
is_small = false;
is_gimbal = true;
is_chassis = false;
}

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

@@ -34,12 +34,17 @@ int Energy::findFans(const cv::Mat src) {
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if(length>20&&width>20){
// fans.emplace_back(cv::minAreaRect(fan_contour));
// cout<<"fan area: "<<length<<'\t'<<width<<endl;
// cout<<nonZeroRateOfRotateRect(src_bin,cur_rect)<<endl;
// }
// double cur_contour_area = contourArea(fan_contour);
// double non_zero_rate = nonZeroRateOfRotateRect(src_bin, cur_rect);
// if (length > 60 && width > 20) {
// fans.emplace_back(cv::minAreaRect(fan_contour));
// cout << cur_rect.center << endl;
// cout << "fan area: " << length << '\t' << width << endl;
// cout << "non zero: " << nonZeroRateOfRotateRect(src_bin, cur_rect) << endl;
// cout << "rate: " << cur_contour_area / cur_size.area() << endl;
// }
}
// showFans("fan", src_bin);
if (fans.size() < last_fans_cnt) {
last_fans_cnt = static_cast<int>(fans.size());
return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1
@@ -63,10 +68,10 @@ int Energy::findArmors(const cv::Mat src) {
std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("armor struct", src_bin);
imshow("armor struct", src_bin);
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
// findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
// for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
// {
@@ -85,12 +90,11 @@ int Energy::findArmors(const cv::Mat src) {
continue;
}
armors.emplace_back(cv::minAreaRect(armor_contour));
// RotatedRect cur_rect = minAreaRect(armor_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if (length > 10 && width > 7) {
// if (length > 5 && width > 5) {
// armors.emplace_back(cv::minAreaRect(armor_contour));
// cout << "armor area: " << length << '\t' << width << '\t' << cur_rect.center << endl;
// }
@@ -98,7 +102,9 @@ int Energy::findArmors(const cv::Mat src) {
}
// cout<<armors.size()<<endl;
// showArmors("armor",src);
if (armors.size() < 1)cout << "no armors!" << endl;
// if (armors.size() < 1)cout << "no armors!" << endl;
return static_cast<int>(armors.size());
}
@@ -165,7 +171,7 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
}
std::vector<vector<Point> > flow_strip_fan_contours;
FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("flow strip fan struct", src_bin);
imshow("flow strip fan struct", src_bin);
findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
@@ -208,7 +214,7 @@ bool Energy::findFlowStrip(const cv::Mat src) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
}
FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
imshow("flow strip struct", src_bin);
// imshow("flow strip struct", src_bin);
std::vector<vector<Point> > flow_strip_contours;
findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

6
energy/src/energy/get/gimble_rotation_get.cpp
View File

@@ -19,17 +19,17 @@ void Energy::changeMark() {
last_mark = mcuData.mark;
origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch;
isMark = false;
is_mark = false;
manual_mark = true;
// LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
} else if (mcuData.mark == 1) {//正在标定
last_mark = mcuData.mark;
isMark = true;
is_mark = true;
// LOGM(STR_CTR(WORD_BLUE,"Marking..."));
} else {//未在标定
last_mark = mcuData.mark;
isMark = false;
is_mark = false;
}
//cout<<"mark: "<<int(mcuData.mark)<<endl;
}

8
energy/src/energy/get/guess_point_get.cpp
View File

@@ -12,7 +12,7 @@ using namespace cv;
// 此函数获得猜测的击打点
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::guessTarget() {
vector<int> all_fan_angles;
vector<float> all_fan_angles;
float angle;
for (const auto &fan : fans) {
angle = static_cast<float>(180 / PI * atan2(-1 * (fan.center.y - circle_center_point.y),
@@ -33,7 +33,7 @@ bool Energy::guessTarget() {
float max_angle = all_fan_angles.at(all_fan_angles.size() - 1);
float base_angle = min_angle;
while (base_angle > 72)base_angle -= 72;
if (startguessing) {
if (start_guess) {
int i = 0;
for (i = 1; i < all_fan_angles.size(); ++i) {
if (abs(min_angle + 72 * i - all_fan_angles.at(i)) > energy_part_param_.TWIN_ANGEL_MAX) {
@@ -41,8 +41,8 @@ bool Energy::guessTarget() {
break;
}
}
if (i == all_fan_angles.size()) guess_polar_angle = max_angle + 72;
startguessing = false;
if (i == all_fan_angles.size()) guess_polar_angle = (max_angle + 72);
start_guess = false;
guess_devide = devide(guess_polar_angle);
} else if (abs(base_angle - last_base_angle) > 20) {
if (energy_rotation_direction == CLOCKWISE)guess_devide = (guess_devide + 4) % 5;

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

@@ -28,7 +28,7 @@ void Energy::getAllTargetArmorCenters() {
// 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getRecentTargetArmorCenters() {
if (changeTarget()) {
if (change_target) {
while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();//目标切换时清空
return;
}

5
energy/src/energy/get/predict_point_get.cpp
View File

@@ -15,10 +15,9 @@ using std::vector;
// 此函数获取预测点坐标
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getPredictPoint(cv::Point target_point) {
if (isBig) {
if (is_big) {
if (energy_rotation_direction == 1) rotate(target_point);
if (energy_rotation_direction == -1) rotate(target_point);
} else if (isSmall) predict_point = target_point;
} else if (is_small) predict_point = target_point;
}

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

@@ -19,16 +19,16 @@ void Energy::JudgeMode() {
} else {
if (abs(recent_target_armor_centers.back() - recent_target_armor_centers.front()) > 10) {
if(last_mode!=BIG){
isBig = true;
isSmall = false;
is_big = true;
is_small = false;
energy_mode_init = false;
LOGM(STR_CTR(WORD_CYAN,"start big!"));
}
last_mode = BIG;
} else {
if(last_mode!=SMALL){
isBig = false;
isSmall = true;
is_big = false;
is_small = true;
energy_mode_init = false;
LOGM(STR_CTR(WORD_GREEN,"start small!"));
}

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

@@ -16,9 +16,9 @@ using namespace cv;
void Energy::judgeShoot(){
if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
shoot = true;
isPredicting = false;
isGuessing = true;
startguessing = true;
is_predicting = false;
is_guessing = true;
start_guess = true;
gettimeofday(&time_start_guess, NULL);
LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else

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

@@ -15,12 +15,13 @@ using namespace cv;
void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (chassis_src.empty())
run(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if (isGimbal) {
energy_part_param_ = gimbal_energy_part_param_;
else if (is_gimbal) {
energy_part_param_ = chassis_energy_part_param_;
clearAll();
initImage(gimbal_src);
if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findCenterROI(gimbal_src))return;
@@ -32,36 +33,39 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
startChassis();
initEnergy();
destroyAllWindows();
} else if (isChassis) {
energy_part_param_ = gimbal_energy_part_param_;
} else if (is_chassis) {
energy_part_param_ = chassis_energy_part_param_;
clearAll();
initImage(chassis_src);
changeMark();
if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch
if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
if (findArmors(chassis_src) < 1)return;
if (!findFlowStripFan(chassis_src))return;showFlowStripFan("flow strip fan", chassis_src);
if (show_energy)showArmors("armor", chassis_src);
if (!findFlowStripFan(chassis_src))return;
showFlowStripFan("flow strip fan", chassis_src);
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;
if (isBig && energy_rotation_init) {
if (is_big && energy_rotation_init) {
initRotation();
return;
}
if (isPredicting) {
if (is_predicting) {
getPredictPoint(target_point);
gimbalRotation();
judgeShoot();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (isGuessing && stayGuessing()) {
} else if (is_guessing && stayGuessing()) {
findFans(chassis_src);
if (show_energy)showFans("fans", chassis_src);
if (save_mark)writeDownMark();
if (!guessTarget()) return;
if (show_energy)showGuessTarget("guess", chassis_src);
@@ -82,31 +86,34 @@ void Energy::run(cv::Mat &gimbal_src) {
initImage(gimbal_src);
changeMark();
if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch
if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
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 (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStripFan("strip", gimbal_src);
if (!findCenterR(gimbal_src))return;
if (show_energy)showCenterR("R", gimbal_src);
changeTarget();
getTargetPolarAngle();
JudgeMode();
if (energy_mode_init)return;
if (!getOrigin())return;
if (isBig & energy_rotation_init) {
if (is_big && energy_rotation_init) {
initRotation();
return;
}
if (isPredicting) {
if (is_predicting) {
getPredictPoint(target_point);
gimbalRotation();
judgeShoot();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (isGuessing && stayGuessing()) {
} 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);

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

@@ -68,9 +68,7 @@ double Energy::pointDistance(cv::Point point_1, cv::Point point_2) {
// ---------------------------------------------------------------------------------------------------------------------
int Energy::devide(float angle) {
if (angle < 0)angle += 360;//若angle小于0，说明当前角度范围为-180°~180°
int i = 0;
while (angle - 72 * i > 72)i++;
return i;
return angle / 72.0;
}