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; using namespace cv;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于清空各vector // 此函数用于清空各vector
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -30,4 +29,5 @@ void Energy::initImage(cv::Mat &src){
if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY); if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
if (show_process)imshow("bin", src); 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); if (save_mark)writeDownSlightChange(gimbal_src);
getPredictPoint(target_point); getPredictPoint(target_point);
getAimPoint(predict_point); getAimPoint(predict_point);
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl;
// waitKey(0);
judgeShootInGimbal(); judgeShootInGimbal();
sendEnergy(); sendEnergy();
} }

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

@@ -18,34 +18,51 @@ 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;
if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) {
MINMAX(sum_yaw, -100, 100); 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_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);
// 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_yaw = tmp_yaw;
last_pitch = tmp_pitch; last_pitch = tmp_pitch;
if (ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
} 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);
if (ROBOT_ID == 7) {
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;
} }

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

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