From c0c12b24f0354bbb557991f11de57907f98ad58b Mon Sep 17 00:00:00 2001 From: xinyang <895639507@qq.com> Date: Fri, 31 May 2019 19:29:22 +0800 Subject: [PATCH] =?UTF-8?q?2019=E8=B5=9B=E5=AD=A3RM=E4=B8=AD=E9=83=A8?= =?UTF-8?q?=E5=88=86=E5=8C=BA=E8=B5=9B=EF=BC=8C=E8=87=AA=E7=9E=84=E5=92=8C?= =?UTF-8?q?=E8=83=BD=E9=87=8F=E6=9C=BA=E5=85=B3=EF=BC=8C=E5=AE=8C=E6=95=B4?= =?UTF-8?q?=E7=A8=B3=E5=AE=9A=E7=89=88=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 1 + CMakeLists.txt | 6 +- armor/include/armor_finder/armor_finder.h | 7 +- armor/src/armor_finder/armor_finder.cpp | 4 +- .../image_process/image_process.cpp | 16 +- .../searching_state/searching_state.cpp | 71 ++++- armor/src/show_images/show_images.cpp | 6 +- energy/include/energy/energy.h | 294 +++++++++--------- energy/src/energy/energy.cpp | 98 +++--- energy/src/energy/get/gimble_rotation_get.cpp | 59 ++-- energy/src/energy/get/hit_point_get.cpp | 231 +++++++------- energy/src/energy/param_init.cpp | 274 ++++++++-------- energy/src/energy/run.cpp | 250 ++++++++------- .../send_target_by_uart.cpp | 21 +- main.cpp | 46 +-- others/include/additions/additions.h | 3 +- others/include/log.h | 4 +- others/include/options/additions.h | 16 - others/src/additions/additions.cpp | 35 ++- others/src/camera/camera_wrapper.cpp | 10 + others/src/options/additions.cpp | 71 ----- others/src/serial/serial.cpp | 2 +- tools/TrainCNN/backward.py | 2 +- tools/TrainCNN/forward.py | 6 +- tools/TrainCNN/generate.py | 7 - tools/analysis.py | 0 tools/bind-monitor.sh | 2 +- tools/monitor.sh | 2 +- 28 files changed, 816 insertions(+), 728 deletions(-) delete mode 100644 others/include/options/additions.h delete mode 100644 others/src/options/additions.cpp mode change 100755 => 100644 tools/TrainCNN/backward.py mode change 100755 => 100644 tools/analysis.py mode change 100755 => 100644 tools/bind-monitor.sh mode change 100755 => 100644 tools/monitor.sh diff --git a/.gitignore b/.gitignore index b15ebf5..00b23c5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ cmake-build-debug build .idea +Mark tools/TrainCNN/.idea tools/TrainCNN/__pycache__ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index d8ce09c..9e2e677 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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}) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 5321491..03104b8 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -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); diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index cb37844..578b6d9 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -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; diff --git a/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp b/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp index 0c04fca..e43bdec 100644 --- a/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp +++ b/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp @@ -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); diff --git a/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp b/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp index 39a9a05..be2bdf4 100644 --- a/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp +++ b/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp @@ -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 &armor_boxes) { +static bool findArmorBoxes(LightBlobs &light_blobs, std::vector &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(i)).rect.boundingRect(); @@ -151,6 +154,33 @@ bool judge_light_color(std::vector &light, std::vector &co return !result.empty(); } +void get_blob_color(const cv::Mat &src, std::vector &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(row, col)[2]; + blue_cnt += roi.at(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; } diff --git a/armor/src/show_images/show_images.cpp b/armor/src/show_images/show_images.cpp index d5dd299..322444f 100644 --- a/armor/src/show_images/show_images.cpp +++ b/armor/src/show_images/show_images.cpp @@ -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); } diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 038b7d7..03ff609 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -1,143 +1,151 @@ -// -// Created by xixiliadorabarry on 1/24/19. -// -#ifndef ENERGY_H -#define ENERGY_H - -#include -#include -#include -#include -#include -#include -#include -#include -#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 fans; - std::vector armors; - // std::vector gimble_zero_points; - - cv::Point cycle_center; - cv::Point target_center; - cv::Point last_target_center; - cv::Point hit_point; - std::vectorfanPosition; - std::vectorarmorPosition; - std::vector Armor_center; - std::vector first_armor_centers; - std::vector 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 &fans, int &last_fans_cnt); - int findArmor(const cv::Mat &src, vector &armors, int &last_armors_cnt); - int findGimbleZeroPoint(const cv::Mat &src, vector &gimble_zero_point); - - void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector &fans); - void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector &armors); - void showBothContours(std::string windows_name, const cv::Mat &src, const std::vector &fans, - const std::vector &armors); - - bool isValidFanContour(const vector &fan_contour); - bool isValidArmorContour(const vector &armor_contour); - - void getFanPosition(std::vector &fanPosition, const std::vector &fans, cv::Point cycle_center, double radius); - void getArmorPosition(std::vector &armorPosition, const std::vector &armors, cv::Point cycle_center, double radius); - void getFirstArmorCenters(vector &armors, std::vector &first_armor_centers); - void getAllArmorCenters(); - void getPosition(cv::Point point, double &angle); - - void cycleQuickCalculate(std::vector &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::vectorfanPosition, const std::vectorarmorPosition, float &target_armor); - - void findWholeCycle(const std::vector&first_armor_centers); - - void saveFourPoints(std::vector &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4); - void savePoint2f(std::vector &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 ¢er, 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 +#include +#include +#include +#include +#include +#include +#include +#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 fans; + std::vector armors; + // std::vector gimble_zero_points; + + cv::Point cycle_center; + cv::Point target_center; + cv::Point last_target_center; + cv::Point hit_point; + std::vectorfanPosition; + std::vectorarmorPosition; + std::vector Armor_center; + std::vector first_armor_centers; + std::vector 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 &fans, int &last_fans_cnt); + int findArmor(const cv::Mat &src, vector &armors, int &last_armors_cnt); + int findGimbleZeroPoint(const cv::Mat &src, vector &gimble_zero_point); + + void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector &fans); + void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector &armors); + void showBothContours(std::string windows_name, const cv::Mat &src, const std::vector &fans, + const std::vector &armors); + + bool isValidFanContour(const vector &fan_contour); + bool isValidArmorContour(const vector &armor_contour); + + void getFanPosition(std::vector &fanPosition, const std::vector &fans, cv::Point cycle_center, double radius); + void getArmorPosition(std::vector &armorPosition, const std::vector &armors, cv::Point cycle_center, double radius); + void getFirstArmorCenters(vector &armors, std::vector &first_armor_centers); + void getAllArmorCenters(); + void getPosition(cv::Point point, double &angle); + + void cycleQuickCalculate(std::vector &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::vectorfanPosition, const std::vectorarmorPosition, float &target_armor); + + void findWholeCycle(const std::vector&first_armor_centers); + + void saveFourPoints(std::vector &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4); + void savePoint2f(std::vector &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 ¢er, 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 + diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index e55db89..472c663 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -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); + } + } +} diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index 47ad216..aaf68b2 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -3,6 +3,7 @@ // #include "energy/energy.h" #include +#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: "<(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); - } +// if(position_mode == 1){ +// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(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(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); +// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); +// } // else{ // pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); // } diff --git a/energy/src/energy/get/hit_point_get.cpp b/energy/src/energy/get/hit_point_get.cpp index 43d4370..9394d7c 100644 --- a/energy/src/energy/get/hit_point_get.cpp +++ b/energy/src/energy/get/hit_point_get.cpp @@ -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(radius / 2); - hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); - hit_position = -1*angle_interval; - position_mode = 1; - } - else if(target_armor>=angle_interval && target_armor(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(radius / 2); - hit_point.y = cycle_center.y - static_cast(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(radius / 2); - hit_point.y = cycle_center.y - static_cast(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(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(radius / 2); - hit_point.y = cycle_center.y + static_cast(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(radius / 2); - hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); - hit_position = 2*angle_interval; - position_mode = 1; - } - else if(target_armor>=angle_interval && target_armor(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(radius / 2); - hit_point.y = cycle_center.y + static_cast(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(radius / 2); - hit_point.y = cycle_center.y + static_cast(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(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(radius / 2); - hit_point.y = cycle_center.y - static_cast(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: "< 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(radius / 2); +// hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); +// hit_position = -1*angle_interval; +// position_mode = 1; +// } +// else if(target_armor>=angle_interval && target_armor(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(radius / 2); +// hit_point.y = cycle_center.y - static_cast(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(radius / 2); +// hit_point.y = cycle_center.y - static_cast(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(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(radius / 2); +// hit_point.y = cycle_center.y + static_cast(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(radius / 2); +// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); +// hit_position = 2*angle_interval; +// position_mode = 1; +// } +// else if(target_armor>=angle_interval && target_armor(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(radius / 2); +// hit_point.y = cycle_center.y + static_cast(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(radius / 2); +// hit_point.y = cycle_center.y + static_cast(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(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(radius / 2); +// hit_point.y = cycle_center.y - static_cast(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: "< 330){ + last_target_armor = target_armor; + return false; + } + else{ + last_target_armor = target_armor; + return true; + } +} \ No newline at end of file diff --git a/energy/src/energy/param_init.cpp b/energy/src/energy/param_init.cpp index f4ff5c0..622e581 100644 --- a/energy/src/energy/param_init.cpp +++ b/energy/src/energy/param_init.cpp @@ -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; -} \ No newline at end of file +// +// 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'<200)all_armor_centers.clear(); -// if(first_armor_centers.size()>200)first_armor_centers.clear(); -// cout<<"first_armor_centers.size(): "<0)showFanContours("fan",src,fans); -// fans_cnt=0; - - armors_cnt = findArmor(src, armors, last_armors_cnt); -// cout<<"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(): "<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"<200)all_armor_centers.clear(); +// if(first_armor_centers.size()>200)first_armor_centers.clear(); +// cout<<"first_armor_centers.size(): "<0)showFanContours("fan",src,fans); +// fans_cnt=0; + + armors_cnt = findArmor(src, armors, last_armors_cnt); +// cout<<"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: "<0)hit_point = gimble_zero_points.at(0).rect.center; +*/ + + + + diff --git a/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp b/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp index e7a516f..4dd180a 100644 --- a/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp +++ b/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp @@ -2,20 +2,22 @@ // Created by xixiliadorabarry on 1/24/19. // #include "energy/energy.h" +#include +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(x * (32768 - 1) / 100); y_tmp = static_cast(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"<> 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"<init()){ @@ -141,8 +150,10 @@ int main(int argc, char *argv[]) { } if(save_video){ armor_video_writer.write(armor_src); +// cout< #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 diff --git a/others/include/options/additions.h b/others/include/options/additions.h deleted file mode 100644 index 5171790..0000000 --- a/others/include/options/additions.h +++ /dev/null @@ -1,16 +0,0 @@ -// -// Created by xinyang on 19-4-7. -// - -#ifndef _ADDITIONS_H_ -#define _ADDITIONS_H_ - -#include -#include -#include - -void save_video_file(cv::Mat &src); -void save_labelled_image(cv::Mat &src, cv::Rect2d box); - - -#endif /* _ADDITIONS_H_ */ diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 78bded9..40c0c8e 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -7,24 +7,32 @@ #include #include #include +#include -#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"; } diff --git a/others/src/camera/camera_wrapper.cpp b/others/src/camera/camera_wrapper.cpp index 8fa3ac9..3e42a28 100644 --- a/others/src/camera/camera_wrapper.cpp +++ b/others/src/camera/camera_wrapper.cpp @@ -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); diff --git a/others/src/options/additions.cpp b/others/src/options/additions.cpp deleted file mode 100644 index 71cdc31..0000000 --- a/others/src/options/additions.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// -// Created by xinyang on 19-4-7. -// - -#include -#include -#include -#include -#include -#include -#include -#include - -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); - } -} diff --git a/others/src/serial/serial.cpp b/others/src/serial/serial.cpp index 088fb42..b292f97 100644 --- a/others/src/serial/serial.cpp +++ b/others/src/serial/serial.cpp @@ -2,7 +2,7 @@ #include #include -#define LOG_LEVEL LOG_NONE +//#define LOG_LEVEL LOG_NONE #include using namespace std; diff --git a/tools/TrainCNN/backward.py b/tools/TrainCNN/backward.py old mode 100755 new mode 100644 index 87fbdc0..797defc --- a/tools/TrainCNN/backward.py +++ b/tools/TrainCNN/backward.py @@ -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) diff --git a/tools/TrainCNN/forward.py b/tools/TrainCNN/forward.py index 8d8073b..6200957 100644 --- a/tools/TrainCNN/forward.py +++ b/tools/TrainCNN/forward.py @@ -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]) diff --git a/tools/TrainCNN/generate.py b/tools/TrainCNN/generate.py index d2da420..20c2a63 100644 --- a/tools/TrainCNN/generate.py +++ b/tools/TrainCNN/generate.py @@ -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) diff --git a/tools/analysis.py b/tools/analysis.py old mode 100755 new mode 100644 diff --git a/tools/bind-monitor.sh b/tools/bind-monitor.sh old mode 100755 new mode 100644 index 0e09f2b..1a7c3c1 --- a/tools/bind-monitor.sh +++ b/tools/bind-monitor.sh @@ -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 diff --git a/tools/monitor.sh b/tools/monitor.sh old mode 100755 new mode 100644 index 02381f8..e46f816 --- a/tools/monitor.sh +++ b/tools/monitor.sh @@ -10,5 +10,5 @@ while true; do exec $exe & echo "restart $exe" fi - sleep 2 + sleep 0.5 done