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-01 02:36:03 +08:00
parent cdd6716214
commit b6f3f04b03
4 changed files with 32 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
energy/include/energy/energy.h
View File

@@ -85,6 +85,7 @@ private:
float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
float origin_yaw, origin_pitch;//初始的云台角度设定值 float origin_yaw, origin_pitch;//初始的云台角度设定值
float shoot;//若为2，则要求主控板发弹 float shoot;//若为2，则要求主控板发弹
float last_yaw, last_pitch;//PID中微分项
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项 float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
systime time_start_guess; systime time_start_guess;

2
energy/src/energy/clear/energy_init.cpp
View File

@@ -52,6 +52,8 @@ void Energy::initEnergy() {
yaw_rotation = 0; yaw_rotation = 0;
pitch_rotation = 0; pitch_rotation = 0;
shoot = 0; shoot = 0;
last_yaw = 0;
last_pitch = 0;
sum_yaw = 0; sum_yaw = 0;
sum_pitch = 0; sum_pitch = 0;

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

@@ -8,6 +8,7 @@
using namespace std; using namespace std;
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送能量机关数据 // 此函数用于发送能量机关数据
@@ -17,13 +18,20 @@ void Energy::sendEnergy() {
if (camera_cnt == 1) { if (camera_cnt == 1) {
sum_yaw += yaw_rotation; sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation; sum_pitch += pitch_rotation;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; MINMAX(sum_yaw, -100, 100);
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; MINMAX(sum_pitch, -100, 100);
// float KP = 4.5;
// if(abs(yaw_rotation)<0.3)KP=0.1;
// else if(abs(yaw_rotation)<0.5)KP=0.1;
// else KP = 0.1; " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw + YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch +
PITCH_AIM_KD * (pitch_rotation - last_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;
yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw; yaw_rotation = YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + YAW_AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch; pitch_rotation = PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + PITCH_AIM_KI * sum_pitch;
} }
} }

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

@@ -31,11 +31,23 @@
#ifndef ENERGY_CAMERA_GAIN #ifndef ENERGY_CAMERA_GAIN
#define ENERGY_CAMERA_GAIN (30) #define ENERGY_CAMERA_GAIN (30)
#endif #endif
#ifndef AIM_KP #ifndef YAW_AIM_KD
#define AIM_KP (6) #define YAW_AIM_KD (0)
#endif #endif
#ifndef AIM_KI #ifndef YAW_AIM_KP
#define AIM_KI (0.1) #define YAW_AIM_KP (6)
#endif
#ifndef YAW_AIM_KI
#define YAW_AIM_KI (0.1)
#endif
#ifndef PITCH_AIM_KD
#define PITCH_AIM_KD (0)
#endif
#ifndef PITCH_AIM_KP
#define PITCH_AIM_KP (6)
#endif
#ifndef PITCH_AIM_KI
#define PITCH_AIM_KI (0.1)
#endif #endif
#ifndef COMPENSATE_YAW #ifndef COMPENSATE_YAW
#define COMPENSATE_YAW (5) #define COMPENSATE_YAW (5)