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-23 16:02:50 +08:00
parent 68a642e6a6
commit e62c53d343
14 changed files with 204 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -2,7 +2,7 @@
PROJECT(SJTU-RM-CV) PROJECT(SJTU-RM-CV)
SET(CMAKE_CXX_STANDARD 11) SET(CMAKE_CXX_STANDARD 11)
SET(CMAKE_BUILD_TYPE RELEASE) SET(CMAKE_BUILD_TYPE DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")

5
energy/include/energy/energy.h
View File

@@ -35,6 +35,7 @@ public:
void setEnergyInit();//设置能量机关初始化 void setEnergyInit();//设置能量机关初始化
void setBigEnergyInit();//设置大能量机关初始化 void setBigEnergyInit();//设置大能量机关初始化
void setSmallEnergyInit();//设置小能量机关初始化 void setSmallEnergyInit();//设置小能量机关初始化
void sendEnergy();//发送能量机关数据
void sendTarget(Serial& serial, float x, float y, float z);//发送数据 void sendTarget(Serial& serial, float x, float y, float z);//发送数据
void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据 void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据
@@ -64,6 +65,7 @@ private:
double radius;//大风车半径 double radius;//大风车半径
int send_cnt;//向主控板发送的数据总次数 int send_cnt;//向主控板发送的数据总次数
int camera_cnt;//摄像头数量
int last_fans_cnt;//上一帧的扇叶个数 int last_fans_cnt;//上一帧的扇叶个数
int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区 int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区
int energy_rotation_direction;//风车旋转方向 int energy_rotation_direction;//风车旋转方向
@@ -153,8 +155,9 @@ private:
void writeDownMark();//记录操作手标定的云台初始角度 void writeDownMark();//记录操作手标定的云台初始角度
bool guessTarget();//获得猜测击打点位 bool guessTarget();//获得猜测击打点位
void changeTarget();//判断目标是否改变
bool getOrigin();//获得云台对心所需角度 bool getOrigin();//获得云台对心所需角度
void changeTarget();//判断目标是否改变
void multipleMode(cv::Mat gimbal_src);//多模式切换
void getTargetPolarAngle();//获得目标装甲板极坐标角度 void getTargetPolarAngle();//获得目标装甲板极坐标角度
void getPredictPoint(cv::Point target_point);//获取预测点位 void getPredictPoint(cv::Point target_point);//获取预测点位
void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位 void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位

26
energy/src/energy/change/multiple_mode.cpp Normal file
View File

@@ -0,0 +1,26 @@
//
// Created by sun on 19-7-23.
//
#include "energy/energy.h"
using namespace std;
using namespace cv;
void Energy::multipleMode(cv::Mat gimbal_src) {
if (is_predicting) {
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();
sendEnergy();
} 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(predict_point);
sendEnergy();
}
}

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

