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:
void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(); // 和主控板通讯
bool sendBoxPosition(bool shoot); // 和主控板通讯
};
#endif /* _ARMOR_FINDER_H_ */

25
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() {
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;
}
}
if(orientation != last_orient){
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(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);
}

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

@@ -4,11 +4,11 @@
#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 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<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 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));
}
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);
}