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

energy changed

Browse Source
This commit is contained in:
sun
2019-07-29 19:17:53 +08:00
parent ffd5d74856
commit 00351599b1
3 changed files with 28 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
energy/include/energy/energy.h
View File

@@ -73,7 +73,7 @@ private:
int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
int last_mode;//上一帧的能量机关状态
int manual_delta_x, manual_delta_y;//手动微调量
int extra_delta_y;//在风车运动到最高点附近的额外补偿量
int extra_delta_x, extra_delta_y;//在风车运动到最高点附近的额外补偿量
float target_polar_angle;//待击打装甲板的极坐标角度
float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度

3
energy/src/energy/clear/energy_init.cpp
View File

@@ -39,6 +39,7 @@ void Energy::initEnergy() {
manual_delta_x = 0;
manual_delta_y = 0;
extra_delta_y = 0;
extra_delta_x = 0;
target_polar_angle = -1000;
last_target_polar_angle = -1000;
@@ -76,7 +77,7 @@ void Energy::initEnergy() {
// 此函数对能量机关参数进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.GRAY_THRESH = 90;//home
gimbal_energy_part_param_.GRAY_THRESH = 120;//home
// gimbal_energy_part_param_.GRAY_THRESH = 200;//official
// gimbal_energy_part_param_.GRAY_THRESH = 225;
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230;

28
energy/src/energy/get/aim_point_get.cpp
View File

@@ -17,10 +17,32 @@ extern mcu_data mcuData;
void Energy::getAimPoint(cv::Point target_point_) {
float target_polar_angle_ = static_cast<float>(180 / PI * atan2(-1 * (target_point_.y - circle_center_point.y),
(target_point_.x - circle_center_point.x)));
if (target_polar_angle_ > 50 && target_polar_angle_ < 130)extra_delta_y = -16;
else extra_delta_y = 0;
if (target_polar_angle_ > 40 && target_polar_angle_ < 140){
extra_delta_x = 0;
extra_delta_y = -14;
}
else if (target_polar_angle_ > 30 && target_polar_angle_ <= 40){
extra_delta_x = 8;
extra_delta_y = -8;
}
else if (target_polar_angle_ >= 140 && target_polar_angle_ < 150){
extra_delta_x = -8;
extra_delta_y = -8;
}
else {
extra_delta_x = 0;
extra_delta_y = 0;
}
double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x);
// if (target_polar_angle_ > 20 && target_polar_angle_ <= 90) {
// extra_delta_y = -17 * (target_polar_angle_ - 20) / 70;
// } else if (target_polar_angle_ > 90 && target_polar_angle_ < 160) {
// extra_delta_y = -17 * (160 - target_polar_angle_) / 70;
// } else {
// extra_delta_y = 0;
// }
double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x - extra_delta_x);
double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcuData.delta_y - manual_delta_y - extra_delta_y);
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;