VSAG/amadeus_26_fb
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 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:

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

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

105
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 (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<<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 (energy_rotation_init) {
initRotation();
return;
}
if (is_predicting) {
getPredictPoint(target_point);
getAimPoint(predict_point);
// cout << yaw_rotation << '\t' << pitch_rotation << endl;
judgeShootInGimbal();
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);
}
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, change_target);
}
@@ -146,48 +113,32 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
//
// if (!findCenterROI(gimbal_src))return;
// if (show_energy)showFlowStripFan("strip", gimbal_src);
// if (!findCenterR(gimbal_src))return;
// if (show_energy)showCenterR("R", gimbal_src);
changeTarget();
// cout << "target point: " << target_point << endl;
if (is_predicting) {
getAimPoint(target_point);
judgeShootInGimbal();
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);
getAimPoint(guess_point);
sendTarget(serial, yaw_rotation, pitch_rotation, 5);
}
getAimPoint(target_point);
judgeShootInGimbal();
sendTarget(serial, yaw_rotation, pitch_rotation, change_target);
}
//if (is_predicting) {
//getPredictPoint(target_point);
//gimbalRotation();
//static bool k = false;
//cout<<"delta yaw: "<<abs(yaw_rotation - mcuData.curr_yaw)<<endl;
//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;
//getAimPoint(predict_point);
//cout << yaw_rotation << '\t' << pitch_rotation << endl;
//judgeShootInGimbal();
//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);
//}

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

@@ -3,13 +3,10 @@
//
#include "energy/energy.h"
#include <iostream>
#include <config/setconfig.h>
#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<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] = 0;
buff[8] = 0;
buff[9] = 'e';
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';
serial.WriteData(buff, sizeof(buff));
send_cnt+=1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
}

36
main.cpp
View File

@@ -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);

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

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

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) {
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;

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

@@ -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 <sys/time.h>
@@ -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);
}