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

energy change

Browse Source
This commit is contained in:
sun
2019-07-20 10:03:07 +08:00
parent 6453a977e1
commit ccb87882f3
4 changed files with 7 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
energy/include/energy/energy.h
View File

@@ -80,6 +80,7 @@ private:
float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
float origin_yaw, origin_pitch;//初始的云台角度设定值 float origin_yaw, origin_pitch;//初始的云台角度设定值
float shoot;//若为2，则要求主控板发弹 float shoot;//若为2，则要求主控板发弹
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
timeval time_start_guess; timeval time_start_guess;

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

@@ -47,6 +47,8 @@ void Energy::initEnergy() {
yaw_rotation = 0; yaw_rotation = 0;
pitch_rotation = 0; pitch_rotation = 0;
shoot = 0; shoot = 0;
sum_yaw = 0;
sum_pitch = 0;
circle_center_point = Point(0, 0); circle_center_point = Point(0, 0);
target_point = Point(0, 0); target_point = Point(0, 0);

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

@@ -13,17 +13,12 @@ using namespace cv;
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::getAimPoint(cv::Point target_point) { void Energy::getAimPoint(cv::Point target_point) {
double dx, dy;
//五号车 //五号车
// double dx = -(target_point.x - 320 - 10); // double dx = -(target_point.x - 320 - 10);
// double dy = -(target_point.y - 240 - 22); // double dy = -(target_point.y - 240 - 22);
//四号车 //四号车
#ifndef COMPENSATE_YAW double dx = -(target_point.x - 320 - COMPENSATE_YAW);
dx = -(target_point.x - 320 - COMPENSATE_YAW); double dy = -(target_point.y - 240 - COMPENSATE_PITCH);
#endif
#ifndef COMPENSATE_PITCH
dy = -(target_point.y - 240 - COMPENSATE_PITCH);
#endif
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;

12
energy/src/energy/judge/judge_shoot.cpp
View File

@@ -40,16 +40,8 @@ void Energy::judgeShootInGimbal(){
} else } else
shoot = 2; shoot = 2;
static float sum_yaw = 0;
static float sum_pitch = 0;
sum_yaw += yaw_rotation; sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation; sum_pitch += pitch_rotation;
#ifndef AIM_KP yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
yaw_rotation = AIM_KP * yaw_rotation; pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
pitch_rotation = AIM_KP * pitch_rotation;
#endif
#ifndef AIM_KI
yaw_rotation = yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = pitch_rotation * pitch_rotation + AIM_KI * sum_pitch;
#endif
} }