├── Motor+encoder_rosserial.ino ├── pkg_ros_motor ├── scripts │ └── node_motor.py ├── package.xml └── CMakeLists.txt └── README.md /Motor+encoder_rosserial.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | ros::NodeHandle nh; 7 | 8 | std_msgs::Int64 encoder; 9 | 10 | 11 | ros::Publisher encoderValue("encoderValue", &encoder); 12 | 13 | 14 | 15 | // arduino hardware pins 16 | #define MOTOR_1 0 17 | #define BRAKE 0 18 | #define CW 1 19 | #define CCW 2 20 | 21 | 22 | #define MOTOR_A1_PIN 7 23 | #define MOTOR_B1_PIN 8 24 | 25 | #define PWM_MOTOR_1 5 26 | 27 | const int encoderPinA = 19; 28 | const int encoderPinB = 20; 29 | volatile int64_t currentPosition = 0; 30 | 31 | 32 | void pwm_input( const std_msgs::Int16& pwm_value){ 33 | int pwm =0; 34 | pwm = pwm_value.data; 35 | 36 | if ( pwm > 0 ) 37 | { 38 | motorGo(MOTOR_1,CCW,pwm); 39 | } 40 | else 41 | { 42 | motorGo(MOTOR_1,CW,abs(pwm)); 43 | } 44 | 45 | } 46 | ros::Subscriber pwm("PWM_Values", &pwm_input); 47 | 48 | void setup() 49 | { 50 | nh.initNode(); 51 | nh.advertise(encoderValue); 52 | nh.subscribe(pwm); 53 | 54 | pinMode (encoderPinA, INPUT_PULLUP); 55 | pinMode (encoderPinB, INPUT_PULLUP); 56 | attachInterrupt (digitalPinToInterrupt (encoderPinA), readEncoderA, CHANGE); 57 | attachInterrupt (digitalPinToInterrupt (encoderPinB), readEncoderB, CHANGE); 58 | 59 | TCCR1B = TCCR1B & 0b11111000 | 1; 60 | } 61 | 62 | 63 | 64 | void motorGo(int16_t motor, int16_t direct, uint16_t pwm) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255); 65 | { 66 | if(motor == MOTOR_1) 67 | { 68 | if(direct == CW) 69 | { 70 | digitalWrite(MOTOR_A1_PIN, LOW); 71 | digitalWrite(MOTOR_B1_PIN, HIGH); 72 | } 73 | else if(direct == CCW) 74 | { 75 | digitalWrite(MOTOR_A1_PIN, HIGH); 76 | digitalWrite(MOTOR_B1_PIN, LOW); 77 | } 78 | else 79 | { 80 | digitalWrite(MOTOR_A1_PIN, LOW); 81 | digitalWrite(MOTOR_B1_PIN, LOW); 82 | } 83 | 84 | analogWrite(PWM_MOTOR_1, pwm); 85 | } 86 | 87 | } 88 | void readEncoderA() 89 | { 90 | if (digitalRead(encoderPinA) != digitalRead(encoderPinB)) 91 | { 92 | currentPosition++; 93 | } 94 | else 95 | { 96 | currentPosition--; 97 | } 98 | } 99 | 100 | void readEncoderB() 101 | { 102 | if (digitalRead(encoderPinA) == digitalRead(encoderPinB)) 103 | { 104 | currentPosition++; 105 | } 106 | else 107 | { 108 | currentPosition--; 109 | } 110 | } 111 | 112 | 113 | void loop() 114 | { 115 | 116 | nh.loginfo("Encoder Value"); 117 | 118 | encoder.data = currentPosition; 119 | encoderValue.publish( &encoder ); 120 | 121 | nh.spinOnce(); 122 | delay(100); 123 | 124 | } 125 | -------------------------------------------------------------------------------- /pkg_ros_motor/scripts/node_motor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from simple_pid import PID 4 | 5 | from std_msgs.msg import Int64 6 | from std_msgs.msg import Float64 7 | from std_msgs.msg import Int16 8 | 9 | 10 | PWM_Output = 0 11 | PPR = 41 12 | gearRatio = 60 13 | decodeNumber = 4 14 | pi = 3.14 15 | r = 0.0325 16 | 17 | encoderValue = 0 18 | started = False 19 | started1 = False 20 | desired_Position = 0.0 21 | 22 | current_wheel_distance = (encoderValue * 2 * pi *r) / (PPR * gearRatio * decodeNumber) 23 | current_angle = encoderValue*((PPR * gearRatio * decodeNumber)/360) 24 | 25 | pub = rospy.Publisher('/PWM_Values',Int16,queue_size=100) 26 | pub1 = rospy.Publisher('/Current_angle',Int16,queue_size=100) 27 | def sub_encoderValue(): 28 | rospy.init_node('node_motor', anonymous=True) 29 | rospy.Subscriber('encoderValue',Int64,callback) 30 | rospy.Subscriber('desiredPosition',Float64,callback1) 31 | timer = rospy.Timer(rospy.Duration(0.01),timer_callback) 32 | rospy.spin() 33 | timer.shutdown() 34 | 35 | def callback(data): 36 | global started,encoderValue 37 | print "EncoderValue Received",encoderValue 38 | encoderValue = data.data 39 | if (not started): 40 | started = True 41 | 42 | def callback1(data): 43 | global started1,desired_Position 44 | #print "Desired Received",desired_Position 45 | desired_Position = data.data 46 | if (not started1): 47 | started1 = True 48 | 49 | 50 | 51 | def timer_callback(event): 52 | global started,started1,pub,encoderValue,PWM_Output,desired_Position,current_wheel_distance,current_angle 53 | 54 | if(started1): 55 | if (started): 56 | 57 | previouswheeldistance = current_wheel_distance 58 | pid = PID(100,0.5,1, setpoint=desired_Position,auto_mode=True) 59 | pid.output_limits = (-255, 255) 60 | pid.sample_time = 0.001 61 | PWM_Output = pid(previouswheeldistance) 62 | current_wheel_distance = (encoderValue * 2 * pi *r) / (PPR * gearRatio * decodeNumber) 63 | 64 | #previous_angle=current_angle 65 | #pid = PID(0.022,0.01,2,setpoint=desired_Position) 66 | #pid.output_limits = (-255, 255) 67 | #pid.sample_time = 0.001 68 | #PWM_Output = pid(previous_angle) 69 | 70 | #if( 0 < PWM_Output <= 13): 71 | # PWM_Output = PWM_Output + 11.5 72 | #elif (-13 <= PWM_Output < 0): 73 | # PWM_Output = PWM_Output - 11.5 74 | # 75 | #current_angle = encoderValue/24 76 | 77 | pub.publish(PWM_Output) 78 | 79 | print "Publishing PWM Values",PWM_Output 80 | print "Current Wheel distance",current_wheel_distance 81 | #print "Current angle",current_angle 82 | #print "Desired Position",desired_Position 83 | 84 | if __name__ == '__main__': 85 | print "Running" 86 | sub_encoderValue() -------------------------------------------------------------------------------- /pkg_ros_motor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pkg_ros_motor 4 | 0.0.0 5 | The pkg_ros_motor package 6 | 7 | 8 | 9 | 10 | jeelros 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | catkin 53 | roscpp 54 | rospy 55 | std_msgs 56 | message_generation 57 | 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | roscpp 63 | rospy 64 | std_msgs 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /pkg_ros_motor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(pkg_ros_motor) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | message_generation 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | #add_message_files( 52 | # FILES 53 | # 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | #add_service_files( 58 | # FILES 59 | # AddTwoInts.srv 60 | #) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | generate_messages( 71 | DEPENDENCIES 72 | std_msgs 73 | ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES pkg_ros_basics 107 | CATKIN_DEPENDS roscpp rospy std_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/pkg_ros_basics.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/pkg_ros_basics_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # catkin_install_python(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pkg_ros_basics.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Title: Closed Loop Control of a DC Motor with ROS 2 | == 3 | - [Title: Closed Loop Control of a DC Motor with ROS](#title-closed-loop-control-of-a-dc-motor-with-ros) 4 | - [Introduction](#introduction) 5 | - [Components Used](#components-used) 6 | - [Programming Languages Used](#programming-languages-used) 7 | - [Theoretical Description](#theoretical-description) 8 | - [How does a encoder work?](#how-does-a-encoder-work) 9 | - [What is a PID Controller?](#what-is-a-pid-controller) 10 | - [Program Explaination](#program-explaination) 11 | - [Arduino Code (serial node)](#arduino-code-serial-node) 12 | - [Code to rotate the motor.](#code-to-rotate-the-motor) 13 | - [Code for reading the encoder values](#code-for-reading-the-encoder-values) 14 | - [Circuit Diagram](#circuit-diagram) 15 | - [Python Node (node_motor.py)](#python-node-node_motorpy) 16 | - [Output Video](#output-video) 17 | - [Appendix and FAQ](#appendix-and-faq) 18 | - [Tags: `Closed Loop Control` `Documentation` `DC Motor with encoder` `ROS` `Arduino` `Tutorial` `python` `VNH3ASP30 Motor shield`](#tags-closed-loop-control-documentation-dc-motor-with-encoder-ros-arduino-tutorial-python-vnh3asp30-motor-shield) 19 | 20 | 21 | ## Introduction 22 | 23 | This Project was created with the aim of controlling the motor with ROS Code. The motor is controlled using a PID Controller implemented a ROS Node. Encoder Sensing and Motor movement was done using Arduino Mega 2560. 24 | 25 | ## Components Used 26 | 1. ROS Melodic along with rosserial library/package. 27 | 2. Arduino Mega 2560 28 | 3. DC Geared Motor with Encoder ( 12v, 300RPM at 10Kgcm RMCS-5033 )
29 | datasheet :- https://robokits.download/downloads/RMCS%205033.pdf 30 | 4. Motor Shield : VNH3ASP30 DC Motor Driver 2x14A
31 | datasheet :- https://www.pololu.com/file/0J52/vnh2sp30.pdf 32 | 5. Li-ion Battery 33 | 34 | ## Programming Languages Used 35 | 1. Python 36 | 2. C++ 37 | 38 | ## Theoretical Description 39 | 40 | There is a Arduino Sketch as shown in this repository where ros.h library is included to enable ros capabilities. Now we use the rosserial package which enables serial communication of Arduino with ROS Environment. Now there is a Node that has a PID Controller implementation. 41 | Basic overall structure is 42 | 43 | Arduino Node :- 44 | * Publishes - encoderValue 45 | * Subscribes - PWM_Values 46 | 47 | Node_motor.py :- 48 | * Publishes - PWM_Values 49 | * Subscribes - encoderValue, desiredPosition 50 | 51 | Below is the **rqt_graph** of the program 52 | 53 | ![rosgraph](https://user-images.githubusercontent.com/56308805/111037012-3fede880-8448-11eb-894a-403167de0a31.png) 54 | 55 | Basic Flow of the program :- 56 | 57 | 1. The encodervalue is published from Arduino node. 58 | 2. node_motor subscribes to /encoderValue and /desiredPosition. 59 | 3. Now we Publish the desired angle/position to /desiredPosition. 60 | 4. These inputs are provided to the PID controller implemented in the **node_motor.py** where Setpoint is **/desiredPosition** and feedback is /encodervalue. 61 | 5. The PID controller gives PWM_value as output to control the motor direction and speed at a given refresh rate. 62 | 6. This PWM value is published to /PWM_value and Arduino node subscribes to the /PWM_value and accordingly give commands to the motor driver. 63 | 64 | #### How does a encoder work? 65 | 66 | Incremental encoders, in their basic form, detect light, magnetic or physical changes on a disc that rotates with the shaft. 67 | Here motor is equiped with a optical encoder which observes the transition of light on a receiver as shown in the picture 68 | 69 | encoder 70 | 71 | Now, if we use a single channel encoder as shown above there is a disadvantage that we cannot determine the direction of the movement of the motor (i.e. Clockwise or Anti-Clockwise) 72 | 73 | To overcome this disadvantage we use quadrature encoder as shown below. 74 | 75 | encoder_quad 76 | 77 | Now we can calculate the phase difference between the output signals of the encoder and determine the direction as shown. 78 | 79 | #### What is a PID Controller? 80 | 81 | A PID controller (proportional–integral–derivative controller) is a closed loop control mechanism employing feedback that is widely used in industrial control systems and a variety of other applications requiring continuously modulated control. A PID controller continuously calculates an *error value e(t)* as the difference between a desired *setpoint (SP)* and a measured *process variable (PV)* and applies a correction based on proportional, integral, and derivative terms (denoted P, I, and D respectively). 82 | 83 | In our case:- 84 | 85 | pid 86 | 87 | 88 | We need to tune the P,I,D parameters individually for a specific control system according to the requirement. 89 | 90 | pid_tunning 91 | 92 | 93 | Following are general types of system response depending upon the P,I,D values. 94 | 95 | 96 | introduction-to-pid-damped-controller 97 | 98 | ## Program Explaination 99 | 100 | 101 | ### Arduino Code (serial node) 102 | 103 | #### Code to rotate the motor. 104 | Defining the Motor Pins for motor1 slot on the shield. 105 | ```cpp 106 | #define MOTOR_1 0 107 | #define BRAKE 0 108 | #define CW 1 109 | #define CCW 2 110 | #define MOTOR_A1_PIN 7 111 | #define MOTOR_B1_PIN 8 112 | 113 | #define PWM_MOTOR_1 5 114 | 115 | const int encoderPinA = 19; 116 | const int encoderPinB = 20; 117 | ``` 118 | Now, we define a function to move the motor according to the direction and PWM Signal 119 | ```cpp 120 | void motorGo(int16_t motor, int16_t direct, uint16_t pwm) 121 | if(motor == MOTOR_1) 122 | { 123 | if(direct == CW) 124 | { 125 | digitalWrite(MOTOR_A1_PIN, LOW); 126 | digitalWrite(MOTOR_B1_PIN, HIGH); 127 | } 128 | else if(direct == CCW) 129 | { 130 | digitalWrite(MOTOR_A1_PIN, HIGH); 131 | digitalWrite(MOTOR_B1_PIN, LOW); 132 | } 133 | else 134 | { 135 | digitalWrite(MOTOR_A1_PIN, LOW); 136 | digitalWrite(MOTOR_B1_PIN, LOW); 137 | } 138 | 139 | analogWrite(PWM_MOTOR_1, pwm); 140 | } 141 | ``` 142 | Now we want create a function to move the motor according to the sign of PWM value given to it. 143 | Also we write the subscriber for the topic /PWM_Values. 144 | ```cpp 145 | void pwm_input( const std_msgs::Int16& pwm_value){ 146 | int pwm =0; 147 | pwm = pwm_value.data; 148 | 149 | if ( pwm > 0 ) 150 | { 151 | motorGo(MOTOR_1,CCW,pwm); 152 | } 153 | else 154 | { 155 | motorGo(MOTOR_1,CW,abs(pwm)); 156 | } 157 | ros::Subscriber pwm("PWM_Values", &pwm_input); 158 | ``` 159 | #### Code for reading the encoder values 160 | Setting up the encoder pins as interrupt pins 161 | ```cpp 162 | volatile int64_t currentPosition = 0; 163 | 164 | void setup() 165 | { 166 | nh.initNode(); 167 | nh.advertise(encoderValue); 168 | nh.subscribe(pwm); 169 | 170 | pinMode (encoderPinA, INPUT_PULLUP); 171 | pinMode (encoderPinB, INPUT_PULLUP); 172 | attachInterrupt (digitalPinToInterrupt (encoderPinA), readEncoderA, CHANGE); 173 | attachInterrupt (digitalPinToInterrupt (encoderPinB), readEncoderB, CHANGE); 174 | 175 | TCCR1B = TCCR1B & 0b11111000 | 1; 176 | } 177 | ``` 178 | Now we code the Interrupt service routine program to update the encoder value upon the movement of the shaft 179 | ```cpp 180 | void readEncoderA() 181 | { 182 | if (digitalRead(encoderPinA) != digitalRead(encoderPinB)) 183 | { 184 | currentPosition++; 185 | } 186 | else 187 | { 188 | currentPosition--; 189 | } 190 | } 191 | 192 | void readEncoderB() 193 | { 194 | if (digitalRead(encoderPinA) == digitalRead(encoderPinB)) 195 | { 196 | currentPosition++; 197 | } 198 | else 199 | { 200 | currentPosition--; 201 | } 202 | } 203 | ``` 204 | Now we integrate the ros.h library to enable ROS compatiblity 205 | Header files for including ros and std_msgs package which is to define the data type of that topic. 206 | ```cpp 207 | #include 208 | #include 209 | #include 210 | #include 211 | 212 | ros::NodeHandle nh; 213 | 214 | std_msgs::Int64 encoder; 215 | 216 | ros::Publisher encoderValue("encoderValue", &encoder); 217 | ``` 218 | Now we write the publisher inside loop to continuosly publish the updated encodervalues to the given topic. 219 | 220 | ```cpp 221 | void loop() 222 | { 223 | 224 | nh.loginfo("Encoder Value"); 225 | 226 | encoder.data = currentPosition; 227 | encoderValue.publish( &encoder ); 228 | 229 | nh.spinOnce(); 230 | delay(100); 231 | 232 | } 233 | ``` 234 | --- 235 | #### Circuit Diagram 236 | Circuit diagram for interfacing encoder motor with Arduino Mega and VNH2P30 Motor Shield. 237 | 238 | circuit_diagram 239 | 240 | --- 241 | ### Python Node (node_motor.py) 242 | 243 | This code is written for the angular control of the motor, but it can be modified for the distance/position control according to the wheel diameter. 244 | 245 | importing rospy and std_msgs packages. Defining global variables and motor specifications. 246 | ```python 247 | #!/usr/bin/env python 248 | import rospy 249 | from simple_pid import PID 250 | 251 | from std_msgs.msg import Int64 252 | from std_msgs.msg import Float64 253 | from std_msgs.msg import Int16 254 | 255 | PWM_Output = 0 256 | PPR = 41 257 | gearRatio = 60 258 | decodeNumber = 4 259 | 260 | encoderValue = 0 261 | started = False 262 | started1 = False 263 | desired_Position = 0.0 264 | 265 | ``` 266 | Defining Callback functions for subscribers and a general function with timer for subscribing to two topics */encoderValue* and */desiredPosition*. 267 | ```python 268 | pub = rospy.Publisher('/PWM_Values',Int16,queue_size=100) 269 | pub1 = rospy.Publisher('/Current_angle',Int16,queue_size=100) 270 | 271 | def sub_encoderValue(): 272 | rospy.init_node('node_motor', anonymous=True) 273 | rospy.Subscriber('encoderValue',Int64,callback) 274 | rospy.Subscriber('desiredPosition',Float64,callback1) 275 | timer = rospy.Timer(rospy.Duration(0.01),timer_callback) 276 | rospy.spin() 277 | timer.shutdown() 278 | 279 | def callback(data): 280 | global started,encoderValue 281 | print "EncoderValue Received",encoderValue 282 | encoderValue = data.data 283 | if (not started): 284 | started = True 285 | 286 | def callback1(data): 287 | global started1,desired_Position 288 | desired_Position = data.data 289 | if (not started1): 290 | started1 = True 291 | ``` 292 | 293 | Defining a callback function to calculate the PID output according to the updated */encoderValue* and */desiredPosition*. After that the we run the sub_encoder() function with runs whole program. 294 | ```python 295 | def timer_callback(event): 296 | global started,started1,pub,encoderValue,PWM_Output,desired_Position,current_wheel_distance,current_angle 297 | 298 | if(started1): 299 | if (started): 300 | 301 | previous_angle=current_angle 302 | pid = PID(0.022,0.01,2,setpoint=desired_Position) 303 | pid.output_limits = (-255, 255) 304 | pid.sample_time = 0.001 305 | PWM_Output = pid(previous_angle) 306 | 307 | if( 0 < PWM_Output <= 13): 308 | PWM_Output = PWM_Output + 11.5 309 | elif (-13 <= PWM_Output < 0): 310 | PWM_Output = PWM_Output - 11.5 311 | 312 | current_angle = encoderValue/24 313 | 314 | pub.publish(PWM_Output) 315 | 316 | print "Publishing PWM Values",PWM_Output 317 | print "Current angle",current_angle 318 | print "Desired Position",desired_Position 319 | 320 | if __name__ == '__main__': 321 | print "Running" 322 | sub_encoderValue() 323 | 324 | ``` 325 | --- 326 | ## Output Video 327 | 328 | **Steps to run the code** 329 | 330 | ```shell 331 | roscore 332 | ``` 333 | check your port number in Arduino IDE.Running the serial node after uploading the code to arduino 334 | ``` 335 | rosrun rosserial_arduino serial_node.py /dev/tty/ACM0 336 | ``` 337 | running the motor control node. 338 | ``` 339 | rosrun pkg_name node_motor.py 340 | ``` 341 | Now we publish the required output angle to /desiredPosition topic using terminal. 342 | ``` 343 | rostopic pub /desiredPosition std_msgs/Float64 "data: 0.0" 344 | ``` 345 | You will output similar to this 346 | YouTube Video Link:- https://youtu.be/7hB2t6oS2qI 347 | 348 | 349 | ## Appendix and FAQ 350 | 351 | 352 | **Find this document incomplete?** Leave a comment! 353 | Suggestions are always welcome. 354 | 355 | 356 | ###### Tags: `Closed Loop Control` `Documentation` `DC Motor with encoder` `ROS` `Arduino` `Tutorial` `python` `VNH3ASP30 Motor shield` 357 | 358 | --- 359 | --------------------------------------------------------------------------------