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

2019赛季RM中部分区赛，自瞄和能量机关，完整稳定版。

Browse Source
This commit is contained in:
xinyang
2019-05-31 19:29:22 +08:00
parent 76bc5988c0
commit c0c12b24f0
28 changed files with 816 additions and 728 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -1,5 +1,6 @@
cmake-build-debug
build
.idea
Mark
tools/TrainCNN/.idea
tools/TrainCNN/__pycache__

6
CMakeLists.txt
View File

@@ -1,12 +1,11 @@
CMAKE_MINIMUM_REQUIRED(VERSION 3.5)
PROJECT(AutoAim)
PROJECT(RM_Auto_Aim)
SET(CMAKE_CXX_STANDARD 11)
SET(CMAKE_BUILD_TYPE RELEASE)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")
# Todo
## 使用编译期固定选项，以略微提升性能。
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFIX_OPTIONS")
@@ -22,6 +21,7 @@ IF(CCACHE_FOUND)
MESSAGE("< Use ccache for compiler >")
ENDIF()
FIND_PACKAGE(Eigen3 REQUIRED)
FIND_PACKAGE(OpenCV 3 REQUIRED)
FIND_PACKAGE(Threads)
@@ -32,7 +32,7 @@ INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include)
FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp")
ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/additions/additions.h)
ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} )
TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS})

7
armor/include/armor_finder/armor_finder.h
View File

@@ -43,9 +43,14 @@ public:
bool sendBoxPosition();
};
struct LightBlob {
#define BLOB_RED ENEMY_RED
#define BLOB_BLUE ENEMY_BLUE
class LightBlob {
public:
cv::RotatedRect rect;
double length;
uint8_t BlobColor;
explicit LightBlob(cv::RotatedRect &r) : rect(r) {
length = max(rect.size.height, rect.size.width);

4
armor/src/armor_finder/armor_finder.cpp
View File

@@ -94,8 +94,8 @@ bool sendTarget(Serial& serial, double x, double y, double z) {
bool ArmorFinder::sendBoxPosition() {
auto rect = armor_box;
double dx = rect.x + rect.width/2 - 320;
double dy = rect.y + rect.height/2 - 240 - 30;
double dx = rect.x + rect.width/2 - 320 - 10;
double dy = rect.y + rect.height/2 - 240 - 20;
double yaw = atan(dx / FOCUS_PIXAL) * 180 / 3.14159265459;
double pitch = atan(dy / FOCUS_PIXAL) * 180 / 3.14159265459;
// cout << yaw << endl;

16
armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp
View File

@@ -36,9 +36,9 @@ void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color) {
resize(channels.at(0), blue, cv::Size(640, 480));
resize(channels.at(2), red, cv::Size(640, 480));
if(color == ENEMY_RED){
split = red - blue;
split = red;// - blue;
}else if(color == ENEMY_BLUE){
split = blue - red;
split = blue;// - red;
}
}
@@ -47,18 +47,18 @@ void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color) {
void imagePreProcess(cv::Mat &src) {
// cv::medianBlur(src, src, 5);
static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(1, 4));
static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
erode(src, src, kernel_erode);
static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(2, 4));
static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
dilate(src, src, kernel_dilate);
static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(2, 4));
erode(src, src, kernel_erode2);
static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 6));
static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
dilate(src, src, kernel_dilate2);
static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
erode(src, src, kernel_erode2);
float alpha = 1.5;
int beta = 0;
src.convertTo(src, -1, alpha, beta);

71
armor/src/armor_finder/state_machine/searching_state/searching_state.cpp
View File

