VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

win32 ver

Browse Source
This commit is contained in:
xinyang
2019-05-14 20:03:03 +08:00
parent 601bf2a510
commit 6980145e83
28 changed files with 13037 additions and 395 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
energy/include/energy/energy.h
View File

@@ -14,19 +14,19 @@
#include <sys/timeb.h>
#include "energy/constant.h"
#include "energy/param_struct_define.h"
#include "uart/uart.h"
#include "serial/serial.h"
using std::vector;
class Energy {
public:
Energy(Uart &u);
Energy(Serial &u, int &ally_color);
~Energy();
int run(cv::Mat &src);
cv::Point2f uart_hit_point;
clock_t start;
Uart &uart;
Serial &serial;
void setAllyColor(int color);
void setRotation(int rotation);
@@ -51,7 +51,7 @@ private:
double last_target_position;
double last_hit_position;
float target_armor;
int ally_color_;
int &ally_color_;
int energy_part_rotation;
float attack_distance;
int send_cnt;

2
energy/src/energy/energy.cpp
View File

@@ -8,7 +8,7 @@ using std::cout;
using std::endl;
using std::vector;
Energy::Energy(Uart &u):uart(u),
Energy::Energy(Serial &u, int &ally_color):serial(u),ally_color_(ally_color),
src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
{

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

@@ -13,48 +13,37 @@ extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
void Energy::gimbleRotation(){
//该方法用于标定激光零点的情况，对操作手友好，但建立在云台稳定情况下
// yaw_rotation = static_cast<float>(180 / PI * atan2(-1*STRETCH*(hit_point.x-ZERO_POINT_X), ATTACK_DISTANCE));
// 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;
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(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));
mark_yaw = -1.41;
mark_pitch = 12.79;
if(position_mode == 1){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), 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));
}
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));
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 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));
}
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));
}
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));
}
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));
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 if(position_mode == 4){
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 if(position_mode == 5){
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 if(position_mode == 6){
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));

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

@@ -15,53 +15,50 @@ void Energy::getHitPoint(){
int limit_angle = 6;
int angle_interval = 60;
if(energy_part_rotation==1){ //顺时针
if(energy_part_rotation==1){
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;//-60
hit_position = -1*angle_interval;
position_mode = 1;
// cout<<"666"<<'\t'<<hit_position<<endl;
}
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;//0
hit_position = 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;//60
hit_position = angle_interval;
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;//120
hit_position = 2*angle_interval;
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;//180
hit_position = 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;//-120
hit_position = -2 * angle_interval;
position_mode = 6;
}
else{
// last_hit_position = 2000;
position_mode = 0;
return;
}
}
if(energy_part_rotation==-1){ //逆时针
if(energy_part_rotation==-1){
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);
@@ -99,55 +96,26 @@ void Energy::getHitPoint(){
position_mode = 6;
}
else{
// last_hit_position = 2000;
position_mode = 0;
return;
}
}
// cout<<"last: "<<last_position_mode<<'\t'<<"now: "<<position_mode<<endl;
if(position_mode!=0 && position_mode!=last_position_mode){
// last_hit_position = hit_position;
last_position_mode = position_mode;
isSendTarget = true;
cout<<"hit position: "<<hit_position<<endl;
return;
}
else if(position_mode == 0){
// last_hit_position = hit_position;
// last_position_mode = position_mode;
isSendTarget = false;
return;
}
else{
// last_hit_position = hit_position;
last_position_mode = position_mode;
isSendTarget = false;
return;
}
// if(hit_position>360&&last_hit_position>360){
// isSendTarget = false;
// return;
// }
// if(fabs(hit_position - last_hit_position) < 15||fabs(hit_position - last_hit_position) > 345 && fabs(hit_position - last_hit_position) <= 360){
// last_hit_position = hit_position;
// isSendTarget = false;
// return;
// }
// else{
// last_hit_position = hit_position;
// isSendTarget = true;
// cout<<"hit position: "<<hit_position<<endl;
// return;
// }
// double rad = static_cast<double>(energy_part_rotation * energy_part_param_.RPM
// * energy_part_param_.HIT_TIME * 360 / 60);
// rotate(rad, radius, cycle_center, target_center, hit_point);
}
bool Energy::changeTarget(){

32
energy/src/energy/send_target_by_uart/send_target_by_uart.cpp
View File

@@ -3,11 +3,41 @@
//
#include "energy/energy.h"
bool sendTarget(Serial& serial, float x, float y, float z) {
static short x_tmp, y_tmp, z_tmp;
static time_t last_time = time(nullptr);
static int fps;
uint8_t buff[8];
time_t t = time(nullptr);
if (last_time != t) {
last_time = t;
std::cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << std::endl;
fps = 0;
}
fps += 1;
x_tmp = static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e';
return serial.WriteData(buff, sizeof(buff));
}
void Energy::sendTargetByUart(float x, float y, float z) {
// if(!changeTarget()){
// return;
// }
uart.sendTarget(x, y, z);
sendTarget(serial, x, y, z);
send_cnt+=1;
}