@@ -29,6 +29,7 @@ void Energy::initEnergy() {
radius = 0; radius = 0;
send_cnt = 0; send_cnt = 0;
camera_cnt = 1;
last_fans_cnt = 0; last_fans_cnt = 0;
guess_devide = 0; guess_devide = 0;
energy_rotation_direction = ANTICLOCKWISE; energy_rotation_direction = ANTICLOCKWISE;
@@ -101,13 +102,13 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0;
gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 7; gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 5;
gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 25; gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 45;
gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 7; gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 5;
gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 25; gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 45;
gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3;
gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.5; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.3;
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 = 3000; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000;

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

@@ -21,7 +21,7 @@ int Energy::findFans(const cv::Mat src) {
} }
std::vector<vector<Point> > fan_contours; std::vector<vector<Point> > fan_contours;
FanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 FanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("fan struct",src_bin); if (show_process)imshow("fan struct", src_bin);
findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &fan_contour : fan_contours) { for (auto &fan_contour : fan_contours) {
@@ -69,7 +69,7 @@ int Energy::findArmors(const cv::Mat src) {
ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
imshow("armor struct", src_bin); if (show_process)imshow("armor struct", src_bin);
// 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++)//去除外轮廓 // for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
@@ -121,7 +121,7 @@ bool Energy::findCenterR(const cv::Mat src) {
} }
std::vector<vector<Point> > center_R_contours; std::vector<vector<Point> > center_R_contours;
CenterRStruct(src_bin); CenterRStruct(src_bin);
// imshow("R struct",src_bin); if (show_process)imshow("R struct", src_bin);
findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &center_R_contour : center_R_contours) { for (auto &center_R_contour : center_R_contours) {
if (!isValidCenterRContour(center_R_contour)) { if (!isValidCenterRContour(center_R_contour)) {
@@ -172,15 +172,16 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
} }
std::vector<vector<Point> > flow_strip_fan_contours; std::vector<vector<Point> > flow_strip_fan_contours;
FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
imshow("flow strip fan struct", src_bin); if (show_process)imshow("flow strip fan struct", src_bin);
findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
std::vector<cv::RotatedRect> candidate_flow_strip_fans;
for (auto &flow_strip_fan_contour : flow_strip_fan_contours) { for (auto &flow_strip_fan_contour : flow_strip_fan_contours) {
if (!isValidFlowStripFanContour(src_bin, flow_strip_fan_contour)) { if (!isValidFlowStripFanContour(src_bin, flow_strip_fan_contour)) {
continue; continue;
} }
flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour); candidate_flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour));
// 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;
@@ -196,12 +197,51 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
// cout<<cur_contour_area / cur_size.area()<<endl; // cout<<cur_contour_area / cur_size.area()<<endl;
// } // }
// cout << cur_rect.center << endl; // cout << cur_rect.center << endl;
return true;
} }
// showFlowStripFan("strip fan", src_bin); if (candidate_flow_strip_fans.size() == 1) {
cout << "flow strip fan false!" << endl; flow_strip_fan = candidate_flow_strip_fans.at(0);
return true;
} else if (candidate_flow_strip_fans.size() >= 2) {
//用锤子筛选仍然有多个候选区，进一步用锤头做筛选
std::vector<cv::RotatedRect> candidate_target_fans;
for (auto candidate_flow_strip_fan: candidate_flow_strip_fans) {
flow_strip_fan = candidate_flow_strip_fan;
if (!findTargetInFlowStripFan()) {
continue;
}
candidate_target_fans.emplace_back(candidate_flow_strip_fan);
}
if (candidate_target_fans.size() == 1) {
flow_strip_fan = candidate_target_fans.at(0);
return true;
} else if(candidate_target_fans.empty()){
cout<<"No candidate target fan contains a target armor!"<<endl;
return false;
} else { //用锤子+锤头筛选仍然有多个候选区，进一步用锤柄做筛选
for (auto candidate_target_fan: candidate_target_fans) {
flow_strip_fan = candidate_target_fan;
findTargetInFlowStripFan();
cv::Mat src_mask = src.clone();
target_armor.size.height *= 1.3;
target_armor.size.width *= 1.3;
Point2f vertices[4];
vector<Point2f> mask_rect;
target_armor.points(vertices);
for (int i = 0; i < 4; i++)
line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
if (findFlowStrip(src_mask)) {
flow_strip_fan = candidate_target_fan;
return true;
}
}
cout<<"No candidate target fan contains a flow strip!"<<endl;
return false;
}
} else {
cout << "flow strip fan false!" << endl;
// waitKey(0); // waitKey(0);
return false; return false;
}
} }
@@ -216,7 +256,7 @@ bool Energy::findFlowStrip(const cv::Mat src) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
} }
FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
imshow("flow strip struct", src_bin); if (show_process)imshow("flow strip struct", src_bin);
std::vector<vector<Point> > flow_strip_contours; std::vector<vector<Point> > flow_strip_contours;
findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

2
energy/src/energy/find/target_finder.cpp
View File

@@ -68,7 +68,7 @@ bool Energy::findTargetInFlowStripFan() {
return true; return true;
} }
} }
// cout<<"find target false"<<endl; cout<<"find target false"<<endl;
// cout<<armors.size()<<'\t'<<armors.at(0).size<<endl; // cout<<armors.size()<<'\t'<<armors.at(0).size<<endl;
return false; return false;
} }

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

@@ -9,17 +9,17 @@
using namespace std; using namespace std;
using namespace cv; using namespace cv;
extern mcu_data mcuData;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::getAimPoint(cv::Point target_point) { void Energy::getAimPoint(cv::Point target_point) {
//五号车 double dx = -(target_point.x - 320 - COMPENSATE_YAW - mcuData.delta_x);
// double dx = -(target_point.x - 320 - 10); double dy = -(target_point.y - 240 - COMPENSATE_PITCH - mcuData.delta_y);
// double dy = -(target_point.y - 240 - 22);
//四号车
double dx = -(target_point.x - 320 - COMPENSATE_YAW);
double dy = -(target_point.y - 240 - COMPENSATE_PITCH);
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;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
// cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl;
} }

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

