From 68a642e6a652d53c7b7b7c1b50265ae9f163f971 Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 22 Jul 2019 11:40:09 +0800 Subject: [PATCH] energy change --- energy/include/energy/energy.h | 3 +- energy/src/energy/judge/judge_shoot.cpp | 2 +- energy/src/energy/run.cpp | 105 +++++++----------------- energy/src/energy/send/send.cpp | 47 +++++++++-- main.cpp | 36 ++++++-- others/include/camera/camera_wrapper.h | 5 +- others/src/additions/additions.cpp | 9 +- others/src/camera/camera_wrapper.cpp | 9 +- 8 files changed, 113 insertions(+), 103 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 2f07dfc..5a3c6ec 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -35,7 +35,8 @@ public: void setEnergyInit();//设置能量机关初始化 void setBigEnergyInit();//设置大能量机关初始化 void setSmallEnergyInit();//设置小能量机关初始化 - void sendTarget(Serial &serial, float x, float y, float z); + void sendTarget(Serial& serial, float x, float y, float z);//发送数据 + void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据 private: diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index 149974f..5edd7c2 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -36,7 +36,7 @@ void Energy::judgeShootInGimbal(){ is_guessing = true; start_guess = true; gettimeofday(&time_start_guess, NULL); - LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); +// LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); } else shoot = 2; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 1568760..10a5a2d 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -31,7 +31,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (!findCenterR(gimbal_src))return; if (show_energy)showCenterR("R", gimbal_src); -// if (!getOrigin())return; + if (!getOrigin())return; startChassis(); initEnergy(); destroyAllWindows(); @@ -46,35 +46,22 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (findArmors(chassis_src) < 1)return; if (show_energy)showArmors("armor", chassis_src); if (!findFlowStripFan(chassis_src))return; + showFlowStripFan("flow strip fan", chassis_src); if (!findTargetInFlowStripFan()) return; if (!findCenterROI(chassis_src))return; if (show_energy)showFlowStripFan("strip", chassis_src); if (!findCenterR(chassis_src))return; if (show_energy)showCenterR("R", chassis_src); getTargetPolarAngle(); - changeTarget(); -// judgeMode(); -// if (energy_mode_init)return; if (is_big && energy_rotation_init) { initRotation(); return; } -// if (is_predicting) { -// getPredictPoint(target_point); -// gimbalRotation(); -// judgeShootInWorld(); -// sendTarget(serial, yaw_rotation, pitch_rotation, shoot); -// } else if (is_guessing && stayGuessing()) { -// findFans(chassis_src); -// if (show_energy)showFans("fans", chassis_src); -// if (save_mark)writeDownMark(); -// if (!guessTarget()) return; -// if (show_energy)showGuessTarget("guess", chassis_src); -// getPredictPoint(guess_point); -// gimbalRotation(); -// sendTarget(serial, yaw_rotation, pitch_rotation, 5); -// } + getPredictPoint(target_point); + gimbalRotation(); + judgeShootInWorld(); + sendTarget(serial, yaw_rotation, pitch_rotation, change_target); } } @@ -101,35 +88,15 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showCenterR("R", gimbal_src); changeTarget(); getTargetPolarAngle(); -// cout< -#include #include "log.h" using namespace std; - - //---------------------------------------------------------------------------------------------------------------------- // 此函数用于发送数据给主控板 // --------------------------------------------------------------------------------------------------------------------- @@ -39,10 +36,48 @@ void Energy::sendTarget(Serial& serial, float x, float y, float z){ buff[4] = static_cast((y_tmp >> 0) & 0xFF); buff[5] = static_cast((z_tmp >> 8) & 0xFF); buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[7] = 0; - buff[8] = 0; buff[9] = 'e'; serial.WriteData(buff, sizeof(buff)); send_cnt+=1; // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); -} \ No newline at end of file +} + + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于发送数据给主控板 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::sendTarget(Serial& serial, float x, float y, float z, uint16_t u){ + short x_tmp, y_tmp, z_tmp; + uint8_t buff[10]; + +#ifdef WITH_COUNT_FPS + static auto last_time = time(nullptr); + static int fps = 0; + time_t t = time(nullptr); + if (last_time != t) { + last_time = t; + cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; + fps = 0; + } + fps += 1; +#endif + + x_tmp = static_cast(x * (32768 - 1) / 100); + y_tmp = static_cast(y * (32768 - 1) / 100); + z_tmp = static_cast(z * (32768 - 1) / 100); + buff[0] = 's'; + buff[1] = static_cast((x_tmp >> 8) & 0xFF); + buff[2] = static_cast((x_tmp >> 0) & 0xFF); + buff[3] = static_cast((y_tmp >> 8) & 0xFF); + buff[4] = static_cast((y_tmp >> 0) & 0xFF); + buff[5] = static_cast((z_tmp >> 8) & 0xFF); + buff[6] = static_cast((z_tmp >> 0) & 0xFF); + buff[7] = static_cast((u >> 8) & 0xFF); + buff[8] = static_cast((u >> 0) & 0xFF);; + buff[9] = 'e'; + serial.WriteData(buff, sizeof(buff)); + send_cnt+=1; +// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); +} diff --git a/main.cpp b/main.cpp index 7dace8c..fe1b3f4 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -61,8 +61,8 @@ int main(int argc, char *argv[]) { while (true) { // 打开视频源 if (from_camera) { - video_gimbal = new CameraWrapper(0/*, "armor"*/); - video_chassis = new CameraWrapper(1/*, "energy"*/); + video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); + video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "energy"*/); } else { video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); @@ -95,7 +95,13 @@ int main(int argc, char *argv[]) { if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 destroyAllWindows(); - ((CameraWrapper *) video_gimbal)->changeBrightness(ENERGY_CAMERA_GAIN); + delete video_gimbal; + video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } energy.setBigEnergyInit(); checkReconnect(video_chassis->read(chassis_src)); #ifdef CHASSIS_FLIP_MODE @@ -110,13 +116,19 @@ int main(int argc, char *argv[]) { if (!from_camera) extract(gimbal_src, chassis_src); if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 -// energy.runBig(gimbal_src, chassis_src); - energy.runBig(gimbal_src); + energy.runBig(gimbal_src, chassis_src); +// energy.runBig(gimbal_src); last_state = mcuData.state;//更新上一帧状态 } else if (mcuData.state == SMALL_ENERGY_STATE) { if (mcuData.state != SMALL_ENERGY_STATE) { destroyAllWindows(); - ((CameraWrapper *) video_gimbal)->changeBrightness(ENERGY_CAMERA_GAIN); + delete video_gimbal; + video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } energy.setSmallEnergyInit(); } ok = checkReconnect(video_gimbal->read(gimbal_src)); @@ -131,7 +143,13 @@ int main(int argc, char *argv[]) { } else { // 自瞄模式 if (last_state != ARMOR_STATE) { destroyAllWindows(); - ((CameraWrapper *) video_gimbal)->changeBrightness(ARMOR_CAMERA_GAIN); + delete video_gimbal; + video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } } last_state = mcuData.state; ok = checkReconnect(video_gimbal->read(gimbal_src)); @@ -139,7 +157,7 @@ int main(int argc, char *argv[]) { flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); #endif if (!from_camera) extract(gimbal_src); - if (save_video) saveVideos(gimbal_src); +// if (save_video) saveVideos(gimbal_src); if (show_origin) showOrigin(gimbal_src); CNT_TIME("Armor Time", { armorFinder.run(gimbal_src); diff --git a/others/include/camera/camera_wrapper.h b/others/include/camera/camera_wrapper.h index af8c2c5..4716120 100644 --- a/others/include/camera/camera_wrapper.h +++ b/others/include/camera/camera_wrapper.h @@ -38,9 +38,10 @@ private: IplImage* iplImage; int channel; - public: - CameraWrapper(int camera_mode=1, const std::string &n="NULL"); + int gain; + + CameraWrapper(int gain, int camera_mode=1, const std::string &n="NULL"); ~CameraWrapper() final; bool init() final; diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 19dd316..e8373ec 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -75,13 +75,15 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix) { bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) { if (!is_camera_0_connect) { + int curr_gain = ((CameraWrapper* )video_gimbal)->gain; delete video_gimbal; - video_gimbal = new CameraWrapper(0/*, "armor"*/); + video_gimbal = new CameraWrapper(curr_gain, 0/*, "armor"*/); is_camera_0_connect = video_gimbal->init(); } if (!is_camera_1_connect) { + int curr_gain = ((CameraWrapper* )video_gimbal)->gain; delete video_chassis; - video_chassis = new CameraWrapper(1/*, "energy"*/); + video_chassis = new CameraWrapper(curr_gain, 0/*, "energy"*/); is_camera_1_connect = video_chassis->init(); } return is_camera_0_connect && is_camera_1_connect; @@ -89,8 +91,9 @@ bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) { bool checkReconnect(bool is_camera_connect) { if (!is_camera_connect) { + int curr_gain = ((CameraWrapper* )video_gimbal)->gain; delete video_gimbal; - video_gimbal = new CameraWrapper(0/*, "armor"*/); + video_gimbal = new CameraWrapper(curr_gain, 0/*, "armor"*/); is_camera_connect = video_gimbal->init(); } return is_camera_connect; diff --git a/others/src/camera/camera_wrapper.cpp b/others/src/camera/camera_wrapper.cpp index f06ee78..4fd7f38 100644 --- a/others/src/camera/camera_wrapper.cpp +++ b/others/src/camera/camera_wrapper.cpp @@ -13,14 +13,15 @@ using std::cout; using std::endl; using namespace cv; -CameraWrapper::CameraWrapper(int camera_mode, const std::string &n) : +CameraWrapper::CameraWrapper(int gain, int camera_mode, const std::string &n) : name(n), mode(camera_mode), camera_cnts(2), camera_status(-1), iplImage(nullptr), rgb_buffer(nullptr), - channel(3) { + channel(3), + gain(gain){ } @@ -78,7 +79,7 @@ bool CameraWrapper::init() { CameraSetAeState(h_camera, false); CameraSetExposureTime(h_camera, CAMERA_EXPOSURE * 1000); #ifndef WITH_TIME_BASED_CAMERA_GAIN - CameraSetAnalogGain(h_camera, ARMOR_CAMERA_GAIN); + CameraSetAnalogGain(h_camera, gain); #else #include @@ -133,6 +134,7 @@ bool CameraWrapper::init() { } bool CameraWrapper::changeBrightness(int brightness) { + CameraUnInit(h_camera); CameraSetAnalogGain(h_camera, brightness); } @@ -191,4 +193,3 @@ CameraWrapper::~CameraWrapper() { if (rgb_buffer != nullptr) free(rgb_buffer); } -