@@ -31,7 +31,7 @@ static void pipelineLightBlobPreprocess(cv::Mat &src) {
}
static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
static cv::Mat src_gray, src_bin;
static cv::Mat src_gray;
if(src.type() == CV_8UC3){
cvtColor(src, src_gray, CV_BGR2GRAY);
}else if(src.type() == CV_8UC1){
@@ -77,7 +77,10 @@ bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob
&& light_blob_i.length / light_blob_j.length > 0.5);
}
bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
if(light_blob_i.BlobColor != enemy_color || light_blob_j.BlobColor != enemy_color){
return false;
}
if(!lengthRatioJudge(light_blob_i, light_blob_j)){
// std::cout << "lengthRatioJudge" << std::endl;
return false;
@@ -108,10 +111,10 @@ double centerDistance(const cv::Rect2d &box){
return dx*dx + dy*dy;
}
static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &armor_boxes) {
static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &armor_boxes, uint8_t enemy_color) {
for (int i = 0; i < light_blobs.size() - 1; ++i) {
for (int j = i + 1; j < light_blobs.size(); ++j) {
if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j))) {
if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), enemy_color)) {
continue;
}
cv::Rect2d rect_left = light_blobs.at(static_cast<unsigned long>(i)).rect.boundingRect();
@@ -151,6 +154,33 @@ bool judge_light_color(std::vector<LightBlob> &light, std::vector<LightBlob> &co
return !result.empty();
}
void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &blobs) {
for(auto &blob : blobs){
auto region = blob.rect.boundingRect();
region.x -= fmax(2, region.width * 0.1);
region.y -= fmax(2, region.height * 0.05);
region.width += 2 * fmax(2, region.width * 0.1);
region.height += 2 * fmax(2, region.height * 0.05);
region &= cv::Rect(0, 0, 640, 480);
cv::Mat roi = src(region);
long long int red_cnt = 0, blue_cnt = 0;
for(int row=0; row<roi.rows; row++){
for(int col=0; col<roi.cols; col++){
red_cnt += roi.at<cv::Vec3b>(row, col)[2];
blue_cnt += roi.at<cv::Vec3b>(row, col)[0];
}
}
if(red_cnt > blue_cnt){
blob.BlobColor = BLOB_RED;
}else{
blob.BlobColor = BLOB_BLUE;
}
}
}
int prior_red[] = {0, 2, 3, 4, 1, 5, 7, 8, 9, 6};
int prior_blue[]= {5, 7, 8, 9, 6, 0, 2, 3, 4, 1};
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
cv::Mat split, src_bin;
LightBlobs light_blobs, light_blobs_, light_blobs_real;
@@ -159,35 +189,39 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
cv::cvtColor(src, src_gray, CV_BGR2GRAY);
// pipelineLightBlobPreprocess(src_gray);
cv::threshold(src_gray, src_bin, 120, 255, CV_THRESH_BINARY);
cv::threshold(src_gray, src_bin, 170, 255, CV_THRESH_BINARY);
// imshow("gray bin", src_bin);
if(!findLightBlobs(src_bin, light_blobs)){
return false;
}
if(show_light_blobs){
showContours("blobs", src_bin, light_blobs);
showContours("blobs_gray", src_bin, light_blobs);
cv::waitKey(1);
}
imageColorSplit(src, split, enemy_color);
// imshow("split123",split);
imagePreProcess(split);
cv::threshold(split, src_bin, 120, 255, CV_THRESH_BINARY);
// imshow("split",split);
cv::threshold(split, src_bin, 170, 255, CV_THRESH_BINARY);
if(!findLightBlobs(src_bin, light_blobs_)){
return false;
}
if(show_light_blobs){
showContours("blobs_", src_bin, light_blobs_);
showContours("blobs_split", src_bin, light_blobs_);
cv::waitKey(1);
}
if(!judge_light_color(light_blobs, light_blobs_, light_blobs_real)){
return false;
}
get_blob_color(src, light_blobs_real);
if(show_light_blobs){
showContours("blobs_real", src, light_blobs_real);
cv::waitKey(1);
}
if(!findArmorBoxes(light_blobs_real, armor_boxes)){
if(!findArmorBoxes(light_blobs_real, armor_boxes, enemy_color)){
return false;
}
if(show_armor_boxes){
@@ -203,12 +237,23 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
boxes_number[c-1].emplace_back(box);
}
}
for(auto box : boxes_number){
if(!box.empty()){
armor_box = box[0];
if(enemy_color == ENEMY_BLUE) {
for(auto id : prior_blue){
if(!boxes_number[id].empty()){
armor_box = boxes_number[id][0];
break;
}
}
}else if(enemy_color == ENEMY_RED) {
for(auto id : prior_red){
if(!boxes_number[id].empty()){
armor_box = boxes_number[id][0];
break;
}
}
}else{
LOGE("enemy_color ERROR!");
}
if(armor_box == cv::Rect2d(0,0,0,0)){
return false;
}

6
armor/src/show_images/show_images.cpp
View File

@@ -67,7 +67,11 @@ void showContours(std::string windows_name, const cv::Mat &src, const std::vecto
for(const auto &light_blob:light_blobs)
{
rectangle(image2show, light_blob.rect.boundingRect(), Scalar(255,0,0), 3);
if(light_blob.BlobColor == BLOB_RED)
rectangle(image2show, light_blob.rect.boundingRect(), Scalar(0,0,255), 3);
if(light_blob.BlobColor == BLOB_BLUE)
rectangle(image2show, light_blob.rect.boundingRect(), Scalar(255,0,0), 3);
}
imshow(windows_name, image2show);
}

294
energy/include/energy/energy.h
View File

@@ -1,143 +1,151 @@
//
// 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 "serial/serial.h"
#include "additions/additions.h"
using std::vector;
class Energy {
public:
Energy(Serial &u, uint8_t &enemy_color);
~Energy();
int run(cv::Mat &src);
cv::Point2f uart_hit_point;
clock_t start;
Serial &serial;
// void setAllyColor(int color);
void setRotation(int rotation);
void setEnergyRotationInit();
void extract(cv::Mat &src);
void sendTargetByUart(float x, float y, float z);
private:
EnergyPartParam energy_part_param_;
LiftHeight lift_height_;
bool isSendTarget;
bool isMark;
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;
double last_hit_position;
float target_armor;
uint8_t &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;
uint8_t last_mark;
int position_mode;
int last_position_mode;
int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
bool energy_rotation_init;
int clockwise_rotation_init_cnt;
int anticlockwise_rotation_init_cnt;
float origin_yaw, origin_pitch;
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 initEnergy();
void initEnergyPartParam();
void initRotation();
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 changeMark();
void gimbleRotation();
void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);
void imagePreprocess(cv::Mat &src);
void StructingElementClose(cv::Mat &src,int length, int width);
void StructingElementErodeDilate(cv::Mat &src);
};
#endif //ENERGY_H
//
// 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 "serial/serial.h"
#include "additions/additions.h"
#include "options/options.h"
using std::vector;
class Energy {
public:
Energy(Serial &u, uint8_t &color);
~Energy();
int run(cv::Mat &src);
cv::Point2f uart_hit_point;
clock_t start;
Serial &serial;
// void setAllyColor(int color);
void setRotation(int rotation);
void setEnergyRotationInit();
void extract(cv::Mat &src);
void sendTargetByUart(float x, float y, float z);
private:
EnergyPartParam energy_part_param_;
LiftHeight lift_height_;
bool isSendTarget;
bool isMark;
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;
double last_hit_position;
float target_armor;
float last_target_armor;
uint8_t &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;
uint8_t last_mark;
int position_mode;
int last_position_mode;
int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
bool energy_rotation_init;
int clockwise_rotation_init_cnt;
int anticlockwise_rotation_init_cnt;
float red_origin_yaw, red_origin_pitch;
float blue_origin_yaw, blue_origin_pitch;
float origin_yaw, origin_pitch;
float target_cnt;
bool target_cnt_flag;
bool save_new_mark;
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 initEnergy();
void initEnergyPartParam();
void initRotation();
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 changeMark();
void gimbleRotation();
void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);
void imagePreprocess(cv::Mat &src);
void StructingElementClose(cv::Mat &src,int length, int width);
void StructingElementErodeDilate(cv::Mat &src);
};
#endif //ENERGY_H

