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
This commit is contained in:
xinyang
2019-08-06 01:32:40 +08:00
parent 540ddfa0a4 72d6a15d44
commit 741ffdc176
5 changed files with 38 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,11 +8,10 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于清空各vector
// ---------------------------------------------------------------------------------------------------------------------
void Energy::clearAll(){
void Energy::clearAll() {
fans.clear();
armors.clear();
flow_strip_fans.clear();
@@ -24,10 +23,11 @@ void Energy::clearAll(){
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于图像预处理
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initImage(cv::Mat &src){
void Energy::initImage(cv::Mat &src) {
// imagePreprocess(src);
// if(show_process)imshow("img_preprocess", 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();
}

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

@@ -18,19 +18,32 @@ void Energy::sendEnergy() {
if (camera_cnt == 1) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) {
MINMAX(sum_yaw, -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);
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
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;
@@ -41,13 +54,15 @@ void Energy::sendEnergy() {
// last_yaw = tmp_yaw;
// last_pitch = tmp_pitch;
}
} else if (is_small){
} else if (is_small) {
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);
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
if (ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}