VSAG/amadeus_26_fb
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-08-07 20:36:04 +08:00
parent bbff755124
commit 16efe17334
2 changed files with 15 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,10 +18,10 @@ void Energy::sendEnergy() {
if (camera_cnt == 1) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) {
if (ROBOT_ID == 4 || ROBOT_ID == 8) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if (ROBOT_ID == 7) {
} else if (ROBOT_ID == 3 || ROBOT_ID == 7) {
float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
// MINMAX(yaw_I_component, -3, 3);
@@ -50,7 +50,7 @@ void Energy::sendEnergy() {
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
if (ROBOT_ID == 7) {
if (ROBOT_ID == 3 || ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
@@ -68,14 +68,18 @@ void Energy::sendEnergy() {
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) {
yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KI * sum_yaw +
TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation =
TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KI * sum_pitch +
TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KI * sum_yaw +
SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KI * sum_pitch +
SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
if (ROBOT_ID == 7) {
if (ROBOT_ID == 3 || ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}