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);
// 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(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){
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){
pitch_rotation = 5.5+static_cast<float>(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<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){
pitch_rotation = 4.5+static_cast<float>(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<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){
pitch_rotation = 4.5+static_cast<float>(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<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){
pitch_rotation = 5.5+static_cast<float>(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<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){
pitch_rotation = 5.5+static_cast<float>(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<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));

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){
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_position = -1*angle_interval;
hit_position = -1*angle_interval;//-60
position_mode = 1;
// 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){
hit_point.x = cycle_center.x + static_cast<int>(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<int>(radius / 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;
}
else if(target_armor>=-180 && target_armor<-180+limit_angle){
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_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<int>(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<int>(radius / 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;
}
else{

16
main.cpp
View File

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