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-05-14 19:19:07 +08:00
parent 15871a9289
commit b1c4c4f862
3 changed files with 39 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -22,29 +22,39 @@ void Energy::gimbleRotation(){
stretch(hit_point, 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)); // 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)); // 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(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(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); mark_yaw = -1.41;
mark_pitch = 12.79;
if(position_mode == 1){ if(position_mode == 1){
pitch_rotation = 5.5+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(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));
} }
if(position_mode == 2){ else if(position_mode == 2){
pitch_rotation = 5.5+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(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));
} }
if(position_mode == 3){ else if(position_mode == 3){
pitch_rotation = 4.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); yaw_rotation = -0.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
pitch_rotation = 1.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
} }
if(position_mode == 4){ else if(position_mode == 4){
pitch_rotation = 4.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); yaw_rotation = -2+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
pitch_rotation = 1+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
} }
if(position_mode == 5){ else if(position_mode == 5){
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); yaw_rotation = -0.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
pitch_rotation = 0.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
} }
if(position_mode == 6){ else if(position_mode == 6){
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); yaw_rotation = -1+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
pitch_rotation = 1+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
else{
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));
} }
// else{
// pitch_rotation = 5.5+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)); // yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE));

12
energy/src/energy/get/hit_point_get.cpp
View File

@@ -19,7 +19,7 @@ void Energy::getHitPoint(){
if(target_armor>=0 && target_armor<=limit_angle){ if(target_armor>=0 && target_armor<=limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2); hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval; hit_position = -1*angle_interval;//-60
position_mode = 1; position_mode = 1;
// cout<<"666"<<'\t'<<hit_position<<endl; // cout<<"666"<<'\t'<<hit_position<<endl;
@@ -27,31 +27,31 @@ void Energy::getHitPoint(){
else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){ else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius); hit_point.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y; hit_point.y = cycle_center.y;
hit_position = 0; hit_position = 0;//0
position_mode = 2; position_mode = 2;
} }
else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2); hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval; hit_position = angle_interval;//60
position_mode = 3; position_mode = 3;
} }
else if(target_armor>=-180 && target_armor<-180+limit_angle){ else if(target_armor>=-180 && target_armor<-180+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2); hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval; hit_position = 2*angle_interval;//120
position_mode = 4; position_mode = 4;
} }
else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius); hit_point.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y; hit_point.y = cycle_center.y;
hit_position = 180; hit_position = 180;//180
position_mode = 5; position_mode = 5;
} }
else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) { else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
hit_point.x = cycle_center.x - static_cast<int>(radius / 2); hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2 * angle_interval; hit_position = -2 * angle_interval;//-120
position_mode = 6; position_mode = 6;
} }
else{ else{

16
main.cpp
View File

@@ -26,7 +26,7 @@ using namespace cv;
using namespace std; using namespace std;
int state = ARMOR_STATE; int state = ENERGY_STATE;
float curr_yaw = 0, curr_pitch = 0; float curr_yaw = 0, curr_pitch = 0;
float mark_yaw = 0, mark_pitch = 0; float mark_yaw = 0, mark_pitch = 0;
int mark = 0; int mark = 0;
@@ -126,7 +126,7 @@ void uartReceive(Uart *uart) {
while ((data = uart->receive()) != '\n') { while ((data = uart->receive()) != '\n') {
buffer[cnt++] = data; buffer[cnt++] = data;
if (cnt >= 100) { if (cnt >= 100) {
LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); // LOG(RECEIVE_LOG_LEVEL, "data receive over flow!");
cnt = 0; cnt = 0;
} }
} }
@@ -134,28 +134,28 @@ void uartReceive(Uart *uart) {
if (cnt == 11) { if (cnt == 11) {
if (buffer[8] == 'e') { if (buffer[8] == 'e') {
state = ENERGY_STATE; state = ENERGY_STATE;
LOG(RECEIVE_LOG_LEVEL, "Energy state"); // LOG(RECEIVE_LOG_LEVEL, "Energy state");
} else if (buffer[8] == 'a') { } else if (buffer[8] == 'a') {
state = ARMOR_STATE; state = ARMOR_STATE;
LOG(RECEIVE_LOG_LEVEL, "Armor state"); // LOG(RECEIVE_LOG_LEVEL, "Armor state");
} }
if (buffer[10] == 0){ if (buffer[10] == 0){
use_classifier = false; use_classifier = false;
LOG(RECEIVE_LOG_LEVEL, "Classifier off!"); // LOG(RECEIVE_LOG_LEVEL, "Classifier off!");
} else if(buffer[10] == 1){ } else if(buffer[10] == 1){
use_classifier = true; use_classifier = true;
LOG(RECEIVE_LOG_LEVEL, "Classifier on!"); // LOG(RECEIVE_LOG_LEVEL, "Classifier on!");
} }
memcpy(&curr_yaw, buffer, 4); memcpy(&curr_yaw, buffer, 4);
memcpy(&curr_pitch, buffer + 4, 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 (buffer[9] == 1) {
if (mark == 0) { if (mark == 0) {
mark = 1; mark = 1;
mark_yaw = curr_yaw; mark_yaw = curr_yaw;
mark_pitch = curr_pitch; mark_pitch = curr_pitch;
} }
LOG(RECEIVE_LOG_LEVEL, "Marked"); // LOG(RECEIVE_LOG_LEVEL, "Marked");
} }
} }
cnt = 0; cnt = 0;