对上交代码进行修改，主要将能量机关去掉，添加了同济的PnP位姿解算，但是同济有个四元数，获取IMU部分没有启用，可能导致精度不够。当前还存在反陀螺功能，修改为逻辑和弹道预测相结合，主要在时间关系上进行调整。

others/include/constants.h

@@ -0,0 +1,24 @@
+//
+// Created by xinyang on 19-7-8.
+//
+
+#ifndef _CONSTANTS_H_
+#define _CONSTANTS_H_
+
+#define PI (3.14159265459)
+
+#define ENEMY_BLUE 0
+#define ENEMY_RED  1
+
+#define ALLY_BLUE ENEMY_RED
+#define ALLY_RED  ENEMY_BLUE
+
+#define BIG_ENERGY_STATE   'b'
+#define SMALL_ENERGY_STATE 's'
+#define ARMOR_STATE        'a'
+
+#define FOCUS_PIXAL_8MM  (1488)
+#define FOCUS_PIXAL_5MM  (917)
+#define FOCUS_PIXAL      FOCUS_PIXAL_8MM
+
+#endif /* _CONSTANTS_H */