energy update

CMakeLists.txt

@@ -1,12 +1,11 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 3.5)
 
-PROJECT(AutoAim)
+PROJECT(RM-AutoAim)
 SET(CMAKE_CXX_STANDARD 11)
 SET(CMAKE_BUILD_TYPE RELEASE)
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"")
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")
-
 # Todo
 ## 使用编译期固定选项，以略微提升性能。
 #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFIX_OPTIONS")
@@ -22,28 +21,17 @@ IF(CCACHE_FOUND)
     MESSAGE("< Use ccache for compiler >")
 ENDIF()
 
-FIND_PACKAGE(Eigen3 REQUIRED)
 FIND_PACKAGE(OpenCV 3 REQUIRED)
 FIND_PACKAGE(Threads)
 
-INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
+INCLUDE_DIRECTORIES("D:/Program\ Files/eigen3.3.7")
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/energy/include)
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include)
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include)
 
 FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp")
-ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/additions/additions.h)
+ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} )
 
 TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
 TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS})
-IF (CMAKE_SYSTEM_NAME MATCHES "Linux")
+TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib")
-    MESSAGE(STATUS "current platform: Linux ")
-    TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libMVSDK.so")
-ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Windows")
-    MESSAGE(STATUS "current platform: Windows")
-    TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib")
-ELSE ()
-    MESSAGE(STATUS "Unsupport platform: ${CMAKE_SYSTEM_NAME}")
-ENDIF()
-
-ADD_CUSTOM_TARGET(bind-monitor COMMAND "${PROJECT_SOURCE_DIR}/tools/bind-monitor.sh" "${PROJECT_SOURCE_DIR}" "${CMAKE_BINARY_DIR}")

energy/include/energy/energy.h

@@ -31,6 +31,7 @@ public:
 
 //    void setAllyColor(int color);
     void setRotation(int rotation);
+	void setEnergyRotationInit();
 
 	void extract(cv::Mat &src);
 
@@ -42,6 +43,7 @@ private:
 	EnergyPartParam energy_part_param_;
 	LiftHeight lift_height_;
 	bool isSendTarget;
+	bool isMark;
 	int fans_cnt;
 	int armors_cnt;
 	int count;
@@ -61,13 +63,18 @@ private:
 	double phi;//电机yaw轴应旋转的角度
 	float yaw_rotation;
 	float pitch_rotation;
+	uint8_t last_mark;
 	int position_mode;
 	int last_position_mode;
     int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
+	bool energy_rotation_init;
+	int clockwise_rotation_init_cnt;
+	int anticlockwise_rotation_init_cnt;
+	float origin_yaw, origin_pitch;
 
 	std::vector<EnergyPart> fans;
 	std::vector<EnergyPart> armors;
-    std::vector<EnergyPart> gimble_zero_points;
+ //   std::vector<EnergyPart> gimble_zero_points;
 
 	cv::Point cycle_center;
 	cv::Point target_center;
@@ -81,9 +88,9 @@ private:
     cv::Point left, right, top, bottom;
 	cv::Mat src_blue, src_red, src_green;
     
-
+	void initEnergy();
-
 	void initEnergyPartParam();
+	void initRotation();
 
 	int findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans_cnt);
 	int findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_armors_cnt);
@@ -121,6 +128,7 @@ private:
 
     void getHitPoint();
 	bool changeTarget();
+	void changeMark();
     void gimbleRotation();
 
 	void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);

energy/src/energy/energy.cpp

