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-04 23:36:51 +08:00
parent 6996d3f0dd
commit 6e69bb0bd2
2 changed files with 21 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -19,23 +19,35 @@ void Energy::sendEnergy() {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_pitch, -100, 100);\
MINMAX(sum_pitch, -100, 100);
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
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;
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;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}
} else if (is_small){
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_pitch, -100, 100);
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
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);
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}

16
others/include/config/setconfig.h
View File

@@ -32,31 +32,31 @@
#define ENERGY_CAMERA_GAIN (20)
#endif
#ifndef SMALL_YAW_AIM_KD
#define SMALL_YAW_AIM_KD (1.5)
#define SMALL_YAW_AIM_KD (0)
#endif
#ifndef SMALL_YAW_AIM_KP
#define SMALL_YAW_AIM_KP (2.5)
#define SMALL_YAW_AIM_KP (4)
#endif
#ifndef SMALL_PITCH_AIM_KD
#define SMALL_PITCH_AIM_KD (1.3)
#define SMALL_PITCH_AIM_KD (0)
#endif
#ifndef SMALL_PITCH_AIM_KP
#define SMALL_PITCH_AIM_KP (2.4)
#define SMALL_PITCH_AIM_KP (3.7)
#endif
#ifndef BIG_YAW_AIM_KD
#define BIG_YAW_AIM_KD (1)
#define BIG_YAW_AIM_KD (0)
#endif
#ifndef BIG_YAW_AIM_KP
#define BIG_YAW_AIM_KP (5.5)
#define BIG_YAW_AIM_KP (6.5)
#endif
#ifndef BIG_YAW_AIM_KI
#define BIG_YAW_AIM_KI (0.1)
#endif
#ifndef BIG_PITCH_AIM_KD
#define BIG_PITCH_AIM_KD (1)
#define BIG_PITCH_AIM_KD (0)
#endif
#ifndef BIG_PITCH_AIM_KP
#define BIG_PITCH_AIM_KP (5.5)
#define BIG_PITCH_AIM_KP (6.5)
#endif
#ifndef BIG_PITCH_AIM_KI
#define BIG_PITCH_AIM_KI (0.1)