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

反陀螺v1.0

Browse Source
This commit is contained in:
xinyang
2019-07-19 23:18:19 +08:00
parent 35080fb4ec
commit d0c49f43ad
3 changed files with 26 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -123,7 +123,7 @@ private:
public: public:
void run(cv::Mat &src); // 自瞄主函数 void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(); // 和主控板通讯 bool sendBoxPosition(bool shoot); // 和主控板通讯
}; };
#endif /* _ARMOR_FINDER_H_ */ #endif /* _ARMOR_FINDER_H_ */

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

@@ -10,25 +10,35 @@ static double getTimeIntervalms(const timeval& now, const timeval &last){
} }
void ArmorFinder::antiTop() { void ArmorFinder::antiTop() {
double top_periodms = 0; static double top_periodms = 0;
static double last_top_periodms = 0;
timeval curr_time; timeval curr_time;
if(anti_top_state == ANTI_TOP){ bool shoot = 0;
/*if(anti_top_state == ANTI_TOP){
cout << "anti top" << endl; cout << "anti top" << endl;
}else if(anti_top_state == NORMAL){ }else if(anti_top_state == NORMAL){
cout << "Normal" << endl; cout << "Normal" << endl;
} }*/
ArmorBox::BoxOrientation orientation = armor_box.getOrientation(); ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
if(orientation == ArmorBox::UNKNOWN){ if(orientation == ArmorBox::UNKNOWN){
if(anti_top_state == NORMAL){ if(anti_top_state == NORMAL){
sendBoxPosition(); sendBoxPosition(shoot);
return; return;
}else{ }else{
return; return;
} }
} }
gettimeofday(&curr_time, nullptr);
auto interval = getTimeIntervalms(curr_time, last_front_time);
LOGM("interval:%.1lf", interval);
if(anti_top_state == ANTI_TOP && (top_periodms+last_top_periodms)/2.0-130<interval&&interval<(top_periodms+last_top_periodms)/2.0-70){
shoot = 1;
// LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
}else if(interval > 700){
anti_top_state = NORMAL;
anti_top_cnt = 0;
}
if(orientation != last_orient){ if(orientation != last_orient){
gettimeofday(&curr_time, nullptr);
auto interval = getTimeIntervalms(curr_time, last_front_time);
if(interval > 700){ if(interval > 700){
anti_top_cnt = 0; anti_top_cnt = 0;
anti_top_state = NORMAL; anti_top_state = NORMAL;
@@ -39,6 +49,7 @@ void ArmorFinder::antiTop() {
if(orientation == ArmorBox::FRONT){ if(orientation == ArmorBox::FRONT){
anti_top_cnt++; anti_top_cnt++;
if(anti_top_state == ANTI_TOP){ if(anti_top_state == ANTI_TOP){
last_top_periodms = top_periodms;
top_periodms = interval; top_periodms = interval;
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms); LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
} }
@@ -53,10 +64,10 @@ void ArmorFinder::antiTop() {
if(anti_top_state == ANTI_TOP){ if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){ if(orientation == ArmorBox::FRONT){
sendBoxPosition(); sendBoxPosition(shoot);
} }
}else if(anti_top_state == NORMAL){ }else if(anti_top_state == NORMAL){
sendBoxPosition(); sendBoxPosition(shoot);
} }

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

@@ -4,11 +4,11 @@
#include <armor_finder/armor_finder.h> #include <armor_finder/armor_finder.h>
static bool sendTarget(Serial &serial, double x, double y, double z) { static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t shoot) {
static short x_tmp, y_tmp, z_tmp; static short x_tmp, y_tmp, z_tmp;
static time_t last_time = time(nullptr); static time_t last_time = time(nullptr);
static int fps; static int fps;
uint8_t buff[8]; uint8_t buff[9];
time_t t = time(nullptr); time_t t = time(nullptr);
if (last_time != t) { if (last_time != t) {
@@ -29,12 +29,13 @@ static bool sendTarget(Serial &serial, double x, double y, double 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[7] = 'e'; buff[7] = shoot;
buff[8] = 'e';
return serial.WriteData(buff, sizeof(buff)); return serial.WriteData(buff, sizeof(buff));
} }
bool ArmorFinder::sendBoxPosition() { bool ArmorFinder::sendBoxPosition(bool shoot) {
if(armor_box.rect == cv::Rect2d()) return false; if(armor_box.rect == cv::Rect2d()) return false;
auto rect = armor_box.rect; auto rect = armor_box.rect;
double dx = rect.x + rect.width / 2 - 320; double dx = rect.x + rect.width / 2 - 320;
@@ -43,5 +44,5 @@ bool ArmorFinder::sendBoxPosition() {
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height; double dist = DISTANCE_HEIGHT / rect.height;
// cout << yaw << endl; // cout << yaw << endl;
return sendTarget(serial, yaw, -pitch, dist); return sendTarget(serial, yaw, -pitch, dist, shoot);
} }