@@ -8,51 +8,13 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+extern uint8_t last_state;
+
 Energy::Energy(Serial &u, uint8_t &enemy_color):serial(u),ally_color(enemy_color),
         src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
         src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
 {
-    isSendTarget = false;
+	initEnergy();
-
-	fans_cnt = 0;
-	armors_cnt = 0;
-	cycle_center = Point(0, 0);
-	target_center = Point(0, 0);
-	last_target_center = Point(0, 0);
-	hit_point = Point(0,0);
-	target_position = -1;
-	last_target_position = -1;
-    last_hit_position = 20000;
-	target_armor = -1;
-	radius = 0;
-
-//    ally_color = ALLY_RED;
-    energy_part_rotation = CLOCKWISE;
-    attack_distance = ATTACK_DISTANCE;
-	count = 1;
-	last_fans_cnt = 0;
-	last_armors_cnt = 0;
-	send_cnt = 0;
-
-    rectified_focal_length = 1000;
-	theta = 0;
-	phi = 0;
-	yaw_rotation = 0;
-	pitch_rotation = 0;
-
-	isLeftVertexFound = -1;
-	isTopVertexFound = -1;
-	isRightVertexFound = -1;
-	isBottomVertexFound = -1;
-
-	left = Point(640, 480);
-	right = Point(0, 0);
-	top = Point(640, 480);
-	bottom = Point(0, 0);
-
-	position_mode = 0;
-	last_position_mode = 0;
-
 	initEnergyPartParam();
 }
 
@@ -68,3 +30,8 @@ void Energy::setRotation(int rotation){
 	energy_part_rotation = rotation;
 }
 
+void Energy::setEnergyRotationInit() {
+	initEnergy();
+	initEnergyPartParam();
+	energy_rotation_init = true;
+}

energy/src/energy/get/gimble_rotation_get.cpp

@@ -9,16 +9,28 @@ using std::cout;
 using std::endl;
 using std::vector;
 
-extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
+void Energy::changeMark() {
+	if (mcuData.mark == 0 && last_mark == 1) {
+		last_mark = mcuData.mark;
+		origin_yaw = mcuData.curr_yaw;
+		origin_pitch = mcuData.curr_pitch;
+		isMark = true;
+	}
+	else if (mcuData.state == 1) {
+		last_mark = mcuData.mark;
+		isMark = true;
+	}
+	else {
+		last_mark = mcuData.mark;
+		isMark = false;
+	}
+}
 
 
 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;*/
    

energy/src/energy/get/hit_point_get.cpp

@@ -118,13 +118,13 @@ void Energy::getHitPoint(){
     }
 }
 
-bool Energy::changeTarget(){
+//bool Energy::changeTarget(){
-    if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){
+//    if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){
-        last_target_position = target_position;
+//        last_target_position = target_position;
-        return false;
+//        return false;
-    }
+//    }
-    else{
+//    else{
-        last_target_position = target_position;
+//        last_target_position = target_position;
-        return true;
+//        return true;
-    }
+//    }
-}
+//}

energy/src/energy/param_init.cpp

