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

energy changed(8/7game PID test)

Browse Source
This commit is contained in:
sun
2019-08-07 03:38:31 +08:00
parent 0257b87720
commit f93f5911ff
9 changed files with 45 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
energy/include/energy/energy.h
View File

@@ -52,6 +52,7 @@ private:
bool is_chassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
bool is_find_target;//判断当前是否找到了目标
bool energy_mode_init;//正在进行大小符判断
bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
bool manual_mark;//若操作手进行过手动标定，则为true
@@ -91,6 +92,7 @@ private:
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
systime time_start_guess;
systime time_start_energy;
cv::RotatedRect centerR;//风车中心字母R的可能候选区
cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
@@ -116,8 +118,6 @@ private:
std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
void initEnergy();//能量机关初始化
void initEnergyPartParam();//能量机关参数初始化
void initRotation();//顺逆时针初始化

1
energy/include/energy/param_struct_define.h
View File

@@ -17,6 +17,7 @@ using std::vector;
// ---------------------------------------------------------------------------------------------------------------------
struct EnergyPartParam {
int GRAY_THRESH;//二值化阈值
int SUB_GRAY_THRESH;//备用二值化阈值
int SPLIT_GRAY_THRESH;//通道分离二值化阈值
int FAN_GRAY_THRESH;//扇叶识别二值化阈值
int ARMOR_GRAY_THRESH;//装甲板识别二值化阈值

10
energy/src/energy/clear/clear.cpp
View File

@@ -26,8 +26,16 @@ void Energy::clearAll() {
void Energy::initImage(cv::Mat &src) {
// imagePreprocess(src);
// if(show_process)imshow("img_preprocess", src);
systime cur_time;
getsystime(cur_time);
float interval = getTimeIntervalms(cur_time, time_start_energy);
if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
if(interval > 3000 && !is_find_target){
threshold(src, src, energy_part_param_.SUB_GRAY_THRESH, 255, THRESH_BINARY);
}else{
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
}
if (show_process)imshow("bin", src);
if (show_energy)waitKey(1);
}

7
energy/src/energy/clear/energy_init.cpp
View File

@@ -17,6 +17,7 @@ void Energy::initEnergy() {
is_mark = false;
is_guessing = false;
is_predicting = true;
is_find_target = false;
energy_mode_init = true;
energy_rotation_init = true;
manual_mark = false;
@@ -71,7 +72,7 @@ void Energy::initEnergy() {
target_armors.clear();
flow_strips.clear();
all_target_armor_centers.clear();
while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
}
@@ -82,7 +83,8 @@ void Energy::initEnergy() {
void Energy::initEnergyPartParam() {
// gimbal_energy_part_param_.GRAY_THRESH = 120;//home
// gimbal_energy_part_param_.GRAY_THRESH = 200;//official
gimbal_energy_part_param_.GRAY_THRESH =180;//game
gimbal_energy_part_param_.GRAY_THRESH = 180;//game
gimbal_energy_part_param_.SUB_GRAY_THRESH = 100;
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180;
gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80;
@@ -157,7 +159,6 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
chassis_energy_part_param_.GRAY_THRESH = 120;//home
// chassis_energy_part_param_.GRAY_THRESH = 200;//official
// chassis_energy_part_param_.GRAY_THRESH = 225;

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

@@ -53,6 +53,8 @@ void Energy::setBigEnergyInit() {
initEnergy();
initEnergyPartParam();
getsystime(time_start_energy);
is_big = true;
is_small = false;
is_gimbal = true;

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

@@ -85,6 +85,7 @@ void Energy::runBig(cv::Mat &gimbal_src) {
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
}
is_find_target = true;
if (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStrip("strip", gimbal_src);
if (!findCenterR(gimbal_src))return;

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

@@ -24,20 +24,30 @@ void Energy::sendEnergy() {
} else if (ROBOT_ID == 7) {
float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -3, 3);
MINMAX(pitch_I_component, -3, 3);
// MINMAX(yaw_I_component, -2, 2);
// MINMAX(pitch_I_component, -2, 2);
// MINMAX(yaw_I_component, -3, 3);
// MINMAX(pitch_I_component, -3, 3);
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
// cout << BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
if(mcu_data.mark == 1){
yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
// cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
// cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
if (ROBOT_ID == 7) {

12
main.cpp
View File

@@ -30,7 +30,7 @@ using namespace std;
McuData mcu_data = { // 单片机端回传结构体
0, // 当前云台yaw角
0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符
SMALL_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位
1, // 是否启用数字识别
ENEMY_BLUE, // 敌方颜色
@@ -42,7 +42,7 @@ WrapperHead *video_gimbal = nullptr; // 云台摄像头视频源
WrapperHead *video_chassis = nullptr; // 底盘摄像头视频源
Serial serial(115200); // 串口对象
uint8_t last_state = INIT_STATE; // 上次状态，用于初始化
uint8_t last_state = INIT_STATE; // 上次状态，用于初始化y
// 自瞄主程序对象
ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/");
// 能量机关主程序对象
@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
} else {
video_gimbal = new VideoWrapper(PROJECT_DIR"/5.avi");
video_chassis = new VideoWrapper(PROJECT_DIR"/5.avi");
video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-small.avi");
video_chassis = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-small.avi");
}
if (video_gimbal->init()) {
LOGM("video_gimbal source initialization successfully.");
@@ -123,8 +123,8 @@ 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);
} else if (curr_state == SMALL_ENERGY_STATE) {
if (last_state != SMALL_ENERGY_STATE) {
LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!"));

2
tools/create-startup.sh
View File

@@ -5,5 +5,5 @@ echo "echo sjturm | sudo -S cpufreq-set -g performance" >> $2/startup-run
echo "mkdir $1/Mark" >> $2/startup-run
echo "mkdir $1/gimbal_video" >> $2/startup-run
echo "mkdir $1/armor_box_photo" >> $2/startup-run
echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --save-video --wait-uart --save-labelled-boxes --show-armor-box\\\"\"" >> $2/startup-run
echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --save-video --save-mark --wait-uart --save-labelled-boxes\\\"\"" >> $2/startup-run
chmod +x $2/startup-run