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

新版反陀螺。

Browse Source
This commit is contained in:
xinyang
2019-08-03 19:00:46 +08:00
parent cdd6716214
commit ef32b54761
18 changed files with 148 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
armor/include/armor_finder/armor_finder.h
View File

@@ -79,6 +79,7 @@ public:
explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0); explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0);
cv::Point2f getCenter() const;
double getBlobsDistance() const; double getBlobsDistance() const;
double lengthDistanceRatio() const; double lengthDistanceRatio() const;
double getBoxDistance() const; double getBoxDistance() const;
@@ -114,17 +115,18 @@ private:
systime frame_time; // 当前帧对应时间; systime frame_time; // 当前帧对应时间;
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化 const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
State state; // 自瞄状态对象实例 State state; // 自瞄状态对象实例
ArmorBox armor_box; // 当前目标装甲板 ArmorBox target_box, last_box; // 目标装甲板
int anti_switch_cnt; // 防止乱切目标计数器
cv::Ptr<cv::Tracker> tracker; // tracker对象实例 cv::Ptr<cv::Tracker> tracker; // tracker对象实例
Classifier classifier; // CNN分类器对象实例，用于数字识别 Classifier classifier; // CNN分类器对象实例，用于数字识别
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用） int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
int tracking_cnt; // 记录追踪帧数，用于定时退出追踪 int tracking_cnt; // 记录追踪帧数，用于定时退出追踪
Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量
const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化 const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化
RoundQueue<double, 4> top_periodms; RoundQueue<double, 4> top_periodms; // 陀螺周期循环队列
RoundQueue<double, 5> box_ratioes; RoundQueue<double, 5> box_ratioes; //
systime last_front_time; // 上一次发生装甲板方向切换的时间 systime last_front_time; // 上一次发生装甲板方向切换的时间
BoxRatioChangeType last_ratio_type; BoxRatioChangeType last_ratio_type; //
int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺 int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺
AntiTopState anti_top_state; // 当前是否识别到陀螺 AntiTopState anti_top_state; // 当前是否识别到陀螺

43
armor/src/armor_finder/anti_top/anti_top.cpp
View File

@@ -13,7 +13,7 @@ static double boxDistance(const cv::Rect2d &a, const cv::Rect2d &b) {
return sqrt(dist.x * dist.x + dist.y * dist.y); return sqrt(dist.x * dist.x + dist.y * dist.y);
} }
template <int length> template<int length>
static double mean(RoundQueue<double, length> &vec) { static double mean(RoundQueue<double, length> &vec) {
double sum = 0; double sum = 0;
for (int i = 0; i < vec.size(); i++) { for (int i = 0; i < vec.size(); i++) {
@@ -33,13 +33,14 @@ ArmorFinder::BoxRatioChangeType ArmorFinder::getRatioChangeType(RoundQueue<doubl
} }
} }
/*
void ArmorFinder::antiTop() { void ArmorFinder::antiTop() {
if (armor_box.rect == cv::Rect2d()) return; if (target_box.rect == cv::Rect2d()) return;
uint16_t shoot_delay = 0; uint16_t shoot_delay = 0;
auto interval = getTimeIntervalms(frame_time, last_front_time); auto interval = getTimeIntervalms(frame_time, last_front_time);
box_ratioes.push(armor_box.rect.width / armor_box.rect.height); box_ratioes.push(target_box.rect.width / target_box.rect.height);
auto change_type = getRatioChangeType(box_ratioes); auto change_type = getRatioChangeType(box_ratioes);
auto orientation = armor_box.getOrientation(); auto orientation = target_box.getOrientation();
if (interval > 700) { if (interval > 700) {
anti_top_cnt = 0; anti_top_cnt = 0;
if (anti_top_state == ANTI_TOP) { if (anti_top_state == ANTI_TOP) {
@@ -76,4 +77,38 @@ void ArmorFinder::antiTop() {
sendBoxPosition(shoot_delay); sendBoxPosition(shoot_delay);
} }
} }
*/
void ArmorFinder::antiTop() {
if (target_box.rect == cv::Rect2d()) return;
uint16_t shoot_delay = 0;
auto interval = getTimeIntervalms(frame_time, last_front_time);
if (anti_top_state == ANTI_TOP && interval > 700) {
anti_top_state = NORMAL;
LOGM(STR_CTR(WORD_YELLOW, "switch to normal"));
}
if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 3.0) {
if (150 < interval && interval < 700) {
if (anti_top_state == ANTI_TOP) {
top_periodms.push(interval);
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), interval);
systime curr_time;
getsystime(curr_time);
auto calculate_time = getTimeIntervalms(curr_time, frame_time);
shoot_delay = mean(top_periodms) - calculate_time;
} else {
if (++anti_top_cnt > 4) {
anti_top_state == ANTI_TOP;
LOGM(STR_CTR(WORD_CYAN, "switch to anti-top"));
}
}
}
}
if (anti_top_state == NORMAL) {
sendBoxPosition(0);
} else if (interval < top_periodms[-1] * 0.2){
sendBoxPosition(shoot_delay);
}
}

