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

Auto Aim

Browse Source
This commit is contained in:
JiatongSun
2019-04-15 00:07:10 +08:00
parent ecc3bdcd9e
commit 15c7894c51
5 changed files with 30 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5)
project(auto-aim)
set(CMAKE_CXX_STANDARD 11)
SET(CMAKE_BUILD_TYPE RELEASE)
SET(CMAKE_BUILD_TYPE DEBUG)
FIND_PROGRAM(CCACHE_FOUND ccache)
IF(CCACHE_FOUND)

6
energy/include/energy/constant.h
View File

@@ -17,8 +17,8 @@ const int CLOCKWISE = 1;
const int ANTICLOCKWISE = -1;
const float ATTACK_DISTANCE = 770;//cm
const double WHOLE_FAN = 80;//cm
//const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
//const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
const int EXTRACT_POINT_X = 200;
const int EXTRACT_POINT_Y = 20;
const int EXTRACT_WIDTH = 240;
@@ -30,7 +30,7 @@ const int GM_X = 15;//云台摄像头x方向
const int GM_H = 16;//云台摄像头y方向
//const double STRETCH = 231.0/640.0;//实际距离与图像伸缩比
const double STRETCH = 231.0/640.0;
const double STRETCH = 0.34;
const int ZERO_POINT_X = 281;
const int ZERO_POINT_Y = 188;

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

@@ -9,6 +9,8 @@ using std::cout;
using std::endl;
using std::vector;
extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
void Energy::gimbleRotation(){
//该方法用于标定激光零点的情况，对操作手友好，但建立在云台稳定情况下
@@ -16,11 +18,16 @@ void Energy::gimbleRotation(){
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-STRETCH*(hit_point.y-ZERO_POINT_Y)), ATTACK_DISTANCE));
//该方法用于操作手自己完成对心工作的情况，对操作手要求高
cv::Point2f real_hit_point;
stretch(hit_point, real_hit_point);
// cv::Point2f real_hit_point;
// stretch(hit_point, real_hit_point);
// yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(YAW_ORIGIN_RAD)-real_hit_point.x), ATTACK_DISTANCE));
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE));
yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(aim.yaw)-real_hit_point.x), ATTACK_DISTANCE));
pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(aim.pitch)-real_hit_point.y), ATTACK_DISTANCE));
// yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(curr_yaw)-real_hit_point.x), ATTACK_DISTANCE));
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(curr_pitch)-real_hit_point.y), ATTACK_DISTANCE));
yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE));
pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(9.1*PI/180)-STRETCH*(hit_point.y-cycle_center.y)), ATTACK_DISTANCE));
cout<<"cur: "<<curr_yaw<<endl;
cout<<"hit point: "<<hit_point<<'\t'<<"cycle center: "<<cycle_center<<endl;
}

15
energy/src/energy/run.cpp
View File

@@ -24,7 +24,7 @@ int Energy::run(cv::Mat &src){
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",src);
/*
fans_cnt = findFan(src, fans, last_fans_cnt);
// cout<<"fans_cnt: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧
@@ -39,14 +39,16 @@ int Energy::run(cv::Mat &src){
if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
if(armors_cnt != fans_cnt+1) return 0;
*/
//此处用于标定云台在摄像头视频中的零点
// findGimbleZeroPoint(src,gimble_zero_points);
// cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
// showFanContours("zero",src,gimble_zero_points);
findGimbleZeroPoint(src,gimble_zero_points);
cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
showFanContours("zero",src,gimble_zero_points);
cycle_center = cv::Point(291,305);
if(gimble_zero_points.size()>0)hit_point = gimble_zero_points.at(0).rect.center;
/*
getAllArmorCenters();
cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit();
@@ -62,6 +64,7 @@ int Energy::run(cv::Mat &src){
getHitPoint();
cout << "The hit point position is " << hit_point << endl;
// hit_point = cycle_center;
*/
gimbleRotation();
sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);

11
main.cpp
View File

@@ -13,6 +13,7 @@
#include "camera/wrapper_head.h"
#include "armor_finder/armor_finder.h"
#include <options/options.h>
//#define LOG_LEVEL LOG_WARRING
#include <log.h>
#include <thread>
@@ -23,7 +24,7 @@ using namespace std;
#define ENERGY_STATE 1
#define ARMOR_STATE 0
int state = ARMOR_STATE;
int state = ENERGY_STATE;
float curr_yaw=0, curr_pitch=0;
float mark_yaw=0, mark_pitch=0;
@@ -98,16 +99,16 @@ void uartReceive(Uart* uart){
while((data=uart->receive()) != '\n'){
buffer[cnt++] = data;
if(cnt >= 100){
LOGE("data receive over flow!");
// LOGE("data receive over flow!");
}
}
if(cnt == 10){
if(buffer[8] == 'e'){
state = ENERGY_STATE;
LOGM("Energy state");
// LOGM("Energy state");
}else if(buffer[8] == 'a'){
state = ARMOR_STATE;
LOGM("Armor state");
// LOGM("Armor state");
}
memcpy(&curr_yaw, buffer, 4);
memcpy(&curr_pitch, buffer+4, 4);
@@ -115,7 +116,7 @@ void uartReceive(Uart* uart){
if(buffer[9] == 1){
mark_yaw = curr_yaw;
mark_pitch = curr_pitch;
LOGM("Marked");
// LOGM("Marked");
}
}
cnt = 0;