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

Merge remote-tracking branch 'origin/master'

Browse Source 
# Conflicts:
#	main.cpp
This commit is contained in:
xinyang
2019-08-06 11:55:43 +08:00
parent 9392c201db 72d6a15d44
commit 6cdb6b4832
5 changed files with 56 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,7 +8,6 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于清空各vector
// ---------------------------------------------------------------------------------------------------------------------
@@ -30,4 +29,5 @@ void Energy::initImage(cv::Mat &src){
if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
if (show_process)imshow("bin", src);
if (show_energy)waitKey(1);
}

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

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

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

@@ -18,34 +18,51 @@ void Energy::sendEnergy() {
if (camera_cnt == 1) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_pitch, -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);
// 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);
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'
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
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;
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
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;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
// sum_yaw += yaw_rotation - mcu_data.curr_yaw;
// sum_pitch += pitch_rotation - mcu_data.curr_pitch;
// double tmp_yaw = yaw_rotation;
// double tmp_pitch = pitch_rotation;
// 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;
// last_yaw = tmp_yaw;
// last_pitch = tmp_pitch;
}
} 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_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);
if (ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}

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

@@ -35,35 +35,40 @@
#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)
#define SMALL_YAW_AIM_KD (0.35)
#endif
#ifndef SMALL_YAW_AIM_KP
#define SMALL_YAW_AIM_KP (4)
#define SMALL_YAW_AIM_KP (0.85)
#endif
#ifndef SMALL_PITCH_AIM_KD
#define SMALL_PITCH_AIM_KD (0)
#define SMALL_PITCH_AIM_KD (0.35)
#endif
#ifndef SMALL_PITCH_AIM_KP
#define SMALL_PITCH_AIM_KP (3.7)
#define SMALL_PITCH_AIM_KP (0.85)
#endif
#ifndef BIG_YAW_AIM_KD
#define BIG_YAW_AIM_KD (0)
#define BIG_YAW_AIM_KD (0.35)
#endif
#ifndef BIG_YAW_AIM_KP
#define BIG_YAW_AIM_KP (6.5)
#define BIG_YAW_AIM_KP (0.85)
#endif
#ifndef BIG_YAW_AIM_KI
#define BIG_YAW_AIM_KI (0.1)
#define BIG_YAW_AIM_KI (0)
#endif
#ifndef BIG_PITCH_AIM_KD
#define BIG_PITCH_AIM_KD (0)
#define BIG_PITCH_AIM_KD (0.35)
#endif
#ifndef BIG_PITCH_AIM_KP
#define BIG_PITCH_AIM_KP (6.5)
#define BIG_PITCH_AIM_KP (0.85)
#endif
#ifndef BIG_PITCH_AIM_KI
#define BIG_PITCH_AIM_KI (0.1)
#define BIG_PITCH_AIM_KI (0)
#endif
#ifndef COMPENSATE_YAW
#define COMPENSATE_YAW (5)
@@ -72,7 +77,7 @@
#define COMPENSATE_PITCH (74)
#endif
#ifndef EXTRA_DELTA_X
#define EXTRA_DELTA_X (10)
#define EXTRA_DELTA_X (0)
#endif
#ifndef EXTRA_DELTA_Y
#define EXTRA_DELTA_Y (10)