17
armor/src/armor_finder/armor_box/armor_box.cpp
View File

@@ -8,6 +8,15 @@
ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) : ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) :
rect(pos), light_blobs(blobs), box_color(color), id(i) {}; rect(pos), light_blobs(blobs), box_color(color), id(i) {};
cv::Point2f ArmorBox::getCenter() const {
return cv::Point2f(
rect.x + rect.width / 2,
rect.y + rect.height / 2
);
}
double ArmorBox::getBlobsDistance() const { double ArmorBox::getBlobsDistance() const {
if (light_blobs.size() == 2) { if (light_blobs.size() == 2) {
auto &x = light_blobs[0].rect.center; auto &x = light_blobs[0].rect.center;
@@ -78,10 +87,10 @@ bool ArmorBox::operator<(const ArmorBox &box) const {
return prior_red[id2name[id]] < prior_red[id2name[box.id]]; return prior_red[id2name[id]] < prior_red[id2name[box.id]];
} }
} else { } else {
auto d1 = (rect.x-IMAGE_CENTER_X)*(rect.x-IMAGE_CENTER_X) auto d1 = (rect.x - IMAGE_CENTER_X) * (rect.x - IMAGE_CENTER_X)
+ (rect.y-IMAGE_CENTER_Y)*(rect.y-IMAGE_CENTER_Y); + (rect.y - IMAGE_CENTER_Y) * (rect.y - IMAGE_CENTER_Y);
auto d2 = (box.rect.x-IMAGE_CENTER_X)*(box.rect.x-IMAGE_CENTER_X) auto d2 = (box.rect.x - IMAGE_CENTER_X) * (box.rect.x - IMAGE_CENTER_X)
+ (box.rect.y-IMAGE_CENTER_Y)*(box.rect.y-IMAGE_CENTER_Y); + (box.rect.y - IMAGE_CENTER_Y) * (box.rect.y - IMAGE_CENTER_Y);
return d1 < d2; return d1 < d2;
} }
} }

11
armor/src/armor_finder/armor_finder.cpp
View File

