auto-aim

energy/include/energy/constant.h

@@ -0,0 +1,43 @@
+//
+// Created by xixiliadorabarry on 1/24/19.
+//
+#ifndef CONSTANT_H
+#define CONSTANT_H
+
+#define d2r (CV_PI / 180.0)
+
+const int ALLY_BLUE = 123;
+const int ALLY_RED = 456;
+const int SRC_WIDTH_CAMERA = 640;
+const int SRC_HEIGHT_CAMERA = 480;
+const int SRC_WIDTH = 320;
+const int SRC_HEIGHT = 240;
+const double PI = 3.1415926;
+const int CLOCKWISE = 1;
+const int ANTICLOCKWISE = -1;
+const float ATTACK_DISTANCE = 770;//cm
+const double WHOLE_FAN = 80;//cm
+//const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
+const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
+const int EXTRACT_POINT_X = 200;
+const int EXTRACT_POINT_Y = 20;
+const int EXTRACT_WIDTH = 240;
+const int EXTRACT_HEIGHT = 180;
+
+//以摄像头正方向位y轴
+const int GM_L = 14;//云台摄像头z方向
+const int GM_X = 15;//云台摄像头x方向
+const int GM_H = 16;//云台摄像头y方向
+
+//const double STRETCH = 231.0/640.0;//实际距离与图像伸缩比
+const double STRETCH = 231.0/640.0;
+
+const int ZERO_POINT_X = 281;
+const int ZERO_POINT_Y = 188;
+const double YAW_ORIGIN_RAD = PI/180*2.25;
+const double PITCH_ORIGIN_RAD = PI/180*14.85;
+
+const double LIFT_HEIGHT = 20;//云台抬升高度
+
+#endif //CONSTANT_H
+

energy/include/energy/energy.h

@@ -0,0 +1,129 @@
+//
+// Created by xixiliadorabarry on 1/24/19.
+//
+#ifndef ENERGY_H
+#define ENERGY_H
+
+#include <iostream>
+#include <vector>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <stdio.h>
+#include <time.h>
+#include <sys/timeb.h>
+#include "energy/constant.h"
+#include "energy/param_struct_define.h"
+#include "uart/uart.h"
+
+using std::vector;
+
+class Energy {
+public:
+	Energy(Uart &u);
+	~Energy();
+	int run(cv::Mat &src);
+
+    cv::Point2f uart_hit_point;
+	clock_t start;
+	Uart &uart;
+
+    void setAllyColor(int color);
+    void setRotation(int rotation);
+
+	void extract(cv::Mat &src);
+
+	void sendTargetByUart(float x, float y, float z);
+
+
+private:
+
+	EnergyPartParam energy_part_param_;
+	int fans_cnt;
+	int armors_cnt;
+	int count;
+	int last_fans_cnt;
+	int last_armors_cnt;
+	double radius;
+	double target_position;
+	double last_target_position;
+	float target_armor;
+	int ally_color_;
+	int energy_part_rotation;
+	float attack_distance;
+	int send_cnt;
+	double rectified_focal_length;
+	double theta;//电机pitch轴应旋转的角度
+	double phi;//电机yaw轴应旋转的角度
+	float yaw_rotation;
+	float pitch_rotation;
+    int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
+
+	std::vector<EnergyPart> fans;
+	std::vector<EnergyPart> armors;
+    std::vector<EnergyPart> gimble_zero_points;
+
+	cv::Point cycle_center;
+	cv::Point target_center;
+	cv::Point last_target_center;
+	cv::Point hit_point;
+	std::vector<float>fanPosition;
+	std::vector<float>armorPosition;
+	std::vector<cv::Point> Armor_center;
+    std::vector<cv::Point> first_armor_centers;
+    std::vector<cv::Point> all_armor_centers;
+    cv::Point left, right, top, bottom;
+	cv::Mat src_blue, src_red, src_green;
+    
+
+
+	void initEnergyPartParam();
+
+	int findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans_cnt);
+	int findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_armors_cnt);
+    int findGimbleZeroPoint(const cv::Mat &src, vector<EnergyPart> &gimble_zero_point);
+
+	void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans);
+	void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &armors);
+	void showBothContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans,
+		const std::vector<EnergyPart> &armors);
+
+	bool isValidFanContour(const vector<cv::Point> &fan_contour);
+	bool isValidArmorContour(const vector<cv::Point> &armor_contour);
+
+    void getFanPosition(std::vector<float> &fanPosition, const std::vector<EnergyPart> &fans, cv::Point cycle_center, double radius);
+	void getArmorPosition(std::vector<float> &armorPosition, const std::vector<EnergyPart> &armors, cv::Point cycle_center, double radius);
+	void getFirstArmorCenters(vector<EnergyPart> &armors, std::vector<cv::Point> &first_armor_centers);
+	void getAllArmorCenters();
+	void getPosition(cv::Point point, double &angle);
+
+	void cycleQuickCalculate(std::vector<cv::Point> &first_armor_centers, cv::Point &cycle_center, double &radius);
+    void cycleDefaultCalculateConst(cv::Point &cycle_center, double &radius);
+    void cycleCalculate();
+    void cycleLeastFit();
+
+	void findTarget(const std::vector<float>fanPosition, const std::vector<float>armorPosition, float &target_armor);
+
+    void findWholeCycle(const std::vector<cv::Point>&first_armor_centers);
+
+	void saveFourPoints(std::vector<cv::Point> &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4);
+    void savePoint2f(std::vector<cv::Point2f> &point_save, cv::Point point);
+    double pointDistance(cv::Point point_1, cv::Point point_2);
+    void rotate(double rad, double radius, cv::Point center, cv::Point point_old, cv::Point &point_new);
+	void stretch(cv::Point point_1, cv::Point2f &point_2);
+	void cycle(cv::Point p1, cv::Point p2, cv::Point p3, cv::Point &center, double &radius);
+
+    void getHitPoint();
+	bool changeTarget();
+    void gimbleRotation();
+
+	void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);
+	void imagePreprocess(cv::Mat &src);
+
+	void StructingElementClose(cv::Mat &src);
+	void StructingElementErodeDilate(cv::Mat &src);
+
+};
+
+#endif //ENERGY_H
+