98
energy/src/energy/energy.cpp
View File

@@ -1,37 +1,61 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
extern uint8_t last_state;
Energy::Energy(Serial &u, uint8_t &enemy_color):serial(u),ally_color(enemy_color),
src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
{
initEnergy();
initEnergyPartParam();
}
Energy::~Energy() = default;
//
//void Energy::setAllyColor(int color)
//{
// ally_color_ = color;
//}
void Energy::setRotation(int rotation){
energy_part_rotation = rotation;
}
void Energy::setEnergyRotationInit() {
initEnergy();
initEnergyPartParam();
energy_rotation_init = true;
}
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
#include "log.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
extern uint8_t last_state;
Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
{
initEnergy();
initEnergyPartParam();
save_new_mark = true;
if(ally_color==ALLY_RED){
origin_yaw = red_origin_yaw;
origin_pitch = red_origin_pitch;
}
else if(ally_color==ALLY_BLUE){
origin_yaw = blue_origin_yaw;
origin_pitch = blue_origin_pitch;
}
else {
LOGE_INFO("ally color_run");
}
}
Energy::~Energy() = default;
//
//void Energy::setAllyColor(int color)
//{
// ally_color_ = color;
//}
void Energy::setRotation(int rotation){
energy_part_rotation = rotation;
}
void Energy::setEnergyRotationInit() {
initEnergy();
initEnergyPartParam();
energy_rotation_init = true;
if(!save_new_mark){
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r");
if(fp){
fscanf(fp,"%f %f",&origin_yaw,&origin_pitch);
fclose(fp);
}
}
}

59
energy/src/energy/get/gimble_rotation_get.cpp
View File

@@ -3,6 +3,7 @@
//
#include "energy/energy.h"
#include <cmath>
#include "log.h"
using namespace cv;
using std::cout;
@@ -14,50 +15,56 @@ void Energy::changeMark() {
last_mark = mcuData.mark;
origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch;
isMark = true;
isMark = false;
// LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
}
else if (mcuData.state == 1) {
else if (mcuData.mark == 1) {
last_mark = mcuData.mark;
isMark = true;
// LOGM(STR_CTR(WORD_BLUE,"Marking..."));
}
else {
last_mark = mcuData.mark;
isMark = false;
}
//cout<<"mark: "<<int(mcuData.mark)<<endl;
}
void Energy::gimbleRotation(){
cv::Point2f real_hit_point;
stretch(hit_point, real_hit_point);
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
/*origin_yaw = mark_yaw;
origin_pitch = mark_pitch;*/
if(position_mode == 1){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
}
else if(position_mode == 2){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
}
else if(position_mode == 3){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
}
else if(position_mode == 4){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
}
else if(position_mode == 5){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
}
else if(position_mode == 6){
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
}
// if(position_mode == 1){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 2){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 3){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 4){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 5){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 6){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else{
// pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
// }

231
energy/src/energy/get/hit_point_get.cpp
View File

@@ -11,120 +11,123 @@ using std::endl;
using std::vector;
void Energy::getHitPoint(){
int hit_position = 1000;
int limit_angle = 6;
int angle_interval = 60;
if(energy_part_rotation==1){
if(target_armor>=0 && target_armor<=limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval;
position_mode = 1;
}
else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 0;
position_mode = 2;
}
else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval;
position_mode = 3;
}
else if(target_armor>=-180 && target_armor<-180+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval;
position_mode = 4;
}
else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 180;
position_mode = 5;
}
else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2 * angle_interval;
position_mode = 6;
}
else{
position_mode = 0;
return;
}
}
if(energy_part_rotation==-1){
if(target_armor>=0 && target_armor<=limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval;
position_mode = 1;
}
else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 180;
position_mode = 2;
}
else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2*angle_interval;
position_mode = 3;
}
else if(target_armor>=-180 && target_armor<-180+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval;
position_mode = 4;
}
else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 0;
position_mode = 5;
}
else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval;
position_mode = 6;
}
else{
position_mode = 0;
return;
}
}
if(position_mode!=0 && position_mode!=last_position_mode){
last_position_mode = position_mode;
isSendTarget = true;
cout<<"hit position: "<<hit_position<<endl;
return;
}
else if(position_mode == 0){
isSendTarget = false;
return;
}
else{
last_position_mode = position_mode;
isSendTarget = false;
return;
}
}
//bool Energy::changeTarget(){
// if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){
// last_target_position = target_position;
// return false;
int rad = 60;
if(energy_part_rotation==1) rotate(rad, radius, cycle_center, target_center, hit_point);
if(energy_part_rotation==-1) rotate(-rad, radius, cycle_center, target_center, hit_point);
// int hit_position = 1000;
// int limit_angle = 6;
// int angle_interval = 60;
//
// if(energy_part_rotation==1){
// if(target_armor>=0 && target_armor<=limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -1*angle_interval;
// position_mode = 1;
// }
// else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 0;
// position_mode = 2;
// }
// else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = angle_interval;
// position_mode = 3;
// }
// else if(target_armor>=-180 && target_armor<-180+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = 2*angle_interval;
// position_mode = 4;
// }
// else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 180;
// position_mode = 5;
// }
// else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -2 * angle_interval;
// position_mode = 6;
// }
// else{
// position_mode = 0;
// return;
// }
// }
//
// if(energy_part_rotation==-1){
// if(target_armor>=0 && target_armor<=limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = 2*angle_interval;
// position_mode = 1;
// }
// else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 180;
// position_mode = 2;
// }
// else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -2*angle_interval;
// position_mode = 3;
// }
// else if(target_armor>=-180 && target_armor<-180+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -1*angle_interval;
// position_mode = 4;
// }
// else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 0;
// position_mode = 5;
// }
// else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = angle_interval;
// position_mode = 6;
// }
// else{
// position_mode = 0;
// return;
// }
// }
//
// if(position_mode!=0 && position_mode!=last_position_mode){
// last_position_mode = position_mode;
// isSendTarget = true;
// //cout<<"hit position: "<<hit_position<<endl;
// return;
// }
// else if(position_mode == 0){
// isSendTarget = false;
// return;
// }
// else{
// last_target_position = target_position;
// return true;
// last_position_mode = position_mode;
// isSendTarget = false;
// return;
// }
//}
}
bool Energy::changeTarget(){
if(fabs(target_armor - last_target_armor) < 30||fabs(target_armor - last_target_armor) > 330){
last_target_armor = target_armor;
return false;
}
else{
last_target_armor = target_armor;
return true;
}
}

