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-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 origin_yaw, origin_pitch;//初始的云台角度设定值
float shoot;//若为2，则要求主控板发弹
float last_yaw, last_pitch;//PID中微分项
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
systime time_start_guess;

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

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

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

@@ -8,6 +8,7 @@
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) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
MINMAX(sum_yaw, -100, 100);
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) {
sum_yaw += yaw_rotation - mcuData.curr_yaw;
sum_pitch += pitch_rotation - mcuData.curr_pitch;
yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch;
yaw_rotation = YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + YAW_AIM_KI * sum_yaw;
pitch_rotation = PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + PITCH_AIM_KI * sum_pitch;
}
}
@@ -51,7 +59,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
time_t t = time(nullptr);
if (last_time != t) {
last_time = t;
cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
fps = 0;
}
fps += 1;

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

@@ -31,11 +31,23 @@
#ifndef ENERGY_CAMERA_GAIN
#define ENERGY_CAMERA_GAIN (30)
#endif
#ifndef AIM_KP
#define AIM_KP (6)
#ifndef YAW_AIM_KD
#define YAW_AIM_KD (0)
#endif
#ifndef AIM_KI
#define AIM_KI (0.1)
#ifndef YAW_AIM_KP
#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
#ifndef COMPENSATE_YAW
#define COMPENSATE_YAW (5)