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

energy change

Browse Source
This commit is contained in:
sun
2019-07-19 23:48:33 +08:00
parent 6e864cc52d
commit 6f517f7853
3 changed files with 25 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,6 +4,7 @@
#include "energy/energy.h" #include "energy/energy.h"
#include "energy/constant.h" #include "energy/constant.h"
#include "config/setconfig.h"
using namespace std; using namespace std;
using namespace cv; using namespace cv;
@@ -12,12 +13,17 @@ using namespace cv;
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::getAimPoint(cv::Point target_point) { void Energy::getAimPoint(cv::Point target_point) {
double dx, dy;
//五号车 //五号车
// double dx = -(target_point.x - 320 - 10); // double dx = -(target_point.x - 320 - 10);
// double dy = -(target_point.y - 240 - 22); // double dy = -(target_point.y - 240 - 22);
//四号车 //四号车
double dx = -(target_point.x - 320 - 5); #ifndef COMPENSATE_YAW
double dy = -(target_point.y - 240 - 74); dx = -(target_point.x - 320 - COMPENSATE_YAW);
#endif
#ifndef COMPENSATE_PITCH
dy = -(target_point.y - 240 - COMPENSATE_PITCH);
#endif
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;

7
energy/src/energy/judge/judge_shoot.cpp
View File

@@ -4,6 +4,7 @@
#include "energy/energy.h" #include "energy/energy.h"
#include "log.h" #include "log.h"
#include "config/setconfig.h"
using namespace std; using namespace std;
using namespace cv; using namespace cv;
@@ -43,6 +44,8 @@ void Energy::judgeShootInGimbal(){
static float sum_pitch = 0; static float sum_pitch = 0;
sum_yaw += yaw_rotation; sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation; sum_pitch += pitch_rotation;
yaw_rotation = 6 * yaw_rotation + 0.1 * sum_yaw; #ifndef AIM_KP AIM_KI
pitch_rotation = 6 * pitch_rotation + 0.1 * sum_pitch; yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
#endif
} }

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

@@ -30,6 +30,18 @@
#ifndef ENERGY_CAMERA_GAIN #ifndef ENERGY_CAMERA_GAIN
#define ENERGY_CAMERA_GAIN (20) #define ENERGY_CAMERA_GAIN (20)
#endif #endif
#ifndef AIM_KP
#define AIM_KP (6)
#endif
#ifndef AIM_KI
#define AIM_KI (0.1)
#endif
#ifndef COMPENSATE_YAW
#define COMPENSATE_YAW (5)
#endif
#ifndef COMPENSATE_PITCH
#define COMPENSATE_PITCH (74)
#endif
//#define GIMBAL_FLIP_MODE (-1) //#define GIMBAL_FLIP_MODE (-1)
//#define CHASSIS_FLIP_MODE (-1) //#define CHASSIS_FLIP_MODE (-1)