@@ -50,6 +50,9 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
serial(u), serial(u),
enemy_color(color), enemy_color(color),
state(STANDBY_STATE), state(STANDBY_STATE),
anti_top_cnt(0),
anti_switch_cnt(0),
anti_top_state(NORMAL),
classifier(paras_folder), classifier(paras_folder),
contour_area(0), contour_area(0),
use_classifier(use), use_classifier(use),
@@ -63,15 +66,15 @@ void ArmorFinder::run(cv::Mat &src) {
switch (state) { switch (state) {
case SEARCHING_STATE: case SEARCHING_STATE:
if (stateSearchingTarget(src)) { if (stateSearchingTarget(src)) {
if ((armor_box.rect & cv::Rect2d(0, 0, 640, 480)) == armor_box.rect) { // 判断装甲板区域是否脱离图像区域 if ((target_box.rect & cv::Rect2d(0, 0, 640, 480)) == target_box.rect) { // 判断装甲板区域是否脱离图像区域
if (!classifier || !use_classifier) { /* 如果分类器不可用或者不使用分类器 */ if (!classifier || !use_classifier) { /* 如果分类器不可用或者不使用分类器 */
cv::Mat roi = src(armor_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */ cv::Mat roi = src(target_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */
cv::cvtColor(roi, roi_gray, CV_RGB2GRAY); cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY); cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray); contour_area = cv::countNonZero(roi_gray);
} }
tracker = TrackerToUse::create(); // 成功搜寻到装甲板，创建tracker对象 tracker = TrackerToUse::create(); // 成功搜寻到装甲板，创建tracker对象
tracker->init(src, armor_box.rect); tracker->init(src, target_box.rect);
state = TRACKING_STATE; state = TRACKING_STATE;
tracking_cnt = 0; tracking_cnt = 0;
LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track")); LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
@@ -92,7 +95,7 @@ end:
antiTop(); antiTop();
if (show_armor_box) { // 根据条件显示当前目标装甲板 if (show_armor_box) { // 根据条件显示当前目标装甲板
showArmorBox("box", src, armor_box); showArmorBox("box", src, target_box);
cv::waitKey(1); cv::waitKey(1);
} }
} }

9
armor/src/armor_finder/find/find_armor_box.cpp
View File

@@ -147,7 +147,14 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
armor_box.id = c; armor_box.id = c;
} }
}, armor_boxes.size()); }, armor_boxes.size());
sort(armor_boxes.begin(), armor_boxes.end()); sort(armor_boxes.begin(), armor_boxes.end(), [&](const ArmorBox &a, const ArmorBox &b){
if(last_box.rect != cv::Rect2d()){
return getPointLength(a.getCenter()-last_box.getCenter()) <
getPointLength(b.getCenter()-last_box.getCenter());
}else{
return a < b;
}
});
if(armor_boxes[0].id != 0){ if(armor_boxes[0].id != 0){
box = armor_boxes[0]; box = armor_boxes[0];
} }

21
armor/src/armor_finder/searching_state/searching_state.cpp
View File

@@ -8,10 +8,23 @@
#include <log.h> #include <log.h>
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
if(findArmorBox(src, armor_box)){ if(anti_switch_cnt >= 3){
return true; last_box = ArmorBox();
}else{ }
armor_box = ArmorBox(); if (findArmorBox(src, target_box)) {
if (last_box.rect != cv::Rect2d() &&
(getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 3.0) &&
anti_switch_cnt++ < 3) {
target_box = ArmorBox();
return false;
} else {
last_box = target_box;
anti_switch_cnt = 0;
return true;
}
} else {
target_box = ArmorBox();
anti_switch_cnt++;
return false; return false;
} }
} }

4
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -42,11 +42,11 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
} }
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if (armor_box.rect == cv::Rect2d()) return false; if (target_box.rect == cv::Rect2d()) return false;
if (shoot_delay) { if (shoot_delay) {
LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay); LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay);
} }
auto rect = armor_box.rect; auto rect = target_box.rect;
double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X; double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y; double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;

24
armor/src/armor_finder/tracking_state/tracking_state.cpp
View File

