diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index 35fd77c..97b2721 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -28,14 +28,14 @@ void Energy::StructingElementErodeDilate(cv::Mat &src) { // imshow("dilate_1", src); erode(src,src, element_erode_1); - imshow("erode_1", src); +// imshow("erode_1", src); dilate(src, src, element_dilate_2); - imshow("dilate_2", src); +// imshow("dilate_2", src); erode(src,src, element_erode_2); - imshow("erode_2", src); +// imshow("erode_2", src); dilate(src, src, element_dilate_3); - imshow("dilate_3", src); +// imshow("dilate_3", src); } diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 04cca3e..4fcf680 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -19,7 +19,8 @@ int Energy::findFan(const cv::Mat &src, vector &fans, int &last_fans std::vector > fan_contours; StructingElementClose(src_bin,6,6); - imshow("fan struct",src_bin); + +// imshow("fan struct",src_bin); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); @@ -72,7 +73,7 @@ int Energy::findArmor(const cv::Mat &src, vector &armors, int &last_ StructingElementErodeDilate(src_bin); // StructingElementClose(src_bin,10,10); - imshow("armor struct",src_bin); +// imshow("armor struct",src_bin); findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index 3d69210..c9b3377 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -16,38 +16,38 @@ void Energy::gimbleRotation(){ cv::Point2f real_hit_point; stretch(hit_point, real_hit_point); + float origin_yaw = -0.13; + float origin_pitch = 13.18; + + /*origin_yaw = mark_yaw; + origin_pitch = mark_pitch;*/ if(position_mode == 1){ - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(mark_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); } else if(position_mode == 2){ - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(mark_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); } else if(position_mode == 3){ - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(mark_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); } else if(position_mode == 4){ - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(mark_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); } else if(position_mode == 5){ - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(mark_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); } else if(position_mode == 6){ - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(mark_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); } // else{ // pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); // } - -// yaw_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE)); -// pitch_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(9.1*PI/180)-STRETCH*(hit_point.y-cycle_center.y)), ATTACK_DISTANCE)); -// cout<<"cur: "<200)all_armor_centers.clear(); // if(first_armor_centers.size()>200)first_armor_centers.clear(); @@ -30,7 +30,7 @@ int Energy::run(cv::Mat &src){ // imshow("img_preprocess",src); threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); - imshow("bin",src); +// imshow("bin",src); fans_cnt = findFan(src, fans, last_fans_cnt); @@ -54,7 +54,10 @@ int Energy::run(cv::Mat &src){ // cycle_center = cv::Point(335, 246); // radius = 116.936; - attack_distance = ATTACK_DISTANCE * 120/ radius; +// attack_distance = ATTACK_DISTANCE * 120/ radius; + + attack_distance = 794 + 1245 * 75 * (1/radius - 1/113.9); +// cout << "attack distance: " << attack_distance << endl; getFanPosition(fanPosition, fans, cycle_center, radius); getArmorPosition(armorPosition, armors, cycle_center, radius); diff --git a/main.cpp b/main.cpp index 97b9fb5..dbb148a 100644 --- a/main.cpp +++ b/main.cpp @@ -25,7 +25,7 @@ using namespace cv; using namespace std; -int state = ENERGY_STATE; +int state = ARMOR_STATE; float curr_yaw = 0, curr_pitch = 0; float mark_yaw = 0, mark_pitch = 0; int mark = 0; @@ -118,7 +118,7 @@ int main(int argc, char *argv[]) { return 0; } -#define RECEIVE_LOG_LEVEL LOG_NOTHING +#define RECEIVE_LOG_LEVEL LOG_MSG char uartReadByte(Serial &uart) { while (uart.GetBytesInCOM() == 0); @@ -138,45 +138,45 @@ void uartReceive(Serial* uart) { while ((data = uartReadByte(*uart)) != '\n') { buffer[cnt++] = data; if (cnt >= 100) { - LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); + //LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); cnt = 0; } } - LOGM("%d", cnt); + //LOGM("%d", cnt); if (cnt == 12) { if (buffer[8] == 'e') { state = ENERGY_STATE; - LOG(RECEIVE_LOG_LEVEL, "Energy state"); + //LOG(RECEIVE_LOG_LEVEL, "Energy state"); } else if (buffer[8] == 'a') { state = ARMOR_STATE; - LOG(RECEIVE_LOG_LEVEL, "Armor state"); + //LOG(RECEIVE_LOG_LEVEL, "Armor state"); } if (buffer[10] == 0){ use_classifier = false; - LOG(RECEIVE_LOG_LEVEL, "Classifier off!"); + //LOG(RECEIVE_LOG_LEVEL, "Classifier off!"); } else if(buffer[10] == 1){ use_classifier = true; - LOG(RECEIVE_LOG_LEVEL, "Classifier on!"); + //LOG(RECEIVE_LOG_LEVEL, "Classifier on!"); } if (buffer[11] == ENEMY_BLUE) { - LOG(RECEIVE_LOG_LEVEL, "ENEMY_BLUE!"); + //LOG(RECEIVE_LOG_LEVEL, "ENEMY_BLUE!"); ally_color = ALLY_RED; enemy_color = ENEMY_BLUE; } else if (buffer[11] == ENEMY_RED) { - LOG(RECEIVE_LOG_LEVEL, "ENEMY_RED!"); + //LOG(RECEIVE_LOG_LEVEL, "ENEMY_RED!"); ally_color = ALLY_BLUE; enemy_color = ENEMY_RED; } memcpy(&curr_yaw, buffer, 4); memcpy(&curr_pitch, buffer + 4, 4); - LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); + //LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); if (buffer[9] == 1) { if (mark == 0) { mark = 1; mark_yaw = curr_yaw; mark_pitch = curr_pitch; } - LOG(RECEIVE_LOG_LEVEL, "Marked"); + //LOG(RECEIVE_LOG_LEVEL, "Marked"); } } cnt = 0; diff --git a/others/energy.Config b/others/energy.Config index 327ec1d..a4e571f 100644 --- a/others/energy.Config +++ b/others/energy.Config @@ -1,4 +1,4 @@ -create_time = "2019-05-14 19:59:50"; +create_time = "2019-05-14 20:21:31"; internal_id = "E7FCFFE8999B"; resolution : { diff --git a/others/src/camera/camera_wrapper.cpp b/others/src/camera/camera_wrapper.cpp index 8ed2052..2cfefab 100644 --- a/others/src/camera/camera_wrapper.cpp +++ b/others/src/camera/camera_wrapper.cpp @@ -72,6 +72,9 @@ bool CameraWrapper::init() { return false; } LOGM("successfully loaded %s!", filepath); + double t; + CameraGetExposureTime(h_camera, &t); + LOGM("Exposure time: %lfms", t / 1000.0); /* if(mode == 0){ // 不使用自动曝光 CameraSetAeState(h_camera, false);