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-19 23:50:08 +08:00
parent 6f517f7853
commit 6453a977e1
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -44,8 +44,12 @@ void Energy::judgeShootInGimbal(){
static float sum_pitch = 0;
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
#ifndef AIM_KP AIM_KI
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
#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
}