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 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"));
}