@@ -7,13 +7,13 @@
#include <show_images/show_images.h> #include <show_images/show_images.h>
bool ArmorFinder::stateTrackingTarget(cv::Mat &src) { bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
auto pos = armor_box.rect; auto pos = target_box.rect;
if(!tracker->update(src, pos)){ if(!tracker->update(src, pos)){
armor_box = ArmorBox(); target_box = ArmorBox();
return false; return false;
} }
if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){ if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){
armor_box = ArmorBox(); target_box = ArmorBox();
return false; return false;
} }
@@ -32,21 +32,21 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
ArmorBox box; ArmorBox box;
if(findArmorBox(roi, box)) { if(findArmorBox(roi, box)) {
armor_box = box; target_box = box;
armor_box.rect.x += bigger_rect.x; target_box.rect.x += bigger_rect.x;
armor_box.rect.y += bigger_rect.y; target_box.rect.y += bigger_rect.y;
for(auto &blob : armor_box.light_blobs){ for(auto &blob : target_box.light_blobs){
blob.rect.center.x += bigger_rect.x; blob.rect.center.x += bigger_rect.x;
blob.rect.center.y += bigger_rect.y; blob.rect.center.y += bigger_rect.y;
} }
tracker = TrackerToUse::create(); tracker = TrackerToUse::create();
tracker->init(src, armor_box.rect); tracker->init(src, target_box.rect);
}else{ }else{
roi = src(pos).clone(); roi = src(pos).clone();
if(classifier){ if(classifier){
cv::resize(roi, roi, cv::Size(48, 36)); cv::resize(roi, roi, cv::Size(48, 36));
if(classifier(roi) == 0){ if(classifier(roi) == 0){
armor_box = ArmorBox(); target_box = ArmorBox();
return false; return false;
} }
}else{ }else{
@@ -55,12 +55,12 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY); cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray); contour_area = cv::countNonZero(roi_gray);
if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){ if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
armor_box = ArmorBox(); target_box = ArmorBox();
return false; return false;
} }
} }
armor_box.rect = pos; target_box.rect = pos;
armor_box.light_blobs.clear(); target_box.light_blobs.clear();
} }
return true; return true;
} }

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

@@ -9,7 +9,7 @@
using namespace std; using namespace std;
using namespace cv; using namespace cv;
extern mcu_data mcuData; extern McuData mcu_data;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
@@ -29,12 +29,12 @@ void Energy::getAimPoint(cv::Point target_point_) {
extra_delta_y = 0; extra_delta_y = 0;
} }
double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x - extra_delta_x); double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcu_data.delta_x - manual_delta_x - extra_delta_x);
double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcuData.delta_y - manual_delta_y - extra_delta_y); double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcu_data.delta_y - manual_delta_y - extra_delta_y);
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; // cout << "mcu_data.delta_x: " << mcu_data.delta_x << '\t' << "mcu_data.delta_y: " << mcu_data.delta_y << endl;
// cout << "manual delta: " << manual_delta_x << '\t' << manual_delta_y << endl; // cout << "manual delta: " << manual_delta_x << '\t' << manual_delta_y << endl;
} }

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

@@ -15,23 +15,23 @@ using std::vector;
// 此函数用于操作手手动标定 // 此函数用于操作手手动标定
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::changeMark() { void Energy::changeMark() {
if (mcuData.mark == 0 && last_mark == 1) {//完成标定 if (mcu_data.mark == 0 && last_mark == 1) {//完成标定
last_mark = mcuData.mark; last_mark = mcu_data.mark;
origin_yaw = mcuData.curr_yaw; origin_yaw = mcu_data.curr_yaw;
origin_pitch = mcuData.curr_pitch; origin_pitch = mcu_data.curr_pitch;
is_mark = false; is_mark = false;
manual_mark = true; manual_mark = true;
// LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark")); // LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
} else if (mcuData.mark == 1) {//正在标定 } else if (mcu_data.mark == 1) {//正在标定
last_mark = mcuData.mark; last_mark = mcu_data.mark;
is_mark = true; is_mark = true;
// LOGM(STR_CTR(WORD_BLUE,"Marking...")); // LOGM(STR_CTR(WORD_BLUE,"Marking..."));
} else {//未在标定 } else {//未在标定
last_mark = mcuData.mark; last_mark = mcu_data.mark;
is_mark = false; is_mark = false;
} }
//cout<<"mark: "<<int(mcuData.mark)<<endl; //cout<<"mark: "<<int(mcu_data.mark)<<endl;
} }

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