274
energy/src/energy/param_init.cpp
View File

@@ -1,133 +1,141 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
void Energy::initEnergy() {
isSendTarget = false;
isMark = false;
fans_cnt = 0;
armors_cnt = 0;
cycle_center = Point(0, 0);
target_center = Point(0, 0);
last_target_center = Point(0, 0);
hit_point = Point(0, 0);
target_position = -1000;
last_target_position = -1000;
last_hit_position = 20000;
target_armor = -1000;
radius = 0;
// ally_color = ALLY_RED;
energy_part_rotation = CLOCKWISE;
attack_distance = ATTACK_DISTANCE;
count = 1;
last_fans_cnt = 0;
last_armors_cnt = 0;
send_cnt = 0;
//rectified_focal_length = 1000;
//theta = 0;
//phi = 0;
yaw_rotation = 0;
pitch_rotation = 0;
last_mark = 0;
origin_yaw = -0.13;
origin_pitch = 13.18;
isLeftVertexFound = -1;
isTopVertexFound = -1;
isRightVertexFound = -1;
isBottomVertexFound = -1;
left = Point(640, 480);
right = Point(0, 0);
top = Point(640, 480);
bottom = Point(0, 0);
position_mode = 0;
last_position_mode = 0;
energy_rotation_init = false;
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
Armor_center.clear();
first_armor_centers.clear();
all_armor_centers.clear();
clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0;
}
void Energy::initEnergyPartParam() {
energy_part_param_.RPM = 10;
energy_part_param_.HIT_TIME = 1.14;
energy_part_param_.GRAY_THRESH = 240;
energy_part_param_.SPLIT_GRAY_THRESH = 60;
energy_part_param_.FAN_GRAY_THRESH = 75;
energy_part_param_.ARMOR_GRAY_THRESH = 80;
energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;
energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 90;
energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 140;
energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35;
energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 60;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.TWIN_ANGEL_MAX = 10;
lift_height_.LIFT_0 = 0;
lift_height_.LIFT_30 = 0;
lift_height_.LIFT_60 = 0;
lift_height_.LIFT_90 = 10;
lift_height_.LIFT_minus_30 = 0;
lift_height_.LIFT_minus_60 = 0;
lift_height_.LIFT_minus_90 = 0;
}
void Energy::initRotation() {
target_position = target_armor;
cout << "target position: " << target_position << '\t' << "last target position: " << last_target_position << endl;
if (target_position >= -180 && last_target_position >= -180 && fabs(target_position - last_target_position) < 30) {
if (target_position < last_target_position) clockwise_rotation_init_cnt++;
else if (target_position > last_target_position) anticlockwise_rotation_init_cnt++;
}
if (clockwise_rotation_init_cnt == 5) {
energy_part_rotation = CLOCKWISE;
cout << "rotation: " << energy_part_rotation << endl;
energy_rotation_init = false;
}
else if (anticlockwise_rotation_init_cnt == 5) {
energy_part_rotation = ANTICLOCKWISE;
cout << "rotation: " << energy_part_rotation << endl;
energy_rotation_init = false;
}
//else cout << clockwise_rotation_init_cnt << endl;
last_target_position = target_position;
}
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
#include "log.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
void Energy::initEnergy() {
isSendTarget = false;
isMark = false;
fans_cnt = 0;
armors_cnt = 0;
cycle_center = Point(0, 0);
target_center = Point(0, 0);
last_target_center = Point(0, 0);
hit_point = Point(0, 0);
target_position = -1000;
last_target_position = -1000;
last_hit_position = 20000;
target_armor = -1000;
last_target_armor = -1000;
radius = 0;
// ally_color = ALLY_RED;
energy_part_rotation = CLOCKWISE;
attack_distance = ATTACK_DISTANCE;
count = 1;
last_fans_cnt = 0;
last_armors_cnt = 0;
send_cnt = 0;
//rectified_focal_length = 1000;
//theta = 0;
//phi = 0;
yaw_rotation = 0;
pitch_rotation = 0;
last_mark = 0;
red_origin_yaw = -0.35;
red_origin_pitch = 15.11719;
blue_origin_yaw = -0.439453;
blue_origin_pitch = 15.688477;
target_cnt = 0;
target_cnt_flag = true;
isLeftVertexFound = -1;
isTopVertexFound = -1;
isRightVertexFound = -1;
isBottomVertexFound = -1;
left = Point(640, 480);
right = Point(0, 0);
top = Point(640, 480);
bottom = Point(0, 0);
position_mode = 0;
last_position_mode = 0;
energy_rotation_init = false;
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
Armor_center.clear();
first_armor_centers.clear();
all_armor_centers.clear();
clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0;
}
void Energy::initEnergyPartParam() {
energy_part_param_.RPM = 10;
energy_part_param_.HIT_TIME = 1.14;
energy_part_param_.GRAY_THRESH = 240;
energy_part_param_.SPLIT_GRAY_THRESH = 60;
energy_part_param_.FAN_GRAY_THRESH = 75;
energy_part_param_.ARMOR_GRAY_THRESH = 80;
energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;
energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 90;
energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 140;
energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35;
energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 60;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.TWIN_ANGEL_MAX = 10;
lift_height_.LIFT_0 = 0;
lift_height_.LIFT_30 = 0;
lift_height_.LIFT_60 = 0;
lift_height_.LIFT_90 = 10;
lift_height_.LIFT_minus_30 = 0;
lift_height_.LIFT_minus_60 = 0;
lift_height_.LIFT_minus_90 = 0;
}
void Energy::initRotation() {
target_position = target_armor;
// cout << "target position: " << target_position << '\t' << "last target position: " << last_target_position << endl;
if (target_position >= -180 && last_target_position >= -180 && fabs(target_position - last_target_position) < 30) {
if (target_position < last_target_position) clockwise_rotation_init_cnt++;
else if (target_position > last_target_position) anticlockwise_rotation_init_cnt++;
}
if (clockwise_rotation_init_cnt == 30) {
energy_part_rotation = CLOCKWISE;
cout << "rotation: " << energy_part_rotation << endl;
energy_rotation_init = false;
}
else if (anticlockwise_rotation_init_cnt == 30) {
energy_part_rotation = ANTICLOCKWISE;
cout << "rotation: " << energy_part_rotation << endl;
energy_rotation_init = false;
}
// else cout << clockwise_rotation_init_cnt <<'\t'<<anticlockwise_rotation_init_cnt<< endl;
last_target_position = target_position;
}

