添加visual servo 结点

Robin970822 · Mar 30, 2018 · 8279001 · 8279001
1 parent 4b91d98
commit 8279001
Show file tree

Hide file tree

Showing 9 changed files with 394 additions and 92 deletions.
diff --git a/.gitignore b/.gitignore
@@ -4,3 +4,5 @@
 *.pyc
 
 scripts/ckpts/*
+.vscode/settings.json
+.vscode/c_cpp_properties.json
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -21,10 +21,6 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
@@ -34,26 +30,6 @@ find_package(catkin REQUIRED COMPONENTS
 ## Declare ROS messages, services and actions ##
 ################################################
 
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
 ## Generate messages in the 'msg' folder
  add_message_files(
    FILES
@@ -87,16 +63,6 @@ generate_messages(
 ## Declare ROS dynamic reconfigure parameters ##
 ################################################
 
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 # generate_dynamic_reconfigure_options(
 #   cfg/DynReconf1.cfg
@@ -106,19 +72,26 @@ generate_messages(
 ###################################
 ## catkin specific configuration ##
 ###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
+
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES icra_firefly
-  CATKIN_DEPENDS cv_bridge geometry_msgs move_base nav_msgs roscpp rospy serial std_msgs tf message_runtime
+   CATKIN_DEPENDS 
+   cv_bridge 
+   geometry_msgs 
+   move_base 
+   nav_msgs 
+   roscpp 
+   rospy 
+   serial 
+   std_msgs 
+   tf 
+   message_runtime
 #  DEPENDS system_lib
 )
 
+add_subdirectory(VisualServo)
+
 ###########
 ## Build ##
 ###########
@@ -161,50 +134,3 @@ include_directories(
 #   ${catkin_LIBRARIES}
 # )
 
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_icra_firefly.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
diff --git a/VisualServo/MoveControl.cpp b/VisualServo/MoveControl.cpp
@@ -0,0 +1,115 @@
+#include "MoveControl.h"
+#define UP 1
+#define DOWN 2
+#define MIDDLE 3
+
+MoveControl::MoveControl()
+{
+	flag=false;
+	char portName[20]="/dev/ttyUSB0";
+	int baud=100000;
+	if(SP.init_8n1(portName)&&SP.openPort(baud)==true)
+	{
+		printf("\nSystem is Online\n" );	
+		flag=true;
+	}
+	setbuf(stdin,NULL);
+	CH0=1024;
+	CH1=1024;
+	CH2=1024;
+	CH3=1024;
+	S1=3;
+	S2=1;
+}
+
+MoveControl::~MoveControl()
+{
+	SP.closePort();
+}
+
+
+bool MoveControl::Attack_Mode_Change()
+{
+	for(int i=1;i<=3;i++)
+	{
+		S1=MIDDLE;
+		Send_Message();
+		usleep(14*1000);
+	}	
+	for(int i=1;i<=3;i++)
+	{
+		S1=UP;
+		Send_Message();
+		usleep(14*1000);
+	}
+	for(int i=1;i<=3;i++)
+	{
+		S1=MIDDLE;
+		Send_Message();
+		usleep(14*1000);
+	}
+}
+
+bool MoveControl::Attack(int cnt)
+{
+	Attack_Mode_Change(); 
+	for(int j=1;j<=cnt;j++)
+	{	
+		for(int i=1;i<=30;i++)
+		{
+			S1=MIDDLE;
+			Send_Message();
+			usleep(14*1000);
+		}
+		for(int i=1;i<=30;i++)
+		{
+			S1=DOWN;
+			Send_Message();
+			usleep(14*1000);
+		}
+		for(int i=1;i<=30;i++)
+		{
+			S1=MIDDLE;
+			Send_Message();
+			usleep(30*1000);
+		}
+	}
+	Attack_Mode_Change(); 
+}
+
+
+bool MoveControl::Forward_Back(int f_b)
+{
+	CH1=1024+(f_b/100.0)*660;
+}
+
+bool MoveControl::Right_Left(int r_l)
+{
+	CH0=1024+(r_l/100.0)*660;
+}
+
+bool MoveControl::Up_Down(int u_d)
+{
+	CH3=1024+(u_d/100.0)*660;
+}
+
+bool MoveControl::Right_Left_Rotation(int r_l_R)
+{
+	CH2=1024+(r_l_R/100.0)*660;
+}
+
+bool MoveControl::Send_Message()
+{
+	char sendstr[18];
+	for (int i=6;i<=17;i++)
+	{
+	sendstr[i] = 0b00000000;	
+	}
+	sendstr[0]=CH0;
+	sendstr[1]=(CH0>>8)+(CH1<<3);
+	sendstr[2]=(CH1>>5)+(CH2<<6);
+	sendstr[3]=CH2>>2;
+	sendstr[4]=(CH2>>10)+(CH3<<1);
+	sendstr[5]=(CH3>>7)+(S1<<6)+(S2<<4);
+	SP.writeData(sendstr,18);
+}
diff --git a/VisualServo/MoveControl.h b/VisualServo/MoveControl.h
@@ -0,0 +1,26 @@
+#include "SerialPort.h"
+
+class MoveControl
+{
+	public:
+		MoveControl();
+		~MoveControl();
+
+		bool flag;
+		int CH0;
+		int CH1;
+		int CH2;
+		int CH3;
+		int S1;
+		int S2;
+
+		bool Send_Message();
+		bool Attack_Mode_Change();
+		bool Attack(int n);
+		bool Forward_Back(int f_b);
+		bool Right_Left(int r_l);
+		bool Up_Down(int u_d);
+		bool Right_Left_Rotation(int r_l_R);
+	private:
+		SerialPort SP;		
+};