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-09 06:38:43 +08:00
parent ef9f6e94e7
commit 05527de9e2
4 changed files with 35 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,7 +80,7 @@ void Energy::setSmallEnergyInit() {
FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r"); FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
if (fp) { if (fp) {
fscanf(fp, "%d %d", &manual_delta_x, &manual_delta_y); fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y);
fclose(fp); fclose(fp);
} }
} }

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

@@ -20,18 +20,26 @@ void Energy::getAimPoint(cv::Point target_point_) {
if (target_polar_angle_ > 0 && target_polar_angle_ <= 90) { if (target_polar_angle_ > 0 && target_polar_angle_ <= 90) {
extra_delta_x = EXTRA_DELTA_X * (90 - target_polar_angle_) / 90; extra_delta_x = EXTRA_DELTA_X * (90 - target_polar_angle_) / 90;
extra_delta_y = - EXTRA_DELTA_Y * target_polar_angle_ / 90; extra_delta_y = -EXTRA_DELTA_Y * target_polar_angle_ / 90;
} else if (target_polar_angle_ > 90 && target_polar_angle_ < 180) { } else if (target_polar_angle_ > 90 && target_polar_angle_ < 180) {
extra_delta_x = - EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90; extra_delta_x = -EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90;
extra_delta_y = - EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90; extra_delta_y = -EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90;
} else { } else {
extra_delta_x = 0; extra_delta_x = 0;
extra_delta_y = 0; extra_delta_y = 0;
} }
int compensate_yaw = 0, compensate_pitch = 0;
if (mcu_data.enemy_color == ENEMY_BLUE) {
compensate_yaw = RED_COMPENSATE_YAW;
compensate_pitch = RED_COMPENSATE_PITCH;
} else if (mcu_data.enemy_color == ENEMY_RED) {
compensate_yaw = BLUE_COMPENSATE_YAW;
compensate_pitch = BLUE_COMPENSATE_PITCH;
}
double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcu_data.delta_x - manual_delta_x - extra_delta_x); 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); 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; yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;

38
energy/src/energy/mark/mark.cpp
View File

@@ -22,27 +22,25 @@ void Energy::writeDownSlightChange(cv::Mat &src) {
fclose(fp_delta); fclose(fp_delta);
} }
} }
if(fans_cnt>=1){ FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a");
FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a"); if (fp_data) {
if (fp_data) { if(mcu_data.mark == 1){
if(mcu_data.mark == 1){ fprintf(fp_data, "PID: %s\t", "new");
fprintf(fp_data, "PID: %s\t", "new"); } else {
} else { fprintf(fp_data, "PID: %s\t", "default");
fprintf(fp_data, "PID: %s\t", "default");
}
if(is_big){
fprintf(fp_data, "state: %s\t", "big");
} else if(is_small){
fprintf(fp_data, "state: %s\t", "small");
}
fprintf(fp_data, "fps: %d\t", curr_fps);
fprintf(fp_data, "fans_cnt: %d\t", fans_cnt);
fprintf(fp_data, "shoot: %d\t", int(shoot));
fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation);
fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x,
mcu_data.delta_y + manual_delta_y);
fclose(fp_data);
} }
if(is_big){
fprintf(fp_data, "state: %s\t", "big");
} else if(is_small){
fprintf(fp_data, "state: %s\t", "small");
}
fprintf(fp_data, "fps: %d\t", curr_fps);
fprintf(fp_data, "fans_cnt: %d\t", fans_cnt);
fprintf(fp_data, "shoot: %d\t", int(shoot));
fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation);
fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x,
mcu_data.delta_y + manual_delta_y);
fclose(fp_data);
} }
} }

6
main.cpp
View File

@@ -30,10 +30,10 @@ using namespace std;
McuData mcu_data = { // 单片机端回传结构体 McuData mcu_data = { // 单片机端回传结构体
0, // 当前云台yaw角 0, // 当前云台yaw角
0, // 当前云台pitch角 0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符 BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位 0, // 云台角度标记位
0, // 是否为反陀螺模式 0, // 是否为反陀螺模式
ENEMY_BLUE, // 敌方颜色 ENEMY_RED, // 敌方颜色
0, // 能量机关x轴补偿量 0, // 能量机关x轴补偿量
0, // 能量机关y轴补偿量 0, // 能量机关y轴补偿量
}; };
@@ -66,7 +66,7 @@ int main(int argc, char *argv[]) {
video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
} else { } else {
video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/red_small.avi"); video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/blue_small.avi");
video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/red_small.avi"); video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/red_small.avi");
} }
if (video_gimbal->init()) { if (video_gimbal->init()) {