250
energy/src/energy/run.cpp
View File

@@ -1,107 +1,143 @@
//
// Created by xixiliadorabarry on 3/5/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
//extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
//extern int mark;
int Energy::run(cv::Mat &src){
imshow("src",src);
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
// gimble_zero_points.clear();
isSendTarget = false;
changeMark();
if (isMark)return 0;
// if(all_armor_centers.size()>200)all_armor_centers.clear();
// if(first_armor_centers.size()>200)first_armor_centers.clear();
// cout<<"first_armor_centers.size(): "<<first_armor_centers.size()<<endl;
// imagePreprocess(src);
// imshow("img_preprocess",src);
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",src);
fans_cnt = findFan(src, fans, last_fans_cnt);
// cout<<"fans_cnt: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧
// if(fans_cnt>0)showFanContours("fan",src,fans);
// fans_cnt=0;
armors_cnt = findArmor(src, armors, last_armors_cnt);
// cout<<"armors_cnt: "<<armors_cnt<<endl;
if(armors_cnt==-1) return 0;//滤去漏判的帧
// if(armors_cnt>0) showArmorContours("armor",src,armors);
if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
if(armors_cnt != fans_cnt+1) return 0;
getAllArmorCenters();
// cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit();
// cycle_center = cv::Point(335, 246);
// radius = 116.936;
// attack_distance = ATTACK_DISTANCE * 120/ radius;
attack_distance = 794 + 1245 * 75 * (1/radius - 1/115.6);
// cout << "attack distance: " << attack_distance << endl;
getFanPosition(fanPosition, fans, cycle_center, radius);
getArmorPosition(armorPosition, armors, cycle_center, radius);
findTarget(fanPosition, armorPosition, target_armor);
// cout << "The target armor's position is " << target_armor << endl;
// cout<<"The target armor center is: "<<target_center<<endl;
if (energy_rotation_init) {
initRotation();
return 0;
}
getHitPoint();
// hit_point = target_center;
// cout << "The hit point position is " << hit_point << endl;
// cout<<"send"<<endl;
cout<<"position mode: "<<position_mode<<endl;
gimbleRotation();
if (!isSendTarget)return 0;
sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);
cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;
}
//-----------------------------------------------------------------------------------
/*
//此处用于标定云台在摄像头视频中的零点
findGimbleZeroPoint(src,gimble_zero_points);
cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
showFanContours("zero",src,gimble_zero_points);
cycle_center = cv::Point(291,305);
if(gimble_zero_points.size()>0)hit_point = gimble_zero_points.at(0).rect.center;
*/
//
// Created by xixiliadorabarry on 3/5/19.
//
#include "energy/energy.h"
#include "log.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
//extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
//extern int mark;
int Energy::run(cv::Mat &src){
// imshow("src",src);
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
// gimble_zero_points.clear();
isSendTarget = false;
changeMark();
if (isMark)return 0;
// cout<<"yaw"<<origin_yaw<<endl;
// if(all_armor_centers.size()>200)all_armor_centers.clear();
// if(first_armor_centers.size()>200)first_armor_centers.clear();
// cout<<"first_armor_centers.size(): "<<first_armor_centers.size()<<endl;
// imagePreprocess(src);
// imshow("img_preprocess",src);
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",src);
fans_cnt = findFan(src, fans, last_fans_cnt);
// cout<<"fans_cnt: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧
// if(fans_cnt>0)showFanContours("fan",src,fans);
// fans_cnt=0;
armors_cnt = findArmor(src, armors, last_armors_cnt);
// cout<<"armors_cnt: "<<armors_cnt<<endl;
if(armors_cnt==-1) return 0;//滤去漏判的帧
// if(armors_cnt>0) showArmorContours("armor",src,armors);
if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
if(armors_cnt>=4 && fans_cnt>=3) {
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w");
if (fp) {
fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
fclose(fp);
save_new_mark = false;
}
FILE *fp_all = fopen(PROJECT_DIR"/Mark/mark_all.txt", "a");
if (fp_all) {
fprintf(fp_all, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
fclose(fp_all);
}
}
if(armors_cnt==5){
FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a");
if(fp_best){
fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch);
fclose(fp_best);
}
}
// cout<<"armors_cnt: "<<armors_cnt<<"fans_cnt: "<<fans_cnt<<endl;
// cout<<"armors_cnt: "<<armors_cnt<<"fans_cnt: "<<fans_cnt<<endl;
// if(armors_cnt != fans_cnt+1)
// {
// return 0;
// }
//cout<<"clock: "<<energy_part_rotation<<endl;
getAllArmorCenters();
// cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit();
// cycle_center = cv::Point(335, 246);
// radius = 116.936;
// attack_distance = ATTACK_DISTANCE * 120/ radius;
// attack_distance = 674 + 1286 * 75 * (1/radius - 1/133.85);
// cout<<"radius"<<radius<<endl;
// cout << "attack distance: " << attack_distance << endl;
// attack_distance = 752;//单项赛
attack_distance = 718;
getFanPosition(fanPosition, fans, cycle_center, radius);
getArmorPosition(armorPosition, armors, cycle_center, radius);
findTarget(fanPosition, armorPosition, target_armor);
// cout << "The target armor's position is " << target_armor << endl;
// cout<<"The target armor center is: "<<target_center<<endl;
// cout<<target_armor<<endl;
if (energy_rotation_init) {
initRotation();
return 0;
}
getHitPoint();
// hit_point = target_center;
// cout << "The hit point position is " << hit_point << endl;
// cout<<"send"<<endl;
// cout<<"position mode: "<<position_mode<<endl;
gimbleRotation();
if(changeTarget())target_cnt++;
// if (!isSendTarget)return 0;
sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
// cout<<target_cnt<<endl;
// cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
// cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;
}
//-----------------------------------------------------------------------------------
/*
//此处用于标定云台在摄像头视频中的零点
findGimbleZeroPoint(src,gimble_zero_points);
cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
showFanContours("zero",src,gimble_zero_points);
cycle_center = cv::Point(291,305);
if(gimble_zero_points.size()>0)hit_point = gimble_zero_points.at(0).rect.center;
*/

