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

energy changed(8/7game PID test)

Browse Source
This commit is contained in:
sun
2019-08-07 03:38:31 +08:00
parent 0257b87720
commit f93f5911ff
9 changed files with 45 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,20 +24,30 @@ void Energy::sendEnergy() {
} 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);
// MINMAX(yaw_I_component, -2, 2);
// MINMAX(pitch_I_component, -2, 2);
// MINMAX(yaw_I_component, -3, 3);
// MINMAX(pitch_I_component, -3, 3);
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
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);
// cout << BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
if(mcu_data.mark == 1){
yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
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);
}
// cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
// cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
if (ROBOT_ID == 7) {