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

energy update

Browse Source
This commit is contained in:
JiatongSun
2019-05-18 17:58:59 +08:00
parent 424cc724d8
commit 2369834564
11 changed files with 186 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
CMakeLists.txt
View File

@@ -1,12 +1,11 @@
CMAKE_MINIMUM_REQUIRED(VERSION 3.5)
PROJECT(AutoAim)
PROJECT(RM-AutoAim)
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,28 +21,17 @@ IF(CCACHE_FOUND)
MESSAGE("< Use ccache for compiler >")
ENDIF()
FIND_PACKAGE(Eigen3 REQUIRED)
FIND_PACKAGE(OpenCV 3 REQUIRED)
FIND_PACKAGE(Threads)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
INCLUDE_DIRECTORIES("D:/Program\ Files/eigen3.3.7")
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/energy/include)
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})
IF (CMAKE_SYSTEM_NAME MATCHES "Linux")
MESSAGE(STATUS "current platform: Linux ")
TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libMVSDK.so")
ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Windows")
MESSAGE(STATUS "current platform: Windows")
TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib")
ELSE ()
MESSAGE(STATUS "Unsupport platform: ${CMAKE_SYSTEM_NAME}")
ENDIF()
ADD_CUSTOM_TARGET(bind-monitor COMMAND "${PROJECT_SOURCE_DIR}/tools/bind-monitor.sh" "${PROJECT_SOURCE_DIR}" "${CMAKE_BINARY_DIR}")
TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib")

14
energy/include/energy/energy.h
View File

@@ -31,6 +31,7 @@ public:
// void setAllyColor(int color);
void setRotation(int rotation);
void setEnergyRotationInit();
void extract(cv::Mat &src);
@@ -42,6 +43,7 @@ private:
EnergyPartParam energy_part_param_;
LiftHeight lift_height_;
bool isSendTarget;
bool isMark;
int fans_cnt;
int armors_cnt;
int count;
@@ -61,13 +63,18 @@ private:
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;
// std::vector<EnergyPart> gimble_zero_points;
cv::Point cycle_center;
cv::Point target_center;
@@ -81,9 +88,9 @@ private:
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);
@@ -121,6 +128,7 @@ private:
void getHitPoint();
bool changeTarget();
void changeMark();
void gimbleRotation();
void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);

49
energy/src/energy/energy.cpp
View File

@@ -8,51 +8,13 @@ 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)
{
isSendTarget = 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 = -1;
last_target_position = -1;
last_hit_position = 20000;
target_armor = -1;
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;
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;
initEnergy();
initEnergyPartParam();
}
@@ -68,3 +30,8 @@ void Energy::setRotation(int rotation){
energy_part_rotation = rotation;
}
void Energy::setEnergyRotationInit() {
initEnergy();
initEnergyPartParam();
energy_rotation_init = true;
}

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

@@ -9,16 +9,28 @@ using std::cout;
using std::endl;
using std::vector;
extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
void Energy::changeMark() {
if (mcuData.mark == 0 && last_mark == 1) {
last_mark = mcuData.mark;
origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch;
isMark = true;
}
else if (mcuData.state == 1) {
last_mark = mcuData.mark;
isMark = true;
}
else {
last_mark = mcuData.mark;
isMark = false;
}
}
void Energy::gimbleRotation(){
cv::Point2f real_hit_point;
stretch(hit_point, real_hit_point);
float origin_yaw = -0.13;
float origin_pitch = 13.18;
/*origin_yaw = mark_yaw;
origin_pitch = mark_pitch;*/

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

@@ -118,13 +118,13 @@ void Energy::getHitPoint(){
}
}
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;
}
else{
last_target_position = target_position;
return true;
}
}
//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;
// }
// else{
// last_target_position = target_position;
// return true;
// }
//}

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

@@ -8,6 +8,66 @@ 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;
@@ -49,3 +109,25 @@ void Energy::initEnergyPartParam() {
}
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;
}

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