21
energy/src/energy/send_target_by_uart/send_target_by_uart.cpp
View File

@@ -2,20 +2,22 @@
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
#include <iostream>
using namespace std;
bool sendTarget(Serial& serial, float x, float y, float z) {
static short x_tmp, y_tmp, z_tmp;
static time_t last_time = time(nullptr);
static int fps;
// static time_t last_time = time(nullptr);
// static int fps;
uint8_t buff[8];
time_t t = time(nullptr);
if (last_time != t) {
last_time = t;
std::cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << std::endl;
fps = 0;
}
fps += 1;
// time_t t = time(nullptr);
// if (last_time != t) {
// last_time = t;
// std::cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << std::endl;
// fps = 0;
// }
// fps += 1;
x_tmp = static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
@@ -39,6 +41,7 @@ void Energy::sendTargetByUart(float x, float y, float z) {
// }
sendTarget(serial, x, y, z);
send_cnt+=1;
// cout<<"send"<<endl;
}

46
main.cpp
View File

@@ -31,18 +31,15 @@ mcu_data mcuData = {
0,
ARMOR_STATE,
0,
0,
ENEMY_BLUE,
1,
ENEMY_RED,
};
int main(int argc, char *argv[]) {
process_options(argc, argv);
Serial serial(115200);
uint8_t last_state = mcuData.state;
thread receive(uartReceive, &serial);
bool keep = true;
int from_camera = 1;
if (!run_with_camera) {
@@ -50,7 +47,7 @@ int main(int argc, char *argv[]) {
cin >> from_camera;
}
while (keep) {
while (true) {
VideoWriter armor_video_writer, energy_video_writer;
if (save_video) {
initVideoWriter(armor_video_writer, PROJECT_DIR"/armor_video/");
@@ -63,8 +60,13 @@ int main(int argc, char *argv[]) {
video_armor = new CameraWrapper(0, "armor");
video_energy = new CameraWrapper(1, "energy");
} else {
video_armor = new VideoWrapper("E:/Robomaster/RM_auto-aim/build/r_l_640.avi");
video_energy = new VideoWrapper("E:/Robomaster/RM_auto-aim/build/r_l_640.avi");
// string armor_video, energy_video;
// lastVideo(armor_video, PROJECT_DIR"/armor_video/");
// video_armor = new VideoWrapper(armor_video);
// lastVideo(energy_video, PROJECT_DIR"/energy_video/");
// video_energy = new VideoWrapper(energy_video);
video_armor = new VideoWrapper("/home/sjturm/Desktop/valid_video/armor/65.avi");
video_energy = new VideoWrapper("/home/sjturm/Desktop/valid_video/energy/121.avi");
}
if (video_armor->init()) {
LOGM("video_armor source initialization successfully.");
@@ -98,11 +100,13 @@ int main(int argc, char *argv[]) {
energy.setRotation(CLOCKWISE);
bool ok = true;
cout<<"start running"<<endl;
do {
CNT_TIME("Total", {
if (mcuData.state == ENERGY_STATE) {
if (last_state == ARMOR_STATE) {
energy.setEnergyRotationInit();
cout << "set" << endl;
}
last_state = mcuData.state;
if (video_energy) {
@@ -112,17 +116,22 @@ int main(int argc, char *argv[]) {
video_energy = nullptr;
}
if(save_video){
energy_video_writer.write(energy_src);
Mat energy_save = energy_src.clone();
cvtColor(energy_save,energy_save,COLOR_GRAY2BGR);
energy_video_writer.write(energy_save);
// cout<<energy_src.type()<<endl;
// LOGM(STR_CTR(WORD_GREEN,"Save ENERGY!"));
}
if (show_origin) {
imshow("energy src", energy_src);
}
if (from_camera == 0) {
cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
imshow("resize", energy_src);
energy.extract(energy_src);
}
// if (from_camera == 0) {
// cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
// imshow("resize", energy_src);
// energy.extract(energy_src);
// }
energy.run(energy_src);
// waitKey(1);
} else {
video_energy = new CameraWrapper(1, "energy");
if(!video_energy->init()){
@@ -141,8 +150,10 @@ int main(int argc, char *argv[]) {
}
if(save_video){
armor_video_writer.write(armor_src);
// cout<<armor_src.type()<<endl;
// LOGM(STR_CTR(WORD_GREEN,"Save ARMOR!"));
}
flip(armor_src, armor_src, 0);
// flip(armor_src, armor_src, 0);
if (show_origin) {
imshow("armor src", armor_src);
}
@@ -157,9 +168,8 @@ int main(int argc, char *argv[]) {
}
}
}
if(waitKey(1) == 'q'){
keep = false;
}
// cout<<last_state<<endl;
waitKey(1);
});
} while (ok);

3
others/include/additions/additions.h
View File

@@ -25,6 +25,7 @@ struct mcu_data{
extern mcu_data mcuData;
void uartReceive(Serial *pSerial);
void initVideoWriter(cv::VideoWriter& video, const std::string &armor_filename_prefix);
void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix);
void lastVideo(std::string &video_name, const std::string &filename_prefix);
#endif /* _ADDITIONS_H_ */

4
others/include/log.h
View File

@@ -138,9 +138,9 @@
#include <sys/time.h>
#define CNT_TIME(tag, codes, ...) do{ \
static timeval ts, te; \
gettimeofday(&ts); \
gettimeofday(&ts, NULL); \
codes; \
gettimeofday(&te); \
gettimeofday(&te, NULL); \
LOGM(tag": %.1lfms", (te.tv_sec-ts.tv_sec)*1000.0+(te.tv_usec-ts.tv_usec)/1000.0); \
}while (0)
#endif

16
others/include/options/additions.h
View File

@@ -1,16 +0,0 @@
//
// Created by xinyang on 19-4-7.
//
#ifndef _ADDITIONS_H_
#define _ADDITIONS_H_
#include <serial/serial.h>
#include <opencv2/core.hpp>
#include <thread>
void save_video_file(cv::Mat &src);
void save_labelled_image(cv::Mat &src, cv::Rect2d box);
#endif /* _ADDITIONS_H_ */

35
others/src/additions/additions.cpp
View File

@@ -7,24 +7,32 @@
#include <additions/additions.h>
#include <log.h>
#include <opencv2/videoio/videoio_c.h>
#include <iostream>
#define RECEIVE_LOG_LEVEL LOG_NOTHING
#define RECEIVE_LOG_LEVEL LOG_MSG
using namespace std;
void uartReceive(Serial* pSerial) {
char buffer[100];
char buffer[20];
int cnt = 0;
LOGM("data receive start!");
LOGM(STR_CTR(WORD_LIGHT_WHITE,"data receive start!"));
while (true) {
char byte;
char byte = 0;
memset(buffer, 0, sizeof(buffer));
while (pSerial->ReadData((uint8_t*)&byte, 1) && byte!='\n') {
buffer[cnt++] = byte;
if (cnt >= 100) {
LOG(RECEIVE_LOG_LEVEL, "data receive over flow!");
if (cnt >= sizeof(buffer)) {
// LOGE("data receive over flow!");
cnt = 0;
}
}
if (cnt==0 && byte=='\n'){
LOGM("%d", cnt);
}
if (cnt == sizeof(mcuData)) {
memcpy(&mcuData, buffer, sizeof(mcuData));
LOGM("Get, state:%c, mark:%d!", mcuData.state, (int)mcuData.mark);
}
cnt = 0;
}
@@ -38,13 +46,24 @@ void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix)
in >> cnt;
in.close();
}
std::string armor_file_name = filename_prefix + std::to_string(cnt) + ".avi";
std::string file_name = filename_prefix + std::to_string(cnt) + ".avi";
cnt++;
std::ofstream out(filename_prefix + "cnt.txt");
if (out.is_open()) {
out << cnt << std::endl;
out.close();
}
video.open(armor_file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true);
video.open(file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true);
}
void lastVideo(std::string &video_name, const std::string &filename_prefix){
std::ifstream in(filename_prefix + "cnt.txt");
int cnt = 0;
if (in.is_open())
{
in >> cnt;
in.close();
}
if(cnt > 1) std::string video_name = filename_prefix + std::to_string(cnt) + ".avi";
}

10
others/src/camera/camera_wrapper.cpp
View File

@@ -61,6 +61,7 @@ bool CameraWrapper::init() {
rgb_buffer = (unsigned char *)malloc(tCapability.sResolutionRange.iHeightMax *
tCapability.sResolutionRange.iWidthMax * 3);
#ifdef Windows
char filepath[200];
sprintf(filepath, PROJECT_DIR"/others/%s.Config", name.data());
if (CameraReadParameterFromFile(h_camera, filepath) != CAMERA_STATUS_SUCCESS) {
@@ -72,6 +73,15 @@ bool CameraWrapper::init() {
return false;
}
LOGM("successfully loaded %s!", filepath);
#elif defined(Linux)
CameraSetAeState(h_camera, false);
CameraSetExposureTime(h_camera, 17*1000);
CameraSetAnalogGain(h_camera, 60);
if(mode == 0){
CameraSetGain(h_camera, 140, 140, 140);
CameraSetLutMode(h_camera, LUTMODE_PRESET);
}
#endif
double t;
CameraGetExposureTime(h_camera, &t);
LOGM("Exposure time: %lfms", t / 1000.0);

71
others/src/options/additions.cpp
View File

@@ -1,71 +0,0 @@
//
// Created by xinyang on 19-4-7.
//
#include <options/additions.h>
#include <options/options.h>
#include <stdio.h>
#include <log.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
using namespace std;
#define VIDEO_SAVE_DIR "/home/sjturm/Desktop/video/"
static cv::VideoWriter *create_video_writer(){
FILE* info = fopen(VIDEO_SAVE_DIR"info.txt", "r");
int cnt=0;
fscanf(info, "%d", &cnt);
fclose(info);
info = fopen(VIDEO_SAVE_DIR"info.txt", "w");
fprintf(info, "%d", ++cnt);
char name[100];
sprintf(name, VIDEO_SAVE_DIR"%d.avi", cnt);
return new cv::VideoWriter(name, cv::VideoWriter::fourcc('P','I','M','1'), 80, cv::Size(640,480),false);
}
void save_video_file(cv::Mat &src){
static cv::VideoWriter *video = create_video_writer();
video->write(src);
}
#define SAVE_DIR "/home/sjturm/Desktop/labelled/"
bool save_label_error_flag = false;
int get_labelled_cnt(){
FILE *fp = fopen(SAVE_DIR"info.txt", "r");
int cnt=0;
fscanf(fp, "%d", &cnt);
fclose(fp);
return cnt+1;
}
void save_labelled_cnt(int cnt){
FILE *fp = fopen(SAVE_DIR"info.txt", "w");
fprintf(fp, "%d", cnt);
fclose(fp);
}
void save_labelled_image(cv::Mat &src, cv::Rect2d box){
if(!save_label_error_flag) {
static int cnt = get_labelled_cnt();
char name[50];
sprintf(name, SAVE_DIR"%d.jpg", cnt);
cv::imwrite(name, src);
sprintf(name, SAVE_DIR"%d.txt", cnt);
FILE *fp = fopen(name, "w");
if (fp == NULL) {
LOGW("Can't create file: %s!\nStop saving!", name);
save_label_error_flag = true;
return;
}
fprintf(fp, "%lf %lf %lf %lf\n", box.x, box.y, box.width, box.height);
fclose(fp);
save_labelled_cnt(cnt);
}
}

2
others/src/serial/serial.cpp
View File

@@ -2,7 +2,7 @@
#include <options/options.h>
#include <iostream>
#define LOG_LEVEL LOG_NONE
//#define LOG_LEVEL LOG_NONE
#include <log.h>
using namespace std;

2
tools/TrainCNN/backward.py Executable file → Normal file
View File

@@ -75,7 +75,7 @@ def train(dataset, show_bar=False):
learning_rate = tf.train.exponential_decay(
LEARNING_RATE_BASE,
global_step,
len(dataset.train_samples) / BATCH / 5,
len(dataset.train_samples) / BATCH,
LEARNING_RATE_DECAY,
staircase=False)
train_step = tf.train.AdamOptimizer(learning_rate).minimize(loss, global_step=global_step)

6
tools/TrainCNN/forward.py
View File

@@ -71,16 +71,14 @@ def forward(x, regularizer=None):
pool_shape = pool2.get_shape().as_list()
node = pool_shape[1] * pool_shape[2] * pool_shape[3]
reshaped = tf.reshape(pool2, [-1, node])
# reshaped = tf.nn.dropout(reshaped, 0.1)
fc1_w = get_weight([node, FC1_OUTPUT_NODES], regularizer)
fc1_w = tf.nn.dropout(get_weight([node, FC1_OUTPUT_NODES], regularizer), 0.1)
fc1_b = get_bias([FC1_OUTPUT_NODES])
fc1 = tf.nn.relu(tf.matmul(reshaped, fc1_w) + fc1_b)
# fc1 = tf.nn.dropout(fc1, 0.2)
vars.extend([fc1_w, fc1_b])
nodes.extend([fc1])
fc2_w = get_weight([FC1_OUTPUT_NODES, FC2_OUTPUT_NODES], regularizer)
fc2_w = tf.nn.dropout(get_weight([FC1_OUTPUT_NODES, FC2_OUTPUT_NODES], regularizer), 0.1)
fc2_b = get_bias([FC2_OUTPUT_NODES])
fc2 = tf.nn.softmax(tf.matmul(fc1, fc2_w) + fc2_b)
vars.extend([fc2_w, fc2_b])

7
tools/TrainCNN/generate.py
View File

@@ -41,13 +41,6 @@ class DataSet:
def generate_data_sets(self, folder):
sets = []
mini = 1000000
for i in range(OUTPUT_NODES):
dir = "%s/%d" % (folder, i)
files = os.listdir(dir)
if mini > len(files):
mini = len(files)
for i in range(OUTPUT_NODES):
dir = "%s/%d" % (folder, i)
files = os.listdir(dir)

0
tools/analysis.py Executable file → Normal file
View File

2
tools/bind-monitor.sh Executable file → Normal file
View File

@@ -1,5 +1,5 @@
#!/bin/bash
echo "#!/bin/bash" > $2/monitor-run
echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --show-armor-box --wait-uart --save-video\\\"\"" >> $2/monitor-run
echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --save-video --wait-uart\\\"\"" >> $2/monitor-run
chmod +x $2/monitor-run

2
tools/monitor.sh Executable file → Normal file
View File

@@ -10,5 +10,5 @@ while true; do
exec $exe &
echo "restart $exe"
fi
sleep 2
sleep 0.5
done