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

energy change

Browse Source
This commit is contained in:
sun
2019-07-22 11:40:09 +08:00
parent 39c3b3e3c2
commit 68a642e6a6
8 changed files with 113 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
energy/include/energy/energy.h
View File

@@ -35,7 +35,8 @@ public:
void setEnergyInit();//设置能量机关初始化 void setEnergyInit();//设置能量机关初始化
void setBigEnergyInit();//设置大能量机关初始化 void setBigEnergyInit();//设置大能量机关初始化
void setSmallEnergyInit();//设置小能量机关初始化 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: private:

2
energy/src/energy/judge/judge_shoot.cpp
View File

@@ -36,7 +36,7 @@ void Energy::judgeShootInGimbal(){
is_guessing = true; is_guessing = true;
start_guess = true; start_guess = true;
gettimeofday(&time_start_guess, NULL); gettimeofday(&time_start_guess, NULL);
LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); // LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else } else
shoot = 2; shoot = 2;

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

@@ -31,7 +31,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (!findCenterR(gimbal_src))return; if (!findCenterR(gimbal_src))return;
if (show_energy)showCenterR("R", gimbal_src); if (show_energy)showCenterR("R", gimbal_src);
// if (!getOrigin())return; if (!getOrigin())return;
startChassis(); startChassis();
initEnergy(); initEnergy();
destroyAllWindows(); destroyAllWindows();
@@ -46,35 +46,22 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (findArmors(chassis_src) < 1)return; if (findArmors(chassis_src) < 1)return;
if (show_energy)showArmors("armor", chassis_src); if (show_energy)showArmors("armor", chassis_src);
if (!findFlowStripFan(chassis_src))return; if (!findFlowStripFan(chassis_src))return;
showFlowStripFan("flow strip fan", chassis_src);
if (!findTargetInFlowStripFan()) return; if (!findTargetInFlowStripFan()) return;
if (!findCenterROI(chassis_src))return; if (!findCenterROI(chassis_src))return;
if (show_energy)showFlowStripFan("strip", chassis_src); if (show_energy)showFlowStripFan("strip", chassis_src);
if (!findCenterR(chassis_src))return; if (!findCenterR(chassis_src))return;
if (show_energy)showCenterR("R", chassis_src); if (show_energy)showCenterR("R", chassis_src);
getTargetPolarAngle(); getTargetPolarAngle();
changeTarget(); changeTarget();
// judgeMode();
// if (energy_mode_init)return;
if (is_big && energy_rotation_init) { if (is_big && energy_rotation_init) {
initRotation(); initRotation();
return; return;
} }
// if (is_predicting) { getPredictPoint(target_point);
// getPredictPoint(target_point); gimbalRotation();
// gimbalRotation(); judgeShootInWorld();
// judgeShootInWorld(); sendTarget(serial, yaw_rotation, pitch_rotation, change_target);
// 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);
// }
} }
} }
@@ -101,35 +88,15 @@ void Energy::runBig(cv::Mat &gimbal_src) {
if (show_energy)showCenterR("R", gimbal_src); if (show_energy)showCenterR("R", gimbal_src);
changeTarget(); changeTarget();
getTargetPolarAngle(); getTargetPolarAngle();
// cout<<circle_center_point<<endl;
// cout << target_point << '\t' << target_polar_angle << endl;
// cout << circle_center_point<< endl;
// judgeMode();
// if (energy_mode_init)return;
// if (!getOrigin())return; // if (!getOrigin())return;
if (energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
return; return;
} }
if (is_predicting) {
getPredictPoint(target_point); getPredictPoint(target_point);
getAimPoint(predict_point); getAimPoint(predict_point);
// cout << yaw_rotation << '\t' << pitch_rotation << endl;
judgeShootInGimbal(); judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot); sendTarget(serial, yaw_rotation, pitch_rotation, change_target);
} else if (is_guessing && stayGuessing()) {
findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
if (save_mark)writeDownMark();
guessTarget();
if (show_energy)showGuessTarget("guess", gimbal_src);
getPredictPoint(guess_point);
getAimPoint(predict_point);
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
}
} }
@@ -146,48 +113,32 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (show_energy)showArmors("armor", gimbal_src); if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return; if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return; if (!findTargetInFlowStripFan()) return;
//
// if (!findCenterROI(gimbal_src))return; // if (!findCenterROI(gimbal_src))return;
// if (show_energy)showFlowStripFan("strip", gimbal_src); // if (show_energy)showFlowStripFan("strip", gimbal_src);
// if (!findCenterR(gimbal_src))return; // if (!findCenterR(gimbal_src))return;
// if (show_energy)showCenterR("R", gimbal_src); // if (show_energy)showCenterR("R", gimbal_src);
changeTarget(); changeTarget();
// cout << "target point: " << target_point << endl;
if (is_predicting) {
getAimPoint(target_point); getAimPoint(target_point);
judgeShootInGimbal(); judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, shoot); sendTarget(serial, yaw_rotation, pitch_rotation, change_target);
} else if (is_guessing && stayGuessing()) {
findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
if (save_mark)writeDownMark();
guessTarget();
if (show_energy)showGuessTarget("guess", gimbal_src);
getAimPoint(guess_point);
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
}
} }
//if (is_predicting) {
//getPredictPoint(target_point); //getPredictPoint(target_point);
//gimbalRotation(); //getAimPoint(predict_point);
//static bool k = false; //cout << yaw_rotation << '\t' << pitch_rotation << endl;
//cout<<"delta yaw: "<<abs(yaw_rotation - mcuData.curr_yaw)<<endl; //judgeShootInGimbal();
//cout<<"delta pitch: "<<abs(pitch_rotation - mcuData.curr_pitch)<<endl;
//cout << "origin_yaw: " << origin_yaw << '\t' << "origin_pitch: " << origin_pitch << endl;
//cout << "predict point: " << predict_point << endl;
//
//if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && abs(pitch_rotation - mcuData.curr_pitch) < 0.6) {
//shoot = 4;
//if (!k) {
//sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
//cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
//k = false;
//}
//waitKey(400);
//} else {
//shoot = 2;
//sendTarget(serial, yaw_rotation, pitch_rotation, shoot); //sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
//} else if (is_guessing && stayGuessing()) {
//findFans(gimbal_src);
//if (show_energy)showFans("fans", gimbal_src);
//if (save_mark)writeDownMark();
//guessTarget();
//if (show_energy)showGuessTarget("guess", gimbal_src);
//getPredictPoint(guess_point);
//getAimPoint(predict_point);
//sendTarget(serial, yaw_rotation, pitch_rotation, 5);
//} //}