@@ -40,8 +40,8 @@ bool Energy::getOrigin() {
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) {
return false; return false;
} else { } else {
origin_yaw = mcuData.curr_yaw; origin_yaw = mcu_data.curr_yaw;
origin_pitch = mcuData.curr_pitch; origin_pitch = mcu_data.curr_pitch;
auto_mark = true; auto_mark = true;
LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!")); LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
return true; return true;

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

@@ -14,7 +14,7 @@ using namespace cv;
// 此函数用于判断世界坐标系下是否可以发弹 // 此函数用于判断世界坐标系下是否可以发弹
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInWorld() { void Energy::judgeShootInWorld() {
if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) { if (abs(yaw_rotation - mcu_data.curr_yaw) < 0.5 && abs(pitch_rotation - mcu_data.curr_pitch) < 0.5) {
shoot = 4; shoot = 4;
// is_predicting = false; // is_predicting = false;
// is_guessing = true; // is_guessing = true;

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

@@ -15,8 +15,8 @@ void Energy::writeDownSlightChange(cv::Mat &src) {
if (findFans(src) >= 4) { if (findFans(src) >= 4) {
FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w");
if (fp_delta) { if (fp_delta) {
fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcuData.delta_x + manual_delta_x, fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x,
mcuData.delta_y + manual_delta_y); mcu_data.delta_y + manual_delta_y);
fclose(fp_delta); fclose(fp_delta);
} }
} }

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

@@ -20,10 +20,10 @@ void Energy::sendEnergy() {
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
} else if (is_chassis) { } else if (is_chassis) {
sum_yaw += yaw_rotation - mcuData.curr_yaw; sum_yaw += yaw_rotation - mcu_data.curr_yaw;
sum_pitch += pitch_rotation - mcuData.curr_pitch; sum_pitch += pitch_rotation - mcu_data.curr_pitch;
yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw; yaw_rotation = AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch; pitch_rotation = AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + AIM_KI * sum_pitch;
} }
} }

16
main.cpp
View File

@@ -27,7 +27,7 @@
using namespace cv; using namespace cv;
using namespace std; using namespace std;
mcu_data mcuData = { // 单片机端回传结构体 McuData mcu_data = { // 单片机端回传结构体
0, // 当前云台yaw角 0, // 当前云台yaw角
0, // 当前云台pitch角 0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符 ARMOR_STATE, // 当前状态，自瞄-大符-小符
@@ -44,9 +44,9 @@ WrapperHead *video_chassis = nullptr; // 底盘摄像头视频源
Serial serial(115200); // 串口对象 Serial serial(115200); // 串口对象
uint8_t last_state = INIT_STATE; // 上次状态，用于初始化 uint8_t last_state = INIT_STATE; // 上次状态，用于初始化
// 自瞄主程序对象 // 自瞄主程序对象
ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier); ArmorFinder armorFinder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.use_classifier);
// 能量机关主程序对象 // 能量机关主程序对象
Energy energy(serial, mcuData.enemy_color); Energy energy(serial, mcu_data.enemy_color);
int box_distance = 0; int box_distance = 0;
@@ -94,7 +94,7 @@ 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 (mcu_data.state == BIG_ENERGY_STATE) {//大能量机关模式
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!")); LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!"));
destroyAllWindows(); destroyAllWindows();
@@ -110,7 +110,7 @@ int main(int argc, char *argv[]) {
checkReconnect(video_chassis->read(chassis_src)); checkReconnect(video_chassis->read(chassis_src));
energy.setBigEnergyInit(); energy.setBigEnergyInit();
} }
last_state = mcuData.state;//更新上一帧状态 last_state = mcu_data.state;//更新上一帧状态
ok = checkReconnect(video_gimbal->read(gimbal_src)); ok = checkReconnect(video_gimbal->read(gimbal_src));
video_chassis->read(chassis_src); video_chassis->read(chassis_src);
#ifdef GIMBAL_FLIP_MODE #ifdef GIMBAL_FLIP_MODE
@@ -124,7 +124,7 @@ int main(int argc, char *argv[]) {
if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
energy.runBig(gimbal_src, chassis_src); energy.runBig(gimbal_src, chassis_src);
// energy.runBig(gimbal_src); // energy.runBig(gimbal_src);
} else if (mcuData.state == SMALL_ENERGY_STATE) { } else if (mcu_data.state == SMALL_ENERGY_STATE) {
if (last_state != SMALL_ENERGY_STATE) { if (last_state != SMALL_ENERGY_STATE) {
LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!"));
destroyAllWindows(); destroyAllWindows();
@@ -139,7 +139,7 @@ int main(int argc, char *argv[]) {
} }
energy.setSmallEnergyInit(); energy.setSmallEnergyInit();
} }
last_state = mcuData.state;//更新上一帧状态 last_state = mcu_data.state;//更新上一帧状态
ok = checkReconnect(video_gimbal->read(gimbal_src)); 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);
@@ -162,7 +162,7 @@ int main(int argc, char *argv[]) {
} }
} }
} }
last_state = mcuData.state; last_state = mcu_data.state;
CNT_TIME(STR_CTR(WORD_GREEN, "read img"), { CNT_TIME(STR_CTR(WORD_GREEN, "read img"), {
if(!checkReconnect(video_gimbal->read(gimbal_src))) continue; if(!checkReconnect(video_gimbal->read(gimbal_src))) continue;
}); });