@@ -8,6 +8,66 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+void Energy::initEnergy() {
+	isSendTarget = false;
+	isMark = false;
+
+	fans_cnt = 0;
+	armors_cnt = 0;
+	cycle_center = Point(0, 0);
+	target_center = Point(0, 0);
+	last_target_center = Point(0, 0);
+	hit_point = Point(0, 0);
+	target_position = -1000;
+	last_target_position = -1000;
+	last_hit_position = 20000;
+	target_armor = -1000;
+	radius = 0;
+
+	//    ally_color = ALLY_RED;
+	energy_part_rotation = CLOCKWISE;
+	attack_distance = ATTACK_DISTANCE;
+	count = 1;
+	last_fans_cnt = 0;
+	last_armors_cnt = 0;
+	send_cnt = 0;
+
+	//rectified_focal_length = 1000;
+	//theta = 0;
+	//phi = 0;
+	yaw_rotation = 0;
+	pitch_rotation = 0;
+	last_mark = 0;
+	origin_yaw = -0.13;
+	origin_pitch = 13.18;
+
+	isLeftVertexFound = -1;
+	isTopVertexFound = -1;
+	isRightVertexFound = -1;
+	isBottomVertexFound = -1;
+
+	left = Point(640, 480);
+	right = Point(0, 0);
+	top = Point(640, 480);
+	bottom = Point(0, 0);
+
+	position_mode = 0;
+	last_position_mode = 0;
+
+	energy_rotation_init = false;
+
+	fans.clear();
+	armors.clear();
+	fanPosition.clear();
+	armorPosition.clear();
+	Armor_center.clear();
+	first_armor_centers.clear();
+	all_armor_centers.clear();
+
+	clockwise_rotation_init_cnt = 0;
+	anticlockwise_rotation_init_cnt = 0;
+}
+
 void Energy::initEnergyPartParam() {
 
     energy_part_param_.RPM = 10;
@@ -49,3 +109,25 @@ void Energy::initEnergyPartParam() {
 }
 
 
+void Energy::initRotation() {
+	target_position = target_armor;
+	cout << "target position: " << target_position << '\t' << "last target position: " << last_target_position << endl;
+	if (target_position >= -180 && last_target_position >= -180 && fabs(target_position - last_target_position) < 30) {
+		if (target_position < last_target_position) clockwise_rotation_init_cnt++;
+		else if (target_position > last_target_position) anticlockwise_rotation_init_cnt++;
+	}
+
+	if (clockwise_rotation_init_cnt == 5) {
+		energy_part_rotation = CLOCKWISE;
+		cout << "rotation: " << energy_part_rotation << endl;
+		energy_rotation_init = false;
+	}
+	else if (anticlockwise_rotation_init_cnt == 5) {
+		energy_part_rotation = ANTICLOCKWISE;
+		cout << "rotation: " << energy_part_rotation << endl;
+		energy_rotation_init = false;
+	}
+	//else cout << clockwise_rotation_init_cnt << endl;
+
+	last_target_position = target_position;
+}

energy/src/energy/run.cpp

@@ -18,10 +18,11 @@ int Energy::run(cv::Mat &src){
     armors.clear();
     fanPosition.clear();
     armorPosition.clear();
-    gimble_zero_points.clear();
+//    gimble_zero_points.clear();
     isSendTarget = false;
 
-    //if(mark==0)return 0;
+	changeMark();
+	if (isMark)return 0;
 
 //    if(all_armor_centers.size()>200)all_armor_centers.clear();
 //    if(first_armor_centers.size()>200)first_armor_centers.clear();
@@ -57,7 +58,7 @@ int Energy::run(cv::Mat &src){
 //    radius = 116.936;
 //    attack_distance = ATTACK_DISTANCE * 120/ radius;
 
-	attack_distance = 794 + 1245 * 75 * (1/radius - 1/113.9);
+	attack_distance = 794 + 1245 * 75 * (1/radius - 1/115.6);
 //	cout << "attack distance: " << attack_distance << endl;
 
     getFanPosition(fanPosition, fans, cycle_center, radius);
@@ -66,21 +67,24 @@ int Energy::run(cv::Mat &src){
 //    cout << "The target armor's position is " << target_armor << endl;
 //    cout<<"The target armor center is: "<<target_center<<endl;
 
+	if (energy_rotation_init) {
+		initRotation();
+		return 0;
+	}
+
     getHitPoint();
 //    hit_point = target_center;
 //    cout << "The hit point position is " << hit_point << endl;
     
-
-    if(!isSendTarget)return 0;
 //    cout<<"send"<<endl;
     cout<<"position mode: "<<position_mode<<endl;
 
     gimbleRotation();
+	if (!isSendTarget)return 0;
 
     sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);
     cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
     cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
-    cout<<"mark_yaw: "<<mcuData.mark_yaw<<'\t'<<"mark_pitch: "<<mcuData.mark_pitch<<endl;
 
 //    cout<<"send_cnt: "<<send_cnt<<endl;
 

main.cpp

@@ -27,19 +27,31 @@ using namespace cv;
 using namespace std;
 
 mcu_data mcuData = {
-        .curr_yaw = 0,
+        0,
-        .curr_pitch = 0,
+        0,
-        .state = ENERGY_STATE,
+        ARMOR_STATE,
-        .mark = 0,
+        0,
-        .use_classifier = 0,
+        0,
-        .enemy_color = ENEMY_BLUE
+        ENEMY_BLUE,
 };
 
+
+
 int main(int argc, char *argv[]) {
     process_options(argc, argv);
     Serial serial(115200);
+	uint8_t last_state = mcuData.state;
     thread receive(uartReceive, &serial);
 
+	thread change([&]() {
+		while (true) {
+			Sleep(10000);
+			if (mcuData.state == ARMOR_STATE) mcuData.state = ENERGY_STATE;
+			else if (mcuData.state == ENERGY_STATE)mcuData.state = ARMOR_STATE;
+			cout << "state changed to " << mcuData.state << endl;
+		}
+	});
+
     int from_camera = 1;
     if (!run_with_camera) {
         cout << "Input 1 for camera, 0 for video files" << endl;
@@ -59,8 +71,8 @@ int main(int argc, char *argv[]) {
             video_armor = new CameraWrapper(0, "armor");
             video_energy = new CameraWrapper(1, "energy");
         } else {
-            video_armor = new VideoWrapper("/home/xinyang/Desktop/DataSets/video/blue_4.mp4");
+            video_armor = new VideoWrapper("E:/Robomaster/RM_auto-aim/build/r_l_640.avi");
-            video_energy = new VideoWrapper("/home/xinyang/Desktop/DataSets/video/blue_4.mp4");
+            video_energy = new VideoWrapper("E:/Robomaster/RM_auto-aim/build/r_l_640.avi");
         }
         if (video_armor->init()) {
             LOGM("video_armor source initialization successfully.");
@@ -83,7 +95,7 @@ int main(int argc, char *argv[]) {
                 video_armor->read(armor_src);
             }
             if (video_energy) {
-                video_energy->read(armor_src);
+                video_energy->read(energy_src);
             }
         }
 
@@ -97,6 +109,11 @@ int main(int argc, char *argv[]) {
         do {
             CNT_TIME("Total", {
                 if (mcuData.state == ENERGY_STATE) {
+					if (last_state == ARMOR_STATE) {
+						energy.setEnergyRotationInit();
+						cout << "set" << endl;
+					}
+					last_state = mcuData.state;
                     if (video_energy) {
                         ok = video_energy->read(energy_src);
                         if (!ok) {
@@ -110,6 +127,8 @@ int main(int argc, char *argv[]) {
                             imshow("energy src", energy_src);
                         }
                         if (from_camera == 0) {
+							cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
+							imshow("resize", energy_src);
                             energy.extract(energy_src);
                         }
                         energy.run(energy_src);
@@ -122,6 +141,7 @@ int main(int argc, char *argv[]) {
                     }
 
                 } else if (mcuData.state == ARMOR_STATE) {
+					last_state = mcuData.state;
                     if (video_armor) {
                         ok = video_armor->read(armor_src);
                         if (!ok) {
@@ -146,6 +166,9 @@ int main(int argc, char *argv[]) {
                         }
                     }
                 }
+				waitKey(1);
+				
+				
             });
         } while (ok);
 

others/include/additions/additions.h

@@ -20,8 +20,6 @@ struct mcu_data{
     uint8_t mark;
     uint8_t use_classifier;
     uint8_t enemy_color;
-    float mark_yaw;
-    float mark_pitch;
 };
 
 extern mcu_data mcuData;

others/src/additions/additions.cpp

@@ -30,20 +30,20 @@ void uartReceive(Serial* pSerial) {
     }
 }
 
-void initVideoWriter(cv::VideoWriter& video, const std::string &armor_filename_prefix){
+void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix){
-    std::ifstream in_1(armor_filename_prefix + "cnt.txt");
+    std::ifstream in(filename_prefix + "cnt.txt");
-    int cnt_1 = 0;
+    int cnt = 0;
-    if (in_1.is_open())
+    if (in.is_open())
     {
-        in_1 >> cnt_1;
+        in >> cnt;
-        in_1.close();
+        in.close();
     }
-    std::string armor_file_name = armor_filename_prefix + std::to_string(cnt_1) + ".avi";
+    std::string armor_file_name = filename_prefix + std::to_string(cnt) + ".avi";
-    cnt_1++;
+    cnt++;
-    std::ofstream out_1(armor_filename_prefix + "cnt.txt");
+    std::ofstream out(filename_prefix + "cnt.txt");
-    if (out_1.is_open()) {
+    if (out.is_open()) {
-        out_1 << cnt_1 << std::endl;
+        out << cnt << std::endl;
-        out_1.close();
+        out.close();
     }
     video.open(armor_file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true);
 }