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 origin_yaw, origin_pitch;//初始的云台角度设定值
float shoot;//若为2，则要求主控板发弹
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
timeval time_start_guess;

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

@@ -47,6 +47,8 @@ void Energy::initEnergy() {
yaw_rotation = 0;
pitch_rotation = 0;
shoot = 0;
sum_yaw = 0;
sum_pitch = 0;
circle_center_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) {
double dx, dy;
//五号车
// double dx = -(target_point.x - 320 - 10);
// double dy = -(target_point.y - 240 - 22);
//四号车
#ifndef COMPENSATE_YAW
dx = -(target_point.x - 320 - COMPENSATE_YAW);
#endif
#ifndef COMPENSATE_PITCH
dy = -(target_point.y - 240 - COMPENSATE_PITCH);
#endif
double dx = -(target_point.x - 320 - COMPENSATE_YAW);
double dy = -(target_point.y - 240 - COMPENSATE_PITCH);
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
// 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
shoot = 2;
static float sum_yaw = 0;
static float sum_pitch = 0;
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
#ifndef AIM_KP
yaw_rotation = AIM_KP * yaw_rotation;
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
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
}