VSAG/amadeus_26_fb
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

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);