@@ -3,6 +3,8 @@
// //
#include "energy/energy.h" #include "energy/energy.h"
#include "log.h" #include "log.h"
#include "config/setconfig.h"
using namespace std; using namespace std;
using namespace cv; using namespace cv;
@@ -14,23 +16,18 @@ using namespace cv;
bool Energy::getOrigin() { bool Energy::getOrigin() {
if (!auto_mark && !manual_mark) { if (!auto_mark && !manual_mark) {
//五号车 //五号车
// double dx = -(circle_center_point.x - 320 - 10); double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW);
// double dy = -(circle_center_point.y - 240 - 22); double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH);
//四号车
double dx = -(circle_center_point.x - 320 - 7);
double dy = -(circle_center_point.y - 240 - 64);
center_delta_yaw = static_cast<float>(atan(dx / FOCUS_PIXAL) * 180 / PI); center_delta_yaw = static_cast<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);
center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI); center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI);
shoot = 1;
sendEnergy();
if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) { 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; return false;
} else { } else {
origin_yaw = mcuData.curr_yaw; origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch; origin_pitch = mcuData.curr_pitch;
auto_mark = true; auto_mark = true;
sendTarget(serial, center_delta_yaw, center_delta_pitch, 1);
LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!")); LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
return true; return true;
} }

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

@@ -13,8 +13,8 @@ using namespace cv;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断世界坐标系下是否可以发弹 // 此函数用于判断世界坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInWorld(){ void Energy::judgeShootInWorld() {
if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.8) { if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) {
shoot = 4; shoot = 4;
is_predicting = false; is_predicting = false;
is_guessing = true; is_guessing = true;
@@ -29,19 +29,16 @@ void Energy::judgeShootInWorld(){
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断云台坐标系下是否可以发弹 // 此函数用于判断云台坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInGimbal(){ void Energy::judgeShootInGimbal() {
if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) { if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) {
shoot = 4; shoot = 3;
is_predicting = false; is_predicting = false;
is_guessing = true; is_guessing = true;
start_guess = true; start_guess = true;
gettimeofday(&time_start_guess, NULL); gettimeofday(&time_start_guess, NULL);
// LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); // LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else } else
shoot = 2; shoot = 1;
sum_yaw += yaw_rotation; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
sum_pitch += pitch_rotation;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
} }

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

@@ -19,8 +19,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
energy_part_param_ = gimbal_energy_part_param_; energy_part_param_ = gimbal_energy_part_param_;
clearAll(); clearAll();
initImage(gimbal_src); initImage(gimbal_src);
// findFans(gimbal_src); camera_cnt = 2;
// showFans("fan",gimbal_src);
if (findArmors(gimbal_src) < 1)return; if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src); if (show_energy)showArmors("armor", gimbal_src);
@@ -46,7 +45,6 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (findArmors(chassis_src) < 1)return; if (findArmors(chassis_src) < 1)return;
if (show_energy)showArmors("armor", chassis_src); if (show_energy)showArmors("armor", chassis_src);
if (!findFlowStripFan(chassis_src))return; if (!findFlowStripFan(chassis_src))return;
showFlowStripFan("flow strip fan", chassis_src);
if (!findTargetInFlowStripFan()) return; if (!findTargetInFlowStripFan()) return;
if (!findCenterROI(chassis_src))return; if (!findCenterROI(chassis_src))return;
if (show_energy)showFlowStripFan("strip", chassis_src); if (show_energy)showFlowStripFan("strip", chassis_src);
@@ -61,7 +59,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
getPredictPoint(target_point); getPredictPoint(target_point);
gimbalRotation(); gimbalRotation();
judgeShootInWorld(); judgeShootInWorld();
sendTarget(serial, yaw_rotation, pitch_rotation, change_target); sendEnergy();
} }
} }
@@ -74,9 +72,6 @@ void Energy::runBig(cv::Mat &gimbal_src) {
clearAll(); clearAll();
initImage(gimbal_src); initImage(gimbal_src);
changeMark();
if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
if (show_process)imshow("bin", gimbal_src); if (show_process)imshow("bin", gimbal_src);
if (findArmors(gimbal_src) < 1)return; if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src); if (show_energy)showArmors("armor", gimbal_src);
@@ -88,7 +83,6 @@ void Energy::runBig(cv::Mat &gimbal_src) {
if (show_energy)showCenterR("R", gimbal_src); if (show_energy)showCenterR("R", gimbal_src);
changeTarget(); changeTarget();
getTargetPolarAngle(); getTargetPolarAngle();
// if (!getOrigin())return;
if (energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
return; return;
@@ -96,7 +90,7 @@ void Energy::runBig(cv::Mat &gimbal_src) {
getPredictPoint(target_point); getPredictPoint(target_point);
getAimPoint(predict_point); getAimPoint(predict_point);
judgeShootInGimbal(); judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, change_target); sendEnergy();
} }
@@ -114,31 +108,11 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (!findFlowStripFan(gimbal_src))return; if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) 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(); changeTarget();
getAimPoint(target_point); getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal(); judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, change_target); sendEnergy();
} }
//if (is_predicting) {
//getPredictPoint(target_point);
//getAimPoint(predict_point);
//cout << yaw_rotation << '\t' << pitch_rotation << endl;
//judgeShootInGimbal();
//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(predict_point);
//sendTarget(serial, yaw_rotation, pitch_rotation, 5);
//}

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

@@ -4,15 +4,46 @@
#include "energy/energy.h" #include "energy/energy.h"
#include <iostream> #include <iostream>
#include "log.h" #include "log.h"
#include "config/setconfig.h"
using namespace std; using namespace std;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送能量机关数据
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendEnergy() {
if (is_big) {
if (camera_cnt == 1) {
sum_yaw += yaw_rotation;
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){
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;
}
}
if (change_target) {
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
} else if (is_predicting) {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else {
sendTarget(serial, yaw_rotation, pitch_rotation, 6);
}
}
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板 // 此函数用于发送数据给主控板
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTarget(Serial& serial, float x, float y, float z){ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
short x_tmp, y_tmp, z_tmp; short x_tmp, y_tmp, z_tmp;
uint8_t buff[10]; uint8_t buff[8];
#ifdef WITH_COUNT_FPS #ifdef WITH_COUNT_FPS
static auto last_time = time(nullptr); static auto last_time = time(nullptr);
@@ -36,19 +67,17 @@ void Energy::sendTarget(Serial& serial, float x, float y, float z){
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF); buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF); buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[9] = 'e'; buff[7] = 'e';
serial.WriteData(buff, sizeof(buff)); serial.WriteData(buff, sizeof(buff));
send_cnt+=1; send_cnt += 1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板 // 此函数用于发送数据给主控板
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTarget(Serial& serial, float x, float y, float z, uint16_t u){ void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
short x_tmp, y_tmp, z_tmp; short x_tmp, y_tmp, z_tmp;
uint8_t buff[10]; uint8_t buff[10];
@@ -78,6 +107,6 @@ void Energy::sendTarget(Serial& serial, float x, float y, float z, uint16_t u){
buff[8] = static_cast<char>((u >> 0) & 0xFF);; buff[8] = static_cast<char>((u >> 0) & 0xFF);;
buff[9] = 'e'; buff[9] = 'e';
serial.WriteData(buff, sizeof(buff)); serial.WriteData(buff, sizeof(buff));
send_cnt+=1; send_cnt += 1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
} }

96
main.cpp
View File

@@ -34,6 +34,8 @@ mcu_data mcuData = { // 单片机端回传结构体
0, // 云台角度标记位 0, // 云台角度标记位
1, // 是否启用数字识别 1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色 ENEMY_RED, // 敌方颜色
0, // 能量机关x轴补偿量
0, // 能量机关y轴补偿量
}; };
WrapperHead *video_gimbal = nullptr; // 云台摄像头视频源 WrapperHead *video_gimbal = nullptr; // 云台摄像头视频源
@@ -64,8 +66,8 @@ int main(int argc, char *argv[]) {
video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/);
video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "energy"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "energy"*/);
} else { } else {
video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal255.avi");
video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal255.avi");
} }
if (video_gimbal->init()) { if (video_gimbal->init()) {
LOGM("video_gimbal source initialization successfully."); LOGM("video_gimbal source initialization successfully.");
@@ -92,9 +94,10 @@ int main(int argc, char *argv[]) {
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();
if(from_camera){
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/); video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/);
if (video_gimbal->init()) { if (video_gimbal->init()) {
@@ -102,26 +105,28 @@ int main(int argc, char *argv[]) {
} else { } else {
LOGW("video_gimbal source unavailable!"); LOGW("video_gimbal source unavailable!");
} }
energy.setBigEnergyInit(); }
checkReconnect(video_chassis->read(chassis_src)); checkReconnect(video_chassis->read(chassis_src));
energy.setBigEnergyInit();
}
ok = checkReconnect(video_gimbal->read(gimbal_src));
video_chassis->read(chassis_src);
#ifdef GIMBAL_FLIP_MODE
flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
#endif
#ifdef CHASSIS_FLIP_MODE #ifdef CHASSIS_FLIP_MODE
flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE); flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE);
#endif #endif
} if (!from_camera) extract(gimbal_src, chassis_src);
ok = checkReconnect(video_gimbal->read(gimbal_src)); if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
video_chassis->read(chassis_src); if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
#ifdef GIMBAL_FLIP_MODE energy.runBig(gimbal_src, chassis_src);
flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
#endif
if (!from_camera) extract(gimbal_src, chassis_src);
if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
energy.runBig(gimbal_src, chassis_src);
// energy.runBig(gimbal_src); // energy.runBig(gimbal_src);
last_state = mcuData.state;//更新上一帧状态 last_state = mcuData.state;//更新上一帧状态
} else if (mcuData.state == SMALL_ENERGY_STATE) { } else if (mcuData.state == SMALL_ENERGY_STATE) {
if (mcuData.state != SMALL_ENERGY_STATE) { if (mcuData.state != SMALL_ENERGY_STATE) {
destroyAllWindows(); destroyAllWindows();
if(from_camera){
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/); video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/);
if (video_gimbal->init()) { if (video_gimbal->init()) {
@@ -129,20 +134,22 @@ int main(int argc, char *argv[]) {
} else { } else {
LOGW("video_gimbal source unavailable!"); LOGW("video_gimbal source unavailable!");
} }
energy.setSmallEnergyInit();
} }
ok = checkReconnect(video_gimbal->read(gimbal_src)); energy.setSmallEnergyInit();
}
ok = checkReconnect(video_gimbal->read(gimbal_src));
#ifdef GIMBAL_FLIP_MODE #ifdef GIMBAL_FLIP_MODE
flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
#endif #endif
if (!from_camera) extract(gimbal_src); if (!from_camera) extract(gimbal_src);
if (save_video) saveVideos(gimbal_src);//保存视频 if (save_video) saveVideos(gimbal_src);//保存视频
if (show_origin) showOrigin(gimbal_src);//显示原始图像 if (show_origin) showOrigin(gimbal_src);//显示原始图像
energy.runSmall(gimbal_src); energy.runSmall(gimbal_src);
last_state = mcuData.state;//更新上一帧状态 last_state = mcuData.state;//更新上一帧状态
} else { // 自瞄模式 } else { // 自瞄模式
if (last_state != ARMOR_STATE) { if (last_state != ARMOR_STATE) {
destroyAllWindows(); destroyAllWindows();
if(from_camera){
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/);
if (video_gimbal->init()) { if (video_gimbal->init()) {
@@ -151,18 +158,19 @@ int main(int argc, char *argv[]) {
LOGW("video_gimbal source unavailable!"); LOGW("video_gimbal source unavailable!");
} }
} }
last_state = mcuData.state;
ok = checkReconnect(video_gimbal->read(gimbal_src));
#ifdef GIMBAL_FLIP_MODE
flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
#endif
if (!from_camera) extract(gimbal_src);
// if (save_video) saveVideos(gimbal_src);
if (show_origin) showOrigin(gimbal_src);
CNT_TIME("Armor Time", {
armorFinder.run(gimbal_src);
});
} }
last_state = mcuData.state;
ok = checkReconnect(video_gimbal->read(gimbal_src));
#ifdef GIMBAL_FLIP_MODE
flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
#endif
if (!from_camera) extract(gimbal_src);
// if (save_video) saveVideos(gimbal_src);
if (show_origin) showOrigin(gimbal_src);
CNT_TIME("Armor Time", {
armorFinder.run(gimbal_src);
});
}
// cv::waitKey(0); // cv::waitKey(0);
// }); // });
} while (ok); } while (ok);

2
others/include/additions/additions.h
View File

@@ -15,6 +15,8 @@ struct mcu_data{
uint8_t mark; uint8_t mark;
uint8_t use_classifier; uint8_t use_classifier;
uint8_t enemy_color; uint8_t enemy_color;
int delta_x;
int delta_y;
}; };
extern mcu_data mcuData; extern mcu_data mcuData;

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

@@ -39,6 +39,7 @@ void uartReceive(Serial *pSerial) {
memcpy(&mcuData, buffer, sizeof(mcuData)); memcpy(&mcuData, buffer, sizeof(mcuData));
// LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); // LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark);
// LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch); // LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch);
// LOGM("Get delta x: %d, delta y: %d!", mcuData.delta_x, mcuData.delta_y);
// static int t = time(nullptr); // static int t = time(nullptr);
// static int cnt = 0; // static int cnt = 0;
// if(time(nullptr) > t){ // if(time(nullptr) > t){