VSAG/vision_sjtu_19
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-07 22:59:02 +08:00
parent 16efe17334
commit 86b37b3f5e
3 changed files with 53 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
energy/src/energy/get/aim_point_get.cpp
View File

@@ -29,6 +29,7 @@ void Energy::getAimPoint(cv::Point target_point_) {
extra_delta_y = 0;
}
double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcu_data.delta_x - manual_delta_x - extra_delta_x);
double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcu_data.delta_y - manual_delta_y - extra_delta_y);
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;

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

@@ -15,17 +15,14 @@ using namespace std;
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendEnergy() {
if (is_big) {
if (camera_cnt == 1) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 4 || ROBOT_ID == 8) {
if (ROBOT_ID == 4) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if (ROBOT_ID == 3 || ROBOT_ID == 7) {
} else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
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);
}
@@ -48,23 +45,25 @@ void Energy::sendEnergy() {
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
// cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
if (ROBOT_ID == 3 || ROBOT_ID == 7) {
if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
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;
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
} else if (is_small) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 4) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
float yaw_I_component = SMALL_YAW_AIM_KI * sum_yaw;
float pitch_I_component = SMALL_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) {
@@ -79,7 +78,7 @@ void Energy::sendEnergy() {
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KI * sum_pitch +
SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
if (ROBOT_ID == 3 || ROBOT_ID == 7) {
if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
@@ -92,9 +91,7 @@ void Energy::sendEnergy() {
sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0);
} else if (is_guessing) {
sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0);
} /*else if (fans_cnt >= 4) {
sendTarget(serial, yaw_rotation, pitch_rotation, 7, 0);
}*/ else {
} else {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);
}

6
main.cpp
View File

@@ -33,7 +33,7 @@ McuData mcu_data = { // 单片机端回传结构体
BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位
1, // 是否启用数字识别
ENEMY_BLUE, // 敌方颜色
ENEMY_RED, // 敌方颜色
0, // 能量机关x轴补偿量
0, // 能量机关y轴补偿量
};
@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
} else {
video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi");
video_chassis = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi");
video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi");
video_chassis = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi");
}
if (video_gimbal->init()) {
LOGM("video_gimbal source initialization successfully.");