energy/include/energy/param_struct_define.h

@@ -0,0 +1,60 @@
+//
+// Created by xixiliadorabarry on 1/24/19.
+//
+#ifndef PARAM_STRUCT_DEFINE_H
+#define PARAM_STRUCT_DEFINE_H
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <vector>
+
+using std::vector;
+
+struct EnergyPart {
+	cv::RotatedRect rect;
+	float angle;
+	vector<cv::Point> contour;
+
+	explicit EnergyPart(vector<cv::Point> &c) : contour(c) {
+		rect = cv::minAreaRect(c);
+		angle = cv::minAreaRect(c).angle;
+	};
+};
+
+struct EnergyPartParam {
+	double RPM;
+	double HIT_TIME;
+
+	int GRAY_THRESH;
+	int SPLIT_GRAY_THRESH;
+	int FAN_GRAY_THRESH;
+	int ARMOR_GRAY_THRESH;
+
+	long FAN_CONTOUR_AREA_MAX;
+	long FAN_CONTOUR_AREA_MIN;
+	long FAN_CONTOUR_LENGTH_MIN;
+	long FAN_CONTOUR_WIDTH_MIN;
+	float FAN_CONTOUR_HW_RATIO_MAX;
+	float FAN_CONTOUR_HW_RATIO_MIN;
+
+	long ARMOR_CONTOUR_AREA_MAX;
+	long ARMOR_CONTOUR_AREA_MIN;
+	long ARMOR_CONTOUR_LENGTH_MIN;
+	long ARMOR_CONTOUR_WIDTH_MIN;
+	long ARMOR_CONTOUR_LENGTH_MAX;
+	long ARMOR_CONTOUR_WIDTH_MAX;
+	float ARMOR_CONTOUR_HW_RATIO_MAX;
+	float ARMOR_CONTOUR_HW_RATIO_MIN;
+
+	float TWIN_ANGEL_MAX;
+};
+
+typedef struct GMAngle_t{
+    float yaw;
+    float pitch;
+}GMAngle_t;
+
+extern GMAngle_t aim;
+
+#endif //PARAM_STRUCT_DEFINE_H
+