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-05 23:51:41 +08:00
parent 74fa004567
commit 00fe7bbab9
2 changed files with 23 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,12 +18,16 @@ void Energy::sendEnergy() {
if (camera_cnt == 1) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
// 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);
// MINMAX(pitch_I_component, -3, 3);
if(ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8){
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if(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);
MINMAX(pitch_I_component, -3, 3);
}
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);
@@ -31,8 +35,10 @@ void Energy::sendEnergy() {
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
// MINMAX(yaw_rotation, -6, 6);
// MINMAX(pitch_rotation, -6, 6);
if(ROBOT_ID == 7){
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
} else if (is_chassis) {
// sum_yaw += yaw_rotation - mcu_data.curr_yaw;
// sum_pitch += pitch_rotation - mcu_data.curr_pitch;
@@ -48,8 +54,10 @@ void Energy::sendEnergy() {
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);
// MINMAX(yaw_rotation, -6, 6);
// MINMAX(pitch_rotation, -6, 6);
if(ROBOT_ID == 7){
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}

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

@@ -31,6 +31,11 @@
#ifndef ENERGY_CAMERA_GAIN
#define ENERGY_CAMERA_GAIN (20)
#endif
#ifndef ROBOT_ID
#define ROBOT_ID (0)
#endif
#ifndef SMALL_YAW_AIM_KD
#define SMALL_YAW_AIM_KD (0.35)
#endif