Auto Aim

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 
 project(auto-aim)
 set(CMAKE_CXX_STANDARD 11)
-SET(CMAKE_BUILD_TYPE RELEASE)
+SET(CMAKE_BUILD_TYPE DEBUG)
 
 FIND_PROGRAM(CCACHE_FOUND ccache)
 IF(CCACHE_FOUND)

energy/include/energy/constant.h

@@ -17,8 +17,8 @@ const int CLOCKWISE = 1;
 const int ANTICLOCKWISE = -1;
 const float ATTACK_DISTANCE = 770;//cm
 const double WHOLE_FAN = 80;//cm
-//const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
+const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
-const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
+//const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
 const int EXTRACT_POINT_X = 200;
 const int EXTRACT_POINT_Y = 20;
 const int EXTRACT_WIDTH = 240;
@@ -30,7 +30,7 @@ const int GM_X = 15;//云台摄像头x方向
 const int GM_H = 16;//云台摄像头y方向
 
 //const double STRETCH = 231.0/640.0;//实际距离与图像伸缩比
-const double STRETCH = 231.0/640.0;
+const double STRETCH = 0.34;
 
 const int ZERO_POINT_X = 281;
 const int ZERO_POINT_Y = 188;

energy/src/energy/get/gimble_rotation_get.cpp

@@ -9,6 +9,8 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
+
 
 void Energy::gimbleRotation(){
 //该方法用于标定激光零点的情况，对操作手友好，但建立在云台稳定情况下
@@ -16,11 +18,16 @@ void Energy::gimbleRotation(){
 //    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-STRETCH*(hit_point.y-ZERO_POINT_Y)), ATTACK_DISTANCE));
 
 //该方法用于操作手自己完成对心工作的情况，对操作手要求高
-    cv::Point2f real_hit_point;
+//    cv::Point2f real_hit_point;
-    stretch(hit_point, real_hit_point);
+//    stretch(hit_point, real_hit_point);
 //    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(YAW_ORIGIN_RAD)-real_hit_point.x), ATTACK_DISTANCE));
 //    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE));
 
-    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(aim.yaw)-real_hit_point.x), ATTACK_DISTANCE));
+//    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(curr_yaw)-real_hit_point.x), ATTACK_DISTANCE));
-    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(aim.pitch)-real_hit_point.y), ATTACK_DISTANCE));
+//    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(curr_pitch)-real_hit_point.y), ATTACK_DISTANCE));
+
+    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE));
+    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(9.1*PI/180)-STRETCH*(hit_point.y-cycle_center.y)), ATTACK_DISTANCE));
+    cout<<"cur: "<<curr_yaw<<endl;
+    cout<<"hit point: "<<hit_point<<'\t'<<"cycle center: "<<cycle_center<<endl;
 }

energy/src/energy/run.cpp

@@ -24,7 +24,7 @@ int Energy::run(cv::Mat &src){
     threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
 //    imshow("bin",src);
 
-
+/*
     fans_cnt = findFan(src, fans, last_fans_cnt);
 //    cout<<"fans_cnt: "<<fans_cnt<<endl;
     if(fans_cnt==-1) return 0;//滤去漏判的帧
@@ -39,14 +39,16 @@ int Energy::run(cv::Mat &src){
     if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
 
     if(armors_cnt != fans_cnt+1) return 0;
-
+*/
 
 //此处用于标定云台在摄像头视频中的零点
-//    findGimbleZeroPoint(src,gimble_zero_points);
+    findGimbleZeroPoint(src,gimble_zero_points);
-//    cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
+    cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
-//    showFanContours("zero",src,gimble_zero_points);
+    showFanContours("zero",src,gimble_zero_points);
-
+    cycle_center = cv::Point(291,305);
+    if(gimble_zero_points.size()>0)hit_point = gimble_zero_points.at(0).rect.center;
 
+/*
     getAllArmorCenters();
     cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
     cycleLeastFit();
@@ -62,6 +64,7 @@ int Energy::run(cv::Mat &src){
     getHitPoint();
     cout << "The hit point position is " << hit_point << endl;
 //    hit_point = cycle_center;
+*/
     gimbleRotation();
 
     sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);

main.cpp

@@ -13,6 +13,7 @@
 #include "camera/wrapper_head.h"
 #include "armor_finder/armor_finder.h"
 #include <options/options.h>
+//#define LOG_LEVEL LOG_WARRING
 #include <log.h>
 
 #include <thread>
@@ -23,7 +24,7 @@ using namespace std;
 #define ENERGY_STATE 1
 #define ARMOR_STATE 0
 
-int state = ARMOR_STATE;
+int state = ENERGY_STATE;
 float curr_yaw=0, curr_pitch=0;
 float mark_yaw=0, mark_pitch=0;
 
@@ -98,16 +99,16 @@ void uartReceive(Uart* uart){
         while((data=uart->receive()) != '\n'){
             buffer[cnt++] = data;
             if(cnt >= 100){
-                LOGE("data receive over flow!");
+//                LOGE("data receive over flow!");
             }
         }
         if(cnt == 10){
             if(buffer[8] == 'e'){
                 state = ENERGY_STATE;
-                LOGM("Energy state");
+//                LOGM("Energy state");
             }else if(buffer[8] == 'a'){
                 state = ARMOR_STATE;
-                LOGM("Armor state");
+//                LOGM("Armor state");
             }
             memcpy(&curr_yaw, buffer, 4);
             memcpy(&curr_pitch, buffer+4, 4);
@@ -115,7 +116,7 @@ void uartReceive(Uart* uart){
             if(buffer[9] == 1){
                 mark_yaw = curr_yaw;
                 mark_pitch = curr_pitch;
-                LOGM("Marked");
+//                LOGM("Marked");
             }
         }
         cnt = 0;