VSAG/vision_sjtu_19
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

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;
}