6
others/include/additions.h
View File

@@ -10,7 +10,7 @@
#include <serial.h> #include <serial.h>
#include <opencv2/core.hpp> #include <opencv2/core.hpp>
struct mcu_data { struct McuData {
float curr_yaw; float curr_yaw;
float curr_pitch; float curr_pitch;
uint8_t state; uint8_t state;
@@ -21,7 +21,7 @@ struct mcu_data {
int delta_y; int delta_y;
}; };
extern mcu_data mcuData; extern McuData mcu_data;
void uartReceive(Serial *pSerial); void uartReceive(Serial *pSerial);
@@ -43,6 +43,8 @@ void extract(cv::Mat &gimbal_src);
float getTimeIntervalms(const systime &now, const systime &last); float getTimeIntervalms(const systime &now, const systime &last);
double getPointLength(const cv::Point2f &p);
template<class type, int length> template<class type, int length>
class RoundQueue { class RoundQueue {
private: private:

16
others/src/additions.cpp
View File

@@ -34,12 +34,12 @@ void uartReceive(Serial *pSerial) {
LOGM(STR_CTR(WORD_LIGHT_WHITE, "data receive start!")); LOGM(STR_CTR(WORD_LIGHT_WHITE, "data receive start!"));
while (true) { while (true) {
memset(buffer, 0, sizeof(buffer)); memset(buffer, 0, sizeof(buffer));
pSerial->ReadData((uint8_t *) buffer, sizeof(mcuData)+1); pSerial->ReadData((uint8_t *) buffer, sizeof(mcu_data)+1);
if (buffer[sizeof(mcuData)] == '\n') { if (buffer[sizeof(mcu_data)] == '\n') {
memcpy(&mcuData, buffer, sizeof(mcuData)); memcpy(&mcu_data, buffer, sizeof(mcu_data));
// LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); // LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark);
// LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch); // LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch);
// LOGM("Get delta x: %d, delta y: %d!", mcuData.delta_x, mcuData.delta_y); // LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.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){
@@ -173,3 +173,7 @@ void extract(cv::Mat &gimbal_src) {//图像预处理，将视频切成640×480
float getTimeIntervalms(const systime &now, const systime &last){ float getTimeIntervalms(const systime &now, const systime &last){
return (now.second-last.second)*1000.0 + (now.millisecond-last.millisecond); return (now.second-last.second)*1000.0 + (now.millisecond-last.millisecond);
} }
double getPointLength(const cv::Point2f &p) {
return sqrt(p.x * p.x + p.y * p.y);
}

2
tools/TrainCNN/backward.py
View File

@@ -96,7 +96,7 @@ def train(dataset, show_bar=False):
_, loss_value, step = sess.run( _, loss_value, step = sess.run(
[train_op, loss, global_step], [train_op, loss, global_step],
feed_dict={x: images_samples, y_: labels_samples, keep_rate:0.4} feed_dict={x: images_samples, y_: labels_samples, keep_rate:0.3}
) )
if step % 500 == 0: if step % 500 == 0: