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-05 19:33:00 +08:00
parent 690e2a9224
commit acfc1e8701
3 changed files with 19 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
energy/src/energy/run.cpp
View File

@@ -107,6 +107,8 @@ void Energy::runBig(cv::Mat &gimbal_src) {
getPredictPoint(target_point); getPredictPoint(target_point);
getAimPoint(predict_point); getAimPoint(predict_point);
judgeShootInGimbal(); judgeShootInGimbal();
cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl;
waitKey(0);
sendEnergy(); sendEnergy();
} }

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

@@ -18,35 +18,36 @@ 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;
MINMAX(sum_yaw, -100, 100); float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
MINMAX(sum_pitch, -100, 100); float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
double tmp_yaw = yaw_rotation; double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_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);
MINMAX(yaw_rotation, -10, 10); MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
last_yaw = tmp_yaw; last_yaw = tmp_yaw;
last_pitch = tmp_pitch; last_pitch = tmp_pitch;
} else if (is_chassis) { } else if (is_chassis) {
sum_yaw += yaw_rotation - mcu_data.curr_yaw; // sum_yaw += yaw_rotation - mcu_data.curr_yaw;
sum_pitch += pitch_rotation - mcu_data.curr_pitch; // sum_pitch += pitch_rotation - mcu_data.curr_pitch;
double tmp_yaw = yaw_rotation; // double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation; // double tmp_pitch = pitch_rotation;
yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw; // yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw;
pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch; // pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch;
last_yaw = tmp_yaw; // last_yaw = tmp_yaw;
last_pitch = tmp_pitch; // last_pitch = tmp_pitch;
} }
} else if (is_small){ } else if (is_small){
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_pitch, -100, 100);
double tmp_yaw = yaw_rotation; double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_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);
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
last_yaw = tmp_yaw; last_yaw = tmp_yaw;
last_pitch = tmp_pitch; last_pitch = tmp_pitch;
} }

2
main.cpp
View File

@@ -30,7 +30,7 @@ using namespace std;
McuData mcu_data = { // 单片机端回传结构体 McuData mcu_data = { // 单片机端回传结构体
0, // 当前云台yaw角 0, // 当前云台yaw角
0, // 当前云台pitch角 0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符 BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位 0, // 云台角度标记位
1, // 是否启用数字识别 1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色 ENEMY_RED, // 敌方颜色