45
energy/src/energy/send/send.cpp
View File

@@ -3,13 +3,10 @@
// //
#include "energy/energy.h" #include "energy/energy.h"
#include <iostream> #include <iostream>
#include <config/setconfig.h>
#include "log.h" #include "log.h"
using namespace std; using namespace std;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板 // 此函数用于发送数据给主控板
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -39,8 +36,46 @@ void Energy::sendTarget(Serial& serial, float x, float y, float z){
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF); buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF); buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 0; buff[9] = 'e';
buff[8] = 0; serial.WriteData(buff, sizeof(buff));
send_cnt+=1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板
// ---------------------------------------------------------------------------------------------------------------------
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<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 100);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = static_cast<char>((u >> 8) & 0xFF);
buff[8] = static_cast<char>((u >> 0) & 0xFF);;
buff[9] = 'e'; buff[9] = 'e';
serial.WriteData(buff, sizeof(buff)); serial.WriteData(buff, sizeof(buff));
send_cnt+=1; send_cnt+=1;

36
main.cpp
View File

@@ -30,7 +30,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体 mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角 0, // 当前云台yaw角
0, // 当前云台pitch角 0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符 BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位 0, // 云台角度标记位
1, // 是否启用数字识别 1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色 ENEMY_RED, // 敌方颜色
@@ -61,8 +61,8 @@ int main(int argc, char *argv[]) {
while (true) { while (true) {
// 打开视频源 // 打开视频源
if (from_camera) { if (from_camera) {
video_gimbal = new CameraWrapper(0/*, "armor"*/); video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/);
video_chassis = new CameraWrapper(1/*, "energy"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "energy"*/);
} else { } else {
video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi");
video_chassis = 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 (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
destroyAllWindows(); 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(); energy.setBigEnergyInit();
checkReconnect(video_chassis->read(chassis_src)); checkReconnect(video_chassis->read(chassis_src));
#ifdef CHASSIS_FLIP_MODE #ifdef CHASSIS_FLIP_MODE
@@ -110,13 +116,19 @@ int main(int argc, char *argv[]) {
if (!from_camera) extract(gimbal_src, chassis_src); if (!from_camera) extract(gimbal_src, chassis_src);
if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
// energy.runBig(gimbal_src, chassis_src); energy.runBig(gimbal_src, chassis_src);
energy.runBig(gimbal_src); // energy.runBig(gimbal_src);
last_state = mcuData.state;//更新上一帧状态 last_state = mcuData.state;//更新上一帧状态
} else if (mcuData.state == SMALL_ENERGY_STATE) { } else if (mcuData.state == SMALL_ENERGY_STATE) {
if (mcuData.state != SMALL_ENERGY_STATE) { if (mcuData.state != SMALL_ENERGY_STATE) {
destroyAllWindows(); 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(); energy.setSmallEnergyInit();
} }
ok = checkReconnect(video_gimbal->read(gimbal_src)); ok = checkReconnect(video_gimbal->read(gimbal_src));
@@ -131,7 +143,13 @@ int main(int argc, char *argv[]) {
} else { // 自瞄模式 } else { // 自瞄模式
if (last_state != ARMOR_STATE) { if (last_state != ARMOR_STATE) {
destroyAllWindows(); 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; last_state = mcuData.state;
ok = checkReconnect(video_gimbal->read(gimbal_src)); 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); flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
#endif #endif
if (!from_camera) extract(gimbal_src); if (!from_camera) extract(gimbal_src);
if (save_video) saveVideos(gimbal_src); // if (save_video) saveVideos(gimbal_src);
if (show_origin) showOrigin(gimbal_src); if (show_origin) showOrigin(gimbal_src);
CNT_TIME("Armor Time", { CNT_TIME("Armor Time", {
armorFinder.run(gimbal_src); armorFinder.run(gimbal_src);

5
others/include/camera/camera_wrapper.h
View File

@@ -38,9 +38,10 @@ private:
IplImage* iplImage; IplImage* iplImage;
int channel; int channel;
public: 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; ~CameraWrapper() final;
bool init() final; bool init() final;

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

@@ -75,13 +75,15 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix) {
bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) { bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) {
if (!is_camera_0_connect) { if (!is_camera_0_connect) {
int curr_gain = ((CameraWrapper* )video_gimbal)->gain;
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(0/*, "armor"*/); video_gimbal = new CameraWrapper(curr_gain, 0/*, "armor"*/);
is_camera_0_connect = video_gimbal->init(); is_camera_0_connect = video_gimbal->init();
} }
if (!is_camera_1_connect) { if (!is_camera_1_connect) {
int curr_gain = ((CameraWrapper* )video_gimbal)->gain;
delete video_chassis; delete video_chassis;
video_chassis = new CameraWrapper(1/*, "energy"*/); video_chassis = new CameraWrapper(curr_gain, 0/*, "energy"*/);
is_camera_1_connect = video_chassis->init(); is_camera_1_connect = video_chassis->init();
} }
return is_camera_0_connect && is_camera_1_connect; 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) { bool checkReconnect(bool is_camera_connect) {
if (!is_camera_connect) { if (!is_camera_connect) {
int curr_gain = ((CameraWrapper* )video_gimbal)->gain;
delete video_gimbal; delete video_gimbal;
video_gimbal = new CameraWrapper(0/*, "armor"*/); video_gimbal = new CameraWrapper(curr_gain, 0/*, "armor"*/);
is_camera_connect = video_gimbal->init(); is_camera_connect = video_gimbal->init();
} }
return is_camera_connect; return is_camera_connect;

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

@@ -13,14 +13,15 @@ using std::cout;
using std::endl; using std::endl;
using namespace cv; 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), name(n),
mode(camera_mode), mode(camera_mode),
camera_cnts(2), camera_cnts(2),
camera_status(-1), camera_status(-1),
iplImage(nullptr), iplImage(nullptr),
rgb_buffer(nullptr), rgb_buffer(nullptr),
channel(3) { channel(3),
gain(gain){
} }
@@ -78,7 +79,7 @@ bool CameraWrapper::init() {
CameraSetAeState(h_camera, false); CameraSetAeState(h_camera, false);
CameraSetExposureTime(h_camera, CAMERA_EXPOSURE * 1000); CameraSetExposureTime(h_camera, CAMERA_EXPOSURE * 1000);
#ifndef WITH_TIME_BASED_CAMERA_GAIN #ifndef WITH_TIME_BASED_CAMERA_GAIN
CameraSetAnalogGain(h_camera, ARMOR_CAMERA_GAIN); CameraSetAnalogGain(h_camera, gain);
#else #else
#include <sys/time.h> #include <sys/time.h>
@@ -133,6 +134,7 @@ bool CameraWrapper::init() {
} }
bool CameraWrapper::changeBrightness(int brightness) { bool CameraWrapper::changeBrightness(int brightness) {
CameraUnInit(h_camera);
CameraSetAnalogGain(h_camera, brightness); CameraSetAnalogGain(h_camera, brightness);
} }
@@ -191,4 +193,3 @@ CameraWrapper::~CameraWrapper() {
if (rgb_buffer != nullptr) if (rgb_buffer != nullptr)
free(rgb_buffer); free(rgb_buffer);
} }