From b1c4c4f862f13d72ac3643e57455c9db40722d02 Mon Sep 17 00:00:00 2001 From: JiatongSun Date: Tue, 14 May 2019 19:19:07 +0800 Subject: [PATCH] Auto Aim --- energy/src/energy/get/gimble_rotation_get.cpp | 40 ++++++++++++------- energy/src/energy/get/hit_point_get.cpp | 12 +++--- main.cpp | 16 ++++---- 3 files changed, 39 insertions(+), 29 deletions(-) diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index 977648c..b19f644 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -22,29 +22,39 @@ void Energy::gimbleRotation(){ stretch(hit_point, real_hit_point); // yaw_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(YAW_ORIGIN_RAD)-real_hit_point.x), ATTACK_DISTANCE)); // pitch_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE)); +// yaw_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); - yaw_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + mark_yaw = -1.41; + mark_pitch = 12.79; if(position_mode == 1){ - pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); } - if(position_mode == 2){ - pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + else if(position_mode == 2){ + yaw_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); } - if(position_mode == 3){ - pitch_rotation = 4.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + else if(position_mode == 3){ + yaw_rotation = -0.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = 1.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); } - if(position_mode == 4){ - pitch_rotation = 4.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + else if(position_mode == 4){ + yaw_rotation = -2+static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = 1+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); } - if(position_mode == 5){ - pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + else if(position_mode == 5){ + yaw_rotation = -0.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = 0.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); } - if(position_mode == 6){ - pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + else if(position_mode == 6){ + yaw_rotation = -1+static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = 1+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } + else{ + yaw_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); } -// else{ -// pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } // yaw_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE)); diff --git a/energy/src/energy/get/hit_point_get.cpp b/energy/src/energy/get/hit_point_get.cpp index 552c5a1..a8c495b 100644 --- a/energy/src/energy/get/hit_point_get.cpp +++ b/energy/src/energy/get/hit_point_get.cpp @@ -19,7 +19,7 @@ void Energy::getHitPoint(){ if(target_armor>=0 && target_armor<=limit_angle){ hit_point.x = cycle_center.x + static_cast(radius / 2); hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); - hit_position = -1*angle_interval; + hit_position = -1*angle_interval;//-60 position_mode = 1; // cout<<"666"<<'\t'<=angle_interval && target_armor(radius); hit_point.y = cycle_center.y; - hit_position = 0; + hit_position = 0;//0 position_mode = 2; } else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ hit_point.x = cycle_center.x + static_cast(radius / 2); hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); - hit_position = angle_interval; + hit_position = angle_interval;//60 position_mode = 3; } else if(target_armor>=-180 && target_armor<-180+limit_angle){ hit_point.x = cycle_center.x - static_cast(radius / 2); hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); - hit_position = 2*angle_interval; + hit_position = 2*angle_interval;//120 position_mode = 4; } else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ hit_point.x = cycle_center.x - static_cast(radius); hit_point.y = cycle_center.y; - hit_position = 180; + hit_position = 180;//180 position_mode = 5; } else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) { hit_point.x = cycle_center.x - static_cast(radius / 2); hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); - hit_position = -2 * angle_interval; + hit_position = -2 * angle_interval;//-120 position_mode = 6; } else{ diff --git a/main.cpp b/main.cpp index 5b5ae10..ed66935 100644 --- a/main.cpp +++ b/main.cpp @@ -26,7 +26,7 @@ using namespace cv; using namespace std; -int state = ARMOR_STATE; +int state = ENERGY_STATE; float curr_yaw = 0, curr_pitch = 0; float mark_yaw = 0, mark_pitch = 0; int mark = 0; @@ -126,7 +126,7 @@ void uartReceive(Uart *uart) { while ((data = uart->receive()) != '\n') { buffer[cnt++] = data; if (cnt >= 100) { - LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); +// LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); cnt = 0; } } @@ -134,28 +134,28 @@ void uartReceive(Uart *uart) { if (cnt == 11) { if (buffer[8] == 'e') { state = ENERGY_STATE; - LOG(RECEIVE_LOG_LEVEL, "Energy state"); +// LOG(RECEIVE_LOG_LEVEL, "Energy state"); } else if (buffer[8] == 'a') { state = ARMOR_STATE; - LOG(RECEIVE_LOG_LEVEL, "Armor state"); +// LOG(RECEIVE_LOG_LEVEL, "Armor state"); } if (buffer[10] == 0){ use_classifier = false; - LOG(RECEIVE_LOG_LEVEL, "Classifier off!"); +// LOG(RECEIVE_LOG_LEVEL, "Classifier off!"); } else if(buffer[10] == 1){ use_classifier = true; - LOG(RECEIVE_LOG_LEVEL, "Classifier on!"); +// LOG(RECEIVE_LOG_LEVEL, "Classifier on!"); } memcpy(&curr_yaw, buffer, 4); memcpy(&curr_pitch, buffer + 4, 4); - LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); +// LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); if (buffer[9] == 1) { if (mark == 0) { mark = 1; mark_yaw = curr_yaw; mark_pitch = curr_pitch; } - LOG(RECEIVE_LOG_LEVEL, "Marked"); +// LOG(RECEIVE_LOG_LEVEL, "Marked"); } } cnt = 0;