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

反陀螺v1.1。

Browse Source
This commit is contained in:
xinyang
2019-07-20 11:10:25 +08:00
parent ccb87882f3
commit 206026b5d9
13 changed files with 26844 additions and 26828 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,31 +13,34 @@ void ArmorFinder::antiTop() {
static double top_periodms = 0;
static double last_top_periodms = 0;
timeval curr_time;
bool shoot = 0;
uint8_t 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(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<interval&&interval<(top_periodms+last_top_periodms)/2.0-70){
if(anti_top_state == ANTI_TOP && (top_periodms+last_top_periodms)/2.0-120<interval&&interval<(top_periodms+last_top_periodms)/2.0-80){
shoot = 1;
// LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
}else if(interval > 700){
anti_top_state = NORMAL;
anti_top_cnt = 0;
}
ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
if(orientation == ArmorBox::UNKNOWN){
if(anti_top_state == NORMAL){
sendBoxPosition(shoot, true);
return;
}else{
sendBoxPosition(shoot, false);
return;
}
}
if(orientation != last_orient){
if(interval > 700){
anti_top_cnt = 0;
@@ -64,10 +67,12 @@ void ArmorFinder::antiTop() {
if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){
sendBoxPosition(shoot);
sendBoxPosition(shoot, true);
}else if(shoot){
sendBoxPosition(shoot, false);
}
}else if(anti_top_state == NORMAL){
sendBoxPosition(shoot);
sendBoxPosition(shoot, true);
}

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

@@ -70,7 +70,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B3:
case B4:
case B5:
if (lengthDistanceRatio() < 0.45) {
if (lengthDistanceRatio() < 0.46) {
return FRONT;
}else{
return SIDE;

4
armor/src/armor_finder/find/find_light_blobs.cpp
View File

@@ -122,7 +122,7 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
color_channel = channels[0]; /* 根据目标颜色进行通道提取 */
else if (enemy_color == ENEMY_RED) /* */
color_channel = channels[2]; /************************/
cv::threshold(color_channel, src_bin, 170, 255, CV_THRESH_BINARY); // 二值化对应通道
cv::threshold(color_channel, src_bin, 140, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin); // 开闭运算
if(src_bin.size() == cv::Size(640, 480) && show_light_blobs)
@@ -137,4 +137,4 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
}
}
return light_blobs.size() >= 2;
}
}

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

@@ -3,6 +3,7 @@
//
#include <armor_finder/armor_finder.h>
#include<log.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t shoot) {
static short x_tmp, y_tmp, z_tmp;
@@ -35,7 +36,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t sho
return serial.WriteData(buff, sizeof(buff));
}
bool ArmorFinder::sendBoxPosition(bool shoot) {
bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) {
if(armor_box.rect == cv::Rect2d()) return false;
auto rect = armor_box.rect;
double dx = rect.x + rect.width / 2 - 320;
@@ -44,5 +45,15 @@ bool ArmorFinder::sendBoxPosition(bool shoot) {
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height;
// cout << yaw << endl;
return sendTarget(serial, yaw, -pitch, dist, shoot);
if(isTrack){
if(shoot){
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
}
return sendTarget(serial, yaw, -pitch, dist, shoot);
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
return sendTarget(serial, 0, -0, dist, 1);
}else if(shoot){
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
return sendTarget(serial, 0, 0, 4, 1);
}
}