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 17:15:16 +08:00
parent 49b3f212b0
commit 2e03e80653
4 changed files with 33 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,16 +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(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(mark_yaw*PI/180)-real_hit_point.x), ATTACK_DISTANCE));
pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(mark_pitch*PI/180)-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;
// 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;
}

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

@@ -8,12 +8,18 @@ using std::cout;
using std::endl;
using std::vector;
extern float mark_yaw, mark_pitch;
extern int mark;
int Energy::run(cv::Mat &src){
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
gimble_zero_points.clear();
if(mark==0)return 0;
// if(all_armor_centers.size()>200)all_armor_centers.clear();
// if(first_armor_centers.size()>200)first_armor_centers.clear();
// cout<<"first_armor_centers.size(): "<<first_armor_centers.size()<<endl;
@@ -24,7 +30,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,22 +45,24 @@ 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);
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();
// cycle_center = cv::Point(248,247);
// radius = 208.439;
cycle_center = cv::Point(416,227);
radius = 203.665;
getFanPosition(fanPosition, fans, cycle_center, radius);
getArmorPosition(armorPosition, armors, cycle_center, radius);
findTarget(fanPosition, armorPosition, target_armor);
@@ -62,14 +70,17 @@ int Energy::run(cv::Mat &src){
cout<<"The target armor center is: "<<target_center<<endl;
getHitPoint();
hit_point = target_center;
cout << "The hit point position is " << hit_point << endl;
// hit_point = cycle_center;
*/
gimbleRotation();
sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);
cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;
cout<<"mark_yaw: "<<mark_yaw<<'\t'<<"mark_pitch: "<<mark_pitch<<endl;
cout<<"send_cnt: "<<send_cnt<<endl;

4
energy/src/energy/show/show.cpp
View File

@@ -88,6 +88,10 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat &src, cons
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
// cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle;
// cout << endl;
cv::Point2f point = armor.rect.center;
cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
}
imshow(windows_name, image2show);

4
main.cpp
View File

@@ -27,6 +27,7 @@ using namespace std;
int state = ENERGY_STATE;
float curr_yaw=0, curr_pitch=0;
float mark_yaw=0, mark_pitch=0;
int mark = 0;
void uartReceive(Uart* uart);
@@ -112,8 +113,9 @@ void uartReceive(Uart* uart){
}
memcpy(&curr_yaw, buffer, 4);
memcpy(&curr_pitch, buffer+4, 4);
LOGM("Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
// LOGM("Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
if(buffer[9] == 1){
mark = 1;
mark_yaw = curr_yaw;
mark_pitch = curr_pitch;
// LOGM("Marked");