diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 867a12c..85f7178 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -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 recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符 - - void initEnergy();//能量机关初始化 void initEnergyPartParam();//能量机关参数初始化 void initRotation();//顺逆时针初始化 diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h index 9e0fa7f..5b96400 100644 --- a/energy/include/energy/param_struct_define.h +++ b/energy/include/energy/param_struct_define.h @@ -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;//装甲板识别二值化阈值 diff --git a/energy/src/energy/clear/clear.cpp b/energy/src/energy/clear/clear.cpp index bd7d946..18c8b73 100644 --- a/energy/src/energy/clear/clear.cpp +++ b/energy/src/energy/clear/clear.cpp @@ -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); } diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index c518df5..cac5553 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -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; diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 60e7b2d..076fa18 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -53,6 +53,8 @@ void Energy::setBigEnergyInit() { initEnergy(); initEnergyPartParam(); + getsystime(time_start_energy); + is_big = true; is_small = false; is_gimbal = true; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index fd5d95b..9f414a2 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -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; diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 2a9ab2e..d03156a 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -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: "<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!")); diff --git a/tools/create-startup.sh b/tools/create-startup.sh index f8703d9..b7826dc 100755 --- a/tools/create-startup.sh +++ b/tools/create-startup.sh @@ -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