VSAG/vision_sjtu_19
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_yaw += yaw_rotation;
sum_pitch += pitch_rotation; sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100); 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); 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 + pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
} else if (is_chassis) { } else if (is_chassis) {
sum_yaw += yaw_rotation - mcuData.curr_yaw; sum_yaw += yaw_rotation - mcuData.curr_yaw;
sum_pitch += pitch_rotation - mcuData.curr_pitch; 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; 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; 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){ } else if (is_small){
sum_yaw += yaw_rotation; sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation; sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100); 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 = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); 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); 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) #define ENERGY_CAMERA_GAIN (20)
#endif #endif
#ifndef SMALL_YAW_AIM_KD #ifndef SMALL_YAW_AIM_KD
#define SMALL_YAW_AIM_KD (1.5) #define SMALL_YAW_AIM_KD (0)
#endif #endif
#ifndef SMALL_YAW_AIM_KP #ifndef SMALL_YAW_AIM_KP
#define SMALL_YAW_AIM_KP (2.5) #define SMALL_YAW_AIM_KP (4)
#endif #endif
#ifndef SMALL_PITCH_AIM_KD #ifndef SMALL_PITCH_AIM_KD
#define SMALL_PITCH_AIM_KD (1.3) #define SMALL_PITCH_AIM_KD (0)
#endif #endif
#ifndef SMALL_PITCH_AIM_KP #ifndef SMALL_PITCH_AIM_KP
#define SMALL_PITCH_AIM_KP (2.4) #define SMALL_PITCH_AIM_KP (3.7)
#endif #endif
#ifndef BIG_YAW_AIM_KD #ifndef BIG_YAW_AIM_KD
#define BIG_YAW_AIM_KD (1) #define BIG_YAW_AIM_KD (0)
#endif #endif
#ifndef BIG_YAW_AIM_KP #ifndef BIG_YAW_AIM_KP
#define BIG_YAW_AIM_KP (5.5) #define BIG_YAW_AIM_KP (6.5)
#endif #endif
#ifndef BIG_YAW_AIM_KI #ifndef BIG_YAW_AIM_KI
#define BIG_YAW_AIM_KI (0.1) #define BIG_YAW_AIM_KI (0.1)
#endif #endif
#ifndef BIG_PITCH_AIM_KD #ifndef BIG_PITCH_AIM_KD
#define BIG_PITCH_AIM_KD (1) #define BIG_PITCH_AIM_KD (0)
#endif #endif
#ifndef BIG_PITCH_AIM_KP #ifndef BIG_PITCH_AIM_KP
#define BIG_PITCH_AIM_KP (5.5) #define BIG_PITCH_AIM_KP (6.5)
#endif #endif
#ifndef BIG_PITCH_AIM_KI #ifndef BIG_PITCH_AIM_KI
#define BIG_PITCH_AIM_KI (0.1) #define BIG_PITCH_AIM_KI (0.1)