@@ -18,10 +18,11 @@ int Energy::run(cv::Mat &src){
armors.clear();
fanPosition.clear();
armorPosition.clear();
gimble_zero_points.clear();
// gimble_zero_points.clear();
isSendTarget = false;
//if(mark==0)return 0;
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();
@@ -57,7 +58,7 @@ int Energy::run(cv::Mat &src){
// radius = 116.936;
// attack_distance = ATTACK_DISTANCE * 120/ radius;
attack_distance = 794 + 1245 * 75 * (1/radius - 1/113.9);
attack_distance = 794 + 1245 * 75 * (1/radius - 1/115.6);
// cout << "attack distance: " << attack_distance << endl;
getFanPosition(fanPosition, fans, cycle_center, radius);
@@ -66,21 +67,24 @@ int Energy::run(cv::Mat &src){
// 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;
if(!isSendTarget)return 0;
// 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<<"mark_yaw: "<<mcuData.mark_yaw<<'\t'<<"mark_pitch: "<<mcuData.mark_pitch<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;

41
main.cpp
View File

@@ -27,19 +27,31 @@ using namespace cv;
using namespace std;
mcu_data mcuData = {
.curr_yaw = 0,
.curr_pitch = 0,
.state = ENERGY_STATE,
.mark = 0,
.use_classifier = 0,
.enemy_color = ENEMY_BLUE
0,
0,
ARMOR_STATE,
0,
0,
ENEMY_BLUE,
};
int main(int argc, char *argv[]) {
process_options(argc, argv);
Serial serial(115200);
uint8_t last_state = mcuData.state;
thread receive(uartReceive, &serial);
thread change([&]() {
while (true) {
Sleep(10000);
if (mcuData.state == ARMOR_STATE) mcuData.state = ENERGY_STATE;
else if (mcuData.state == ENERGY_STATE)mcuData.state = ARMOR_STATE;
cout << "state changed to " << mcuData.state << endl;
}
});
int from_camera = 1;
if (!run_with_camera) {
cout << "Input 1 for camera, 0 for video files" << endl;
@@ -59,8 +71,8 @@ int main(int argc, char *argv[]) {
video_armor = new CameraWrapper(0, "armor");
video_energy = new CameraWrapper(1, "energy");
} else {
video_armor = new VideoWrapper("/home/xinyang/Desktop/DataSets/video/blue_4.mp4");
video_energy = new VideoWrapper("/home/xinyang/Desktop/DataSets/video/blue_4.mp4");
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");
}
if (video_armor->init()) {
LOGM("video_armor source initialization successfully.");
@@ -83,7 +95,7 @@ int main(int argc, char *argv[]) {
video_armor->read(armor_src);
}
if (video_energy) {
video_energy->read(armor_src);
video_energy->read(energy_src);
}
}
@@ -97,6 +109,11 @@ int main(int argc, char *argv[]) {
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) {
ok = video_energy->read(energy_src);
if (!ok) {
@@ -110,6 +127,8 @@ int main(int argc, char *argv[]) {
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);
}
energy.run(energy_src);
@@ -122,6 +141,7 @@ int main(int argc, char *argv[]) {
}
} else if (mcuData.state == ARMOR_STATE) {
last_state = mcuData.state;
if (video_armor) {
ok = video_armor->read(armor_src);
if (!ok) {
@@ -146,6 +166,9 @@ int main(int argc, char *argv[]) {
}
}
}
waitKey(1);
});
} while (ok);

BIN
others/MVCAMSDK_X64.lib Normal file
View File

Binary file not shown.

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

@@ -20,8 +20,6 @@ struct mcu_data{
uint8_t mark;
uint8_t use_classifier;
uint8_t enemy_color;
float mark_yaw;
float mark_pitch;
};
extern mcu_data mcuData;

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

@@ -30,20 +30,20 @@ void uartReceive(Serial* pSerial) {
}
}
void initVideoWriter(cv::VideoWriter& video, const std::string &armor_filename_prefix){
std::ifstream in_1(armor_filename_prefix + "cnt.txt");
int cnt_1 = 0;
if (in_1.is_open())
void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix){
std::ifstream in(filename_prefix + "cnt.txt");
int cnt = 0;
if (in.is_open())
{
in_1 >> cnt_1;
in_1.close();
in >> cnt;
in.close();
}
std::string armor_file_name = armor_filename_prefix + std::to_string(cnt_1) + ".avi";
cnt_1++;
std::ofstream out_1(armor_filename_prefix + "cnt.txt");
if (out_1.is_open()) {
out_1 << cnt_1 << std::endl;
out_1.close();
std::string armor_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);
}