diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index f508f9a..3c70f8d 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -123,7 +123,7 @@ private: public: void run(cv::Mat &src); // 自瞄主函数 - bool sendBoxPosition(); // 和主控板通讯 + bool sendBoxPosition(bool shoot); // 和主控板通讯 }; #endif /* _ARMOR_FINDER_H_ */ diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 71f9afa..beb8b85 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -10,25 +10,35 @@ static double getTimeIntervalms(const timeval& now, const timeval &last){ } void ArmorFinder::antiTop() { - double top_periodms = 0; + static double top_periodms = 0; + static double last_top_periodms = 0; timeval curr_time; - if(anti_top_state == ANTI_TOP){ + bool shoot = 0; + /*if(anti_top_state == ANTI_TOP){ cout << "anti top" << endl; }else if(anti_top_state == NORMAL){ cout << "Normal" << endl; - } + }*/ ArmorBox::BoxOrientation orientation = armor_box.getOrientation(); if(orientation == ArmorBox::UNKNOWN){ if(anti_top_state == NORMAL){ - sendBoxPosition(); + sendBoxPosition(shoot); return; }else{ 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 700){ + anti_top_state = NORMAL; + anti_top_cnt = 0; + } if(orientation != last_orient){ - gettimeofday(&curr_time, nullptr); - auto interval = getTimeIntervalms(curr_time, last_front_time); if(interval > 700){ anti_top_cnt = 0; anti_top_state = NORMAL; @@ -39,6 +49,7 @@ void ArmorFinder::antiTop() { if(orientation == ArmorBox::FRONT){ anti_top_cnt++; if(anti_top_state == ANTI_TOP){ + last_top_periodms = top_periodms; top_periodms = interval; 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(orientation == ArmorBox::FRONT){ - sendBoxPosition(); + sendBoxPosition(shoot); } }else if(anti_top_state == NORMAL){ - sendBoxPosition(); + sendBoxPosition(shoot); } diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 5ef44d0..cc3b24c 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -4,11 +4,11 @@ #include -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 time_t last_time = time(nullptr); static int fps; - uint8_t buff[8]; + uint8_t buff[9]; time_t t = time(nullptr); if (last_time != t) { @@ -29,12 +29,13 @@ static bool sendTarget(Serial &serial, double x, double y, double z) { buff[4] = static_cast((y_tmp >> 0) & 0xFF); buff[5] = static_cast((z_tmp >> 8) & 0xFF); buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[7] = 'e'; + buff[7] = shoot; + buff[8] = 'e'; return serial.WriteData(buff, sizeof(buff)); } -bool ArmorFinder::sendBoxPosition() { +bool ArmorFinder::sendBoxPosition(bool shoot) { if(armor_box.rect == cv::Rect2d()) return false; auto rect = armor_box.rect; double dx = rect.x + rect.width / 2 - 320; @@ -43,5 +44,5 @@ bool ArmorFinder::sendBoxPosition() { double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; double dist = DISTANCE_HEIGHT / rect.height; // cout << yaw << endl; - return sendTarget(serial, yaw, -pitch, dist); + return sendTarget(serial, yaw, -pitch, dist, shoot); }