VSAG/vision_sjtu_19
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-10 15:23:43 +08:00
parent 747fb58eb8
commit 403d267ff3
11 changed files with 248 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
energy/include/energy/energy.h
View File

@@ -37,8 +37,10 @@ public:
private:
EnergyPartParam energy_part_param_;//能量机关的参数设置
bool isMark;//若操作手正在手动标定，则为true
bool centered=false;//云台是否对准中心
bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
int fans_cnt;//图像中的扇叶个数
int armors_cnt;//图像中的装甲板个数
int centerRs_cnt;//图像中可能的风车中心字母R选区个数
@@ -48,7 +50,6 @@ private:
int last_armors_cnt;//上一帧的装甲板个数
int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数
int last_flow_strips_cnt;//上一帧的流动条个数
int gimble_cnt; //经过的帧数
double radius;//大风车半径
float target_polar_angle;//待击打装甲板的极坐标角度
float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
@@ -81,7 +82,6 @@ private:
cv::Point target_point;//目标装甲板中心坐标
cv::Point last_target_point;//上一帧目标装甲板中心坐标
cv::Point predict_point;//预测的击打点坐标
cv::Point former_point;//之前预测的圆心坐标
std::vector<float>fan_polar_angle;//当前帧所有扇叶的极坐标角度
std::vector<float>armor_polar_angle;//当前帧所有装甲板的极坐标角度
std::vector<cv::Point> all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
@@ -91,6 +91,8 @@ private:
void initEnergyPartParam();//能量机关参数初始化
void initRotation();//顺逆时针初始化
bool isGimbleCentered();//判断云台摄像头对心是否完成
int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶
int findArmor(const cv::Mat src, int &last_armors_cnt);//寻找图中所有装甲板
int findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R
@@ -113,6 +115,7 @@ private:
void getFanPolarAngle();//获取扇叶极坐标角度
void getArmorPolarAngle();//获取装甲板极坐标角度
void getAllArmorCenters();//记录所有装甲板中心坐标
void getOrigin();//获得云台对心所需角度
void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径

22
energy/src/energy/change/mode_change.cpp Normal file
View File

@@ -0,0 +1,22 @@
//
// Created by sun on 19-7-10.
//
#include "energy/energy.h"
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断云台对心是否完成
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::isGimbleCentered(){
if(centerRs.size()==1){
circle_center_point = centerRs.at(0).rect.center;
isGimble = false;
isChassis = true;
return true;
}
return false;
}

24
energy/src/energy/change/target_change.cpp Normal file
View File

@@ -0,0 +1,24 @@
//
// Created by sun on 19-7-10.
//
#include "energy/energy.h"
using namespace std;
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;
}
}

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

@@ -20,6 +20,9 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
initEnergy();
initEnergyPartParam();
energy_rotation_init = false;
isGimble = true;
isChassis = false;
save_new_mark = false;
if(ally_color == ALLY_RED){
@@ -50,7 +53,10 @@ Energy::~Energy() = default;
void Energy::setEnergyRotationInit() {
initEnergy();
initEnergyPartParam();
energy_rotation_init = true;
isGimble = true;
isChassis = false;
if(save_new_mark){
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r");

9
energy/src/energy/energy_init.cpp
View File

@@ -16,18 +16,15 @@ using std::vector;
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergy() {
isMark = false;
centered=false;
fans_cnt = 0;
armors_cnt = 0;
centerRs_cnt = 0;
flow_strips_cnt = 0;
flow_strip_fans_cnt = 0;
gimble_cnt = 0;
circle_center_point = Point(0, 0);
target_point = Point(0, 0);
last_target_point = Point(0, 0);
predict_point = Point(0, 0);
former_point = Point(0,0);
target_polar_angle = -1000;
last_target_polar_angle = -1000;
radius = 0;
@@ -42,14 +39,8 @@ void Energy::initEnergy() {
pitch_rotation = 0;
last_mark = 0;
red_origin_yaw = -0.35;
red_origin_pitch = 15.11719;
blue_origin_yaw = -0.439453;
blue_origin_pitch = 15.688477;
target_cnt = 0;
small_energy_shoot = false;
energy_rotation_init = false;
predict_rad = 20;
fans.clear();

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

@@ -9,56 +9,50 @@ using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数通过极坐标角度匹配扇叶和装甲板，找到目标装甲板,计算其极坐标角度和中心坐标
// ---------------------------------------------------------------------------------------------------------------------
void Energy::findTargetByPolar() {
if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误
if (armor_polar_angle.empty())return;//找不到扇叶，识别错误
if (fan_polar_angle.empty()) {
if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误
if (armor_polar_angle.empty())return;//找不到扇叶，识别错误
if (fan_polar_angle.empty()) {
target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板
for (const auto &armor : armors)
{
target_point = armor.rect.center;
}
return;
}
sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序
sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序
int i = 0, j = 0;
for (i = 0; i < fan_polar_angle.size(); ++i) {
if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX
&& armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) {
j++;
continue;//若第i个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功，i与j都加1
}
else {
for (const auto &armor : armors) {
target_point = armor.rect.center;
}
return;
}
sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序
sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序
int i = 0, j = 0;
for (i = 0; i < fan_polar_angle.size(); ++i) {
if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX
&& armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) {
j++;
continue;//若第i个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功，i与j都加1
} else {
target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板
for (const auto &armor : armors)
{
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x)));
if(target_polar_angle == angle){
target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
}
}
return;
}
}
for (const auto &armor : armors) {
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x)));
if (target_polar_angle == angle) {
target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
}
}
return;
}
}
target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标
for (const auto &armor : armors)
{
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x)));
if(target_polar_angle == angle){
target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
}
}
for (const auto &armor : armors) {
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x)));
if (target_polar_angle == angle) {
target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
}
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数根据矩形重合面积匹配扇叶与装甲板
// ---------------------------------------------------------------------------------------------------------------------
@@ -73,19 +67,19 @@ void Energy::findTargetByIntersection() {
while (i < armors.size()) {
for (j = 0; j < fans.size(); ++j) {
std::vector<cv::Point2f> intersection;
if(rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积
if (rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积
continue;
else
rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection);
double cur_contour_area = contourArea(intersection);
if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) {
double cur_contour_area = contourArea(intersection);
if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) {
// cout << endl;
// cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is"
// << cur_contour_area << endl;
break;
}
break;
}
}
if(j == fans.size()){
if (j == fans.size()) {
target_point = armors.at(i).rect.center;
break;
}
@@ -94,20 +88,19 @@ void Energy::findTargetByIntersection() {
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数在流动条区域内寻找装甲板
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findTargetInFlowStripFan(){
bool Energy::findTargetInFlowStripFan() {
int i = 0;
for(i=0; i<armors.size(); ++i){
for (i = 0; i < armors.size(); ++i) {
std::vector<cv::Point2f> intersection;
if(rotatedRectangleIntersection(armors.at(i).rect, flow_strip_fans.at(0).rect, intersection)==0)
if (rotatedRectangleIntersection(armors.at(i).rect, flow_strip_fans.at(0).rect, intersection) == 0)
continue;//返回0表示没有重合面积
double cur_contour_area = contourArea(intersection);
if(cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN)
if (cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN)
continue;
else{
else {
target_armor.emplace_back(armors.at(i));
target_point = armors.at(i).rect.center;
return true;

20
energy/src/energy/get/origin_get.cpp Normal file
View File

@@ -0,0 +1,20 @@
//
// Created by sun on 19-7-10.
//
#include "energy/energy.h"
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心）
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getOrigin(){
double dx = circle_center_point.x - 320;
double dy = circle_center_point.y - 240;
origin_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
origin_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
}

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

@@ -20,18 +20,3 @@ void Energy::getPredictPoint(){
if(energy_rotation_direction==-1) rotate();
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手手动标定
// ---------------------------------------------------------------------------------------------------------------------
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;
}
}

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

@@ -10,70 +10,98 @@ using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){
if(chassis_src.empty())
int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
if (chassis_src.empty())
runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if(!centered) {
else if (isGimble) {
imshow("src", gimble_src);
fans.clear();
armors.clear();
armor_polar_angle.clear();
changeMark();
if (isMark)return 0;
centerRs.clear();
flow_strip_fans.clear();
flow_strips.clear();
center_ROI.clear();
target_armor.clear();
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("yun",gimble_src);
imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(gimble_src);
showFlowStripFanContours("strip", gimble_src);
} else {
fans_cnt = findFan(gimble_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
}
centerRs_cnt = findCenterR(gimble_src);
if (centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (isGimbleCentered()) {
getOrigin();
initEnergy();
destroyAllWindows();
}
} else if (isChassis) {
imshow("src", chassis_src);
fans.clear();
armors.clear();
centerRs.clear();
flow_strip_fans.clear();
flow_strips.clear();
center_ROI.clear();
target_armor.clear();
armors_cnt = findArmor(gimble_src, last_armors_cnt);
if(armors_cnt!=1) return 0;//滤去漏判的帧
// imagePreprocess(chassis_src);
// imshow("img_preprocess", chassis_src);
getAllArmorCenters();
circleLeastFit();
changeMark();
if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
// attack_distance = 752;//单项赛
attack_distance = 718;
threshold(chassis_src, chassis_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
armors_cnt = findArmor(chassis_src, last_armors_cnt);
flow_strip_fans_cnt = findFlowStripFan(chassis_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(chassis_src);
showFlowStripFanContours("strip", chassis_src);
} else {
fans_cnt = findFan(chassis_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
}
centerRs_cnt = findCenterR(chassis_src);
if (centerRs_cnt > 0)showCenterRContours("R", chassis_src);
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x)));
if (energy_rotation_init) {
initRotation();
return 0;
}
if(++gimble_cnt%8==0){
former_point=circle_center_point;
//gimble_cnt=0;
}
if(former_point==predict_point&&gimble_cnt%8==7&&predict_point!=Point(0,0)) {
centered=true;
cout<<"gimble focused!"<<endl;
cout<<"number of framse: "<<gimble_cnt<<endl;
}
predict_point=circle_center_point;
// cout<<gimble_cnt<<endl;
// cout<<"center:("<<predict_point.x<<','<<predict_point.y<<")\n";
getOrigin();
getPredictPoint();
gimbleRotation();
// cout<<"send"<<endl;
// cout<<"position mode: "<<position_mode<<endl;
if(changeTarget())target_cnt++;
if (changeTarget())target_cnt++;
sendBigTarget(serial, yaw_rotation, pitch_rotation, target_cnt);
return 0;
}
// if(centered)
// destroyAllWindows();
return 0;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runBig(cv::Mat &gimble_src){
imshow("src",gimble_src);
int Energy::runBig(cv::Mat &gimble_src) {
imshow("src", gimble_src);
fans.clear();
armors.clear();
centerRs.clear();
@@ -81,81 +109,79 @@ int Energy::runBig(cv::Mat &gimble_src){
flow_strips.clear();
center_ROI.clear();
target_armor.clear();
fan_polar_angle.clear();
armor_polar_angle.clear();
changeMark();
if (isMark)return 0;
// imagePreprocess(gimble_src);
// imshow("img_preprocess", gimble_src);
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("bin",gimble_src);
// imshow("bin",gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
if (energy_rotation_init) {
initRotation();
return 0;
}
getAllArmorCenters();
circleLeastFit();
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
if(flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()){
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(gimble_src);
showFlowStripFanContours("strip", gimble_src);
}else{
} else {
fans_cnt = findFan(gimble_src, last_fans_cnt);
if(fans_cnt==-1) return 0;//滤去漏判的帧
if(armors_cnt==-1) return 0;//滤去漏判的帧
if(armors_cnt != fans_cnt+1) return 0;
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
// getFanPolarAngle();
// getArmorPolarAngle();
// findTargetByPolar();
// if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
}
centerRs_cnt = findCenterR(gimble_src);
if(centerRs_cnt>0)showCenterRContours("R", gimble_src);
writeDownMark();
if (centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x)));
if (energy_rotation_init) {
initRotation();
getOrigin();//一旦确定风车旋向后，就开始移动云台，此时不再更新origin_yaw和origin_pitch
return 0;
}
getPredictPoint();
gimbleRotation();
if(changeTarget())target_cnt++;
if (changeTarget())target_cnt++;
sendBigTarget(serial, yaw_rotation, pitch_rotation, target_cnt);
// cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
// cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;
return 0;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runSmall(cv::Mat &gimble_src) {
imshow("gimble src", gimble_src);
if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
fans.clear();
armors.clear();
centerRs.clear();
flow_strip_fans.clear();
flow_strips.clear();
center_ROI.clear();
target_armor.clear();
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(gimble_src);
showFlowStripFanContours("strip", gimble_src);
} else {
fans_cnt = findFan(gimble_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
}
centerRs_cnt = findCenterR(gimble_src);
if (centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x)));
getAimPoint();
if (changeTarget())target_cnt++;//若云台移动过程中发现有新装甲板亮起，需改变target_cnt值，以及时告知主控板中断进程，防止重复打击
sendSmallTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot);
return 0;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runSmall(cv::Mat &gimble_src){
imshow("gimble src", gimble_src);
if(gimble_src.type()== CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
fans.clear();
armors.clear();-
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("bin",gimble_src);
fans_cnt = findFan(gimble_src, last_fans_cnt);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
if(fans_cnt==-1 || armors_cnt==-1 || armors_cnt != fans_cnt+1) return 0;
findTargetByIntersection();
if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
getAimPoint();
if(changeTarget())target_cnt++;//若云台移动过程中发现有新装甲板亮起，需改变target_cnt值，以及时告知主控板中断进程，防止重复打击
sendSmallTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot);
}

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

@@ -27,6 +27,7 @@ void Energy::sendBigTarget(Serial& serial, float x, float y, float z) {
buff[7] = 'e';
serial.WriteData(buff, sizeof(buff));
send_cnt+=1;
// cout<<"send cnt: "<<send_cnt<<endl;
}

14
main.cpp
View File

@@ -27,7 +27,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角
0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符
BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位
1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色
@@ -37,7 +37,7 @@ WrapperHead *video_gimble = nullptr; // 云台摄像头视频源
WrapperHead *video_chassis = nullptr; // 底盘摄像头视频源
Serial serial(115200); // 串口对象
uint8_t last_state = mcuData.state; // 上次状态，用于初始化
uint8_t last_state = ARMOR_STATE; // 上次状态，用于初始化
// 自瞄主程序对象
ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier);
// 能量机关主程序对象
@@ -97,16 +97,12 @@ int main(int argc, char *argv[]) {
ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
// if (from_camera == 0) {
// cv::resize(chassis_src, chassis_src, cv::Size(640, 480), 2);
// imshow("resize", chassis_src);
// energy.extract(chassis_src);
// }
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
energy.setEnergyRotationInit();
cout << "set" << endl;
}
energy.runBig(gimble_src, chassis_src);//击打大符
// energy.runBig(gimble_src);
last_state = mcuData.state;//更新上一帧状态
} else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
last_state = mcuData.state;
@@ -118,10 +114,10 @@ int main(int argc, char *argv[]) {
armorFinder.run(gimble_src);
});
} else if (mcuData.state == SMALL_ENERGY_STATE) {
// energy.runSmall(gimble_src);
energy.runBig(gimble_src);
energy.runSmall(gimble_src);
}
}
// cv::waitKey(0);
});
} while (ok);
delete video_gimble;