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

fix bug.

Browse Source
This commit is contained in:
xinyang
2019-08-04 23:53:04 +08:00
parent 4b8228e9fd
commit 125c8d36c9
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
energy/src/energy/send/send.cpp
View File

@@ -28,12 +28,12 @@ void Energy::sendEnergy() {
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
} else if (is_chassis) {
sum_yaw += yaw_rotation - mcuData.curr_yaw;
sum_pitch += pitch_rotation - mcuData.curr_pitch;
sum_yaw += yaw_rotation - mcu_data.curr_yaw;
sum_pitch += pitch_rotation - mcu_data.curr_pitch;
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw;
pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch;
yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw;
pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}