ROS OpenCR Motor 구동 / Encoder 값 받기

한이음/GPS 좌표 트래킹을 통한 자율주행 로봇

ROS OpenCR Motor 구동 / Encoder 값 받기

MZ 아이종 2022. 3. 25. 04:17

robot을 동작하기 위해서는 센서를 통해 입력받은 값을 모터로 넘겨줘야 하고, 이동 시에 모터의 encoder를 통해 값을 다시 전달해 주어 계산하면 보다 정확한 주행이 가능하다.

rosserial을 통해 값을 넘겨주고, encoder 값을 ros로 넘겨줘 보자.

사용버전. 노트북

˙ Samsung Ultrabook i7

˙ Ubuntu 18.04.6 LTS

˙ Ros Melodic

˙ OpenCR

˙ MB-3866E (motor)

˙ Tb67h420ftg (motor driver)

단계

1. rosserial을 통해 motor 구동 확인

2. Encoder 값 받기

3. rosserial을 통해 Encoder 값 받기

4. 코드 통합

1. rosserial을 통해 motor 구동 확인

teleop_twist를 통해 값을 입력 받으면 모터가 동작 되도록 한다. (이전에 목적에 맞게 수정한 코드이다.)

연결은 아래와 같다.

Open-CR	4	5	6	9	10	11	GND(핀 많은쪽)
Tb67h420ftg	INB1	INB2	PWMB	INA1	INA2	PWMA	GND(왼쪽)

POWER					VCC	GND
MOTOR A	VCC(빨강)	GND(검은색)
MOTOR B			VCC(빨강)	GND(검은색)
Tb67h420ftg	A+	A-	B+	B-	VIN	GND(오른쪽)

※ MOTOR A 우회전, MOTOR B 좌회전

연결에 대응되는 arduino 쪽 모터 동작 코드는 아래와 같다.

//ros

#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>

//R - motor
#define ENABLE_A  11   
#define IN1_A     9 
#define IN2_A     10 

//L - motor
#define ENABLE_B  6
#define IN3_B     4
#define IN4_B     5

ros::NodeHandle  nh;

float velocity = 0;

void MOTOR( const geometry_msgs::Twist& msg)
{  
  //motor
  if(msg.linear.x == -1) //w
  {
    digitalWrite(IN1_A, HIGH);  
    digitalWrite(IN2_A, LOW);
    
    digitalWrite(IN3_B, LOW);  
    digitalWrite(IN4_B, HIGH);
    
    velocity = msg.angular.x;
  }

  else if(msg.linear.x == 1) //s
  {
    digitalWrite(IN1_A, LOW);  
    digitalWrite(IN2_A, HIGH);
    
    digitalWrite(IN3_B, HIGH);  
    digitalWrite(IN4_B, LOW);
    
    velocity = msg.angular.x;
  }
  
  else if(msg.linear.y == -1) //a
  {
    digitalWrite(IN1_A, HIGH);  
    digitalWrite(IN2_A, LOW);
   
    digitalWrite(IN3_B, HIGH);  
    digitalWrite(IN4_B, LOW);
    
    velocity = msg.angular.y;
  }
  
  else if(msg.linear.y == 1) //d
  {
    digitalWrite(IN1_A, LOW);  
    digitalWrite(IN2_A, HIGH);
    
    digitalWrite(IN3_B, LOW);  
    digitalWrite(IN4_B, HIGH);
    velocity = msg.angular.y;
  }
  
  else if(msg.linear.x == msg.linear.y)
    velocity = 0;
  
  else;
  
  analogWrite(ENABLE_A, velocity);
  analogWrite(ENABLE_B, velocity);
  
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &MOTOR);

void setup()
{
  pinMode(IN1_A, OUTPUT); // 방향1
  pinMode(IN2_A, OUTPUT); // 방향2
  pinMode(ENABLE_A, OUTPUT); // 속도
  
  pinMode(IN3_B, OUTPUT); // 방향3
  pinMode(IN4_B, OUTPUT); // 방향4
  pinMode(ENABLE_B, OUTPUT); // 속도

  nh.initNode();
  nh.subscribe(sub);
}

void loop(){
  nh.spinOnce();
  delay(1);
}

아래의 명령어를 각각의 터미널에 입력한다.

$ roscore  #1st Terminal
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600 #2nd Terminal
$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard #3rd Terminal

2. Encoder 값 받기

rosseial 이전에 Encoder 값이 정상적으로 출력되는지 arduino terminal을 통해 확인한다.

연결은 아래와 같다.

MOTOR A	5V(빨간색)	GND(검은색)			B(초록색)	A(흰색)
MOTOR B	5V(빨간색)	GND(검은색)	B(초록색)	A(흰색)
Open-CR	5V	GND	2	3	7	8

Encoder 값을 확인하기 위한 코드는 아래와 같다.

Encoder의 인터럽트를 사용할 수 있는 핀이 정해져 있고, code 상에서 핀이름도 설정되어 있는 핀 이름과 같아야 한다.

//R-MOTOR
#define encoderAPinA 8
#define encoderAPinB 7

//L-MOTOR
#define encoderBPinA 3
#define encoderBPinB 2

int encoderAPos = 0;
int encoderBPos = 0;

void setup() {
  pinMode(encoderAPinA, INPUT); 
  pinMode(encoderAPinB, INPUT); 
  attachInterrupt(4, doEncoderAA, CHANGE); // encoder pin on interrupt 0 (pin 2)
  attachInterrupt(3, doEncoderAB, CHANGE); // encoder pin on interrupt 1 (pin 3)
  pinMode(encoderBPinA, INPUT); 
  pinMode(encoderBPinB, INPUT); 
  attachInterrupt(0, doEncoderBA, CHANGE); // encoder pin on interrupt 0 (pin 2)
  attachInterrupt(1, doEncoderBB, CHANGE); // encoder pin on interrupt 1 (pin 3)

  Serial.begin (115200);
}

void loop()
{ 
  Serial.print ("Encoder A : ");
  Serial.print (encoderAPos);
  Serial.print(", ");
  Serial.print ("Encoder B : ");
  Serial.println (encoderBPos);

}

void doEncoderAA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoderAPinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderAPinB) == LOW) {  
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderAPinB) == HIGH) {   
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
//  Serial.println (encoderAPos);
}

void doEncoderAB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoderAPinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderAPinA) == HIGH) {  
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderAPinA) == LOW) {   
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
//  Serial.println (encoderAPos);
}

void doEncoderBA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoderBPinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderBPinB) == LOW) {  
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderBPinB) == HIGH) {   
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
//  Serial.println (encoderBPos);
}

void doEncoderBB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoderBPinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderBPinA) == HIGH) {  
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderBPinA) == LOW) {   
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
//  Serial.println (encoderBPos);
}

3. rosserial을 통해 Encoder 값 받기

정상적인 Encoder의 작동을 확인했으니 이제 rosserial을 통해 값을 넘겨줘 확인한다.

처음 ROS -> Arduino 모터동작 시에 만들었던 ros_motor project를 이용한다.

scr 폴더에 subscribe 할 cpp파일을 만들어 준다.

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

void msgCallback(const geometry_msgs::Twist &msg)  //const 상수
{
    ROS_INFO("Encoder A : %f, Encoder B : %f", msg.angular.x,msg.angular.y);
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "ros_arduino_sub");
    ros::NodeHandle nh;
    
    ros::Subscriber sub = nh.subscribe("encoder_pulse",100,msgCallback);
    ros::spin(); //어떤 값이 들어오기 전까지 대기 (다시 위로 올라감)
    return 0;
}

CMakeLists.txt 와 packag.xml 을 수정해 준다.

cmake_minimum_required(VERSION 3.0.2)
project(ros_motor)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  std_msgs
  geometry_msgs
)

## 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
# catkin_python_setup()

################################################
## 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 exec_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 exec_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
   ros_motor_variable.msg
 )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   std_msgs
   geometry_msgs
 )

################################################
## 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 exec_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
#   cfg/DynReconf2.cfg
# )

###################################
## 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 ros_motor
  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/ros_motor.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(ros_motor_pub src/ros_motor_pub.cpp)
add_executable(ros_motor_sub src/ros_motor_sub.cpp)
add_executable(ros_arduino_sub src/ros_arduino_sub.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(ros_motor_pub ${ros_motor_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(ros_motor_sub ${ros_motor_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(ros_arduino_sub ${ros_motor_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(ros_motor_pub
   ${catkin_LIBRARIES}
 )
target_link_libraries(ros_motor_sub
   ${catkin_LIBRARIES}
 )
target_link_libraries(ros_arduino_sub
   ${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
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_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_ros_motor.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)

<?xml version="1.0"?>
<package format="2">
  <name>ros_motor</name>
  <version>0.0.0</version>
  <description>The ros_motor package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="8ajs0114@gmail.com">js</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>BSD</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <url type="website">http://wiki.ros.org/ros_motor</url>

  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <author email="8ajs0114@gmail.com">js</author>


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

arduino code를 수정해 준다.

#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>

//R-MOTOR
#define encoderAPinA 8
#define encoderAPinB 7

//L-MOTOR
#define encoderBPinA 3
#define encoderBPinB 2

float encoderAPos = 0;
float encoderBPos = 0;

ros::NodeHandle  nh;
geometry_msgs::Twist encoder_msg;
ros::Publisher pub_encoder("encoder_pulse", &encoder_msg);

void setup() 
{
  pinMode(encoderAPinA, INPUT); 
  pinMode(encoderAPinB, INPUT); 
  attachInterrupt(4, doEncoderAA, CHANGE); // encoder pin on interrupt 4 (pin 4)
  attachInterrupt(3, doEncoderAB, CHANGE); // encoder pin on interrupt 3 (pin 3)
  pinMode(encoderBPinA, INPUT); 
  pinMode(encoderBPinB, INPUT); 
  attachInterrupt(0, doEncoderBA, CHANGE); // encoder pin on interrupt 0 (pin 0)
  attachInterrupt(1, doEncoderBB, CHANGE); // encoder pin on interrupt 1 (pin 1)

  nh.initNode();
  nh.advertise(pub_encoder);
}

void loop()
{ 
  encoder_msg.angular.x = encoderAPos;
  encoder_msg.angular.y = encoderBPos;
  pub_encoder.publish(&encoder_msg);
  nh.spinOnce();
}

void doEncoderAA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoderAPinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderAPinB) == LOW) {  
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderAPinB) == HIGH) {   
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
//  Serial.println (encoderAPos);
}

void doEncoderAB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoderAPinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderAPinA) == HIGH) {  
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderAPinA) == LOW) {   
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
//  Serial.println (encoderAPos);
}

void doEncoderBA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoderBPinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderBPinB) == LOW) {  
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderBPinB) == HIGH) {   
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
//  Serial.println (encoderBPos);
}

void doEncoderBB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoderBPinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderBPinA) == HIGH) {  
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderBPinA) == LOW) {   
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
//  Serial.println (encoderBPos);
}

아래의 명령어를 각각의 terminal에 입력해 준다.

$ roscore #1st Terminal
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600 #2nd Terminal
$ rosrun ros_motor ros_arduino_sub #3rd Terminal

4. 코드 통합

arduino motor subscribe code와 encoder publish code를 합쳐준다.

//ros
#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>

//R-MOTOR
#define ENABLE_A  11   
#define IN1_A     9 
#define IN2_A     10 

//L-MOTOR
#define ENABLE_B  6
#define IN3_B     4
#define IN4_B     5

//R-ENCODER
#define encoderAPinA 8
#define encoderAPinB 7

//L-ENCODER
#define encoderBPinA 3
#define encoderBPinB 2

float encoderAPos = 0;
float encoderBPos = 0;

ros::NodeHandle  nh;
geometry_msgs::Twist encoder_msg;
ros::Publisher pub_encoder("encoder_pulse", &encoder_msg);

float velocity = 0;

void MOTOR( const geometry_msgs::Twist& msg)
{  
  //motor
  if(msg.linear.x == -1) //w
  {
    digitalWrite(IN1_A, HIGH);  
    digitalWrite(IN2_A, LOW);
    
    digitalWrite(IN3_B, LOW);  
    digitalWrite(IN4_B, HIGH);
    
    velocity = msg.angular.x;
  }

  else if(msg.linear.x == 1) //s
  {
    digitalWrite(IN1_A, LOW);  
    digitalWrite(IN2_A, HIGH);
    
    digitalWrite(IN3_B, HIGH);  
    digitalWrite(IN4_B, LOW);
    
    velocity = msg.angular.x;
  }
  
  else if(msg.linear.y == -1) //a
  {
    digitalWrite(IN1_A, HIGH);  
    digitalWrite(IN2_A, LOW);
   
    digitalWrite(IN3_B, HIGH);  
    digitalWrite(IN4_B, LOW);
    
    velocity = msg.angular.y;
  }
  
  else if(msg.linear.y == 1) //d
  {
    digitalWrite(IN1_A, LOW);  
    digitalWrite(IN2_A, HIGH);
    
    digitalWrite(IN3_B, LOW);  
    digitalWrite(IN4_B, HIGH);
    velocity = msg.angular.y;
  }
  
  else if(msg.linear.x == msg.linear.y)
    velocity = 0;
  
  else;
  
  analogWrite(ENABLE_A, velocity);
  analogWrite(ENABLE_B, velocity);
  
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &MOTOR);

void setup()
{
  //R-MOTOR
  pinMode(IN1_A, OUTPUT); // 방향1
  pinMode(IN2_A, OUTPUT); // 방향2
  pinMode(ENABLE_A, OUTPUT); // 속도

  //L-MOTOR
  pinMode(IN3_B, OUTPUT); // 방향3
  pinMode(IN4_B, OUTPUT); // 방향4
  pinMode(ENABLE_B, OUTPUT); // 속도

  //R-ENCODER
  pinMode(encoderAPinA, INPUT); 
  pinMode(encoderAPinB, INPUT); 
  attachInterrupt(4, doEncoderAA, CHANGE); // encoder pin on interrupt 4 (pin 4)
  attachInterrupt(3, doEncoderAB, CHANGE); // encoder pin on interrupt 3 (pin 3)
 
  //L-ENCODER
  pinMode(encoderBPinA, INPUT); 
  pinMode(encoderBPinB, INPUT); 
  attachInterrupt(0, doEncoderBA, CHANGE); // encoder pin on interrupt 0 (pin 0)
  attachInterrupt(1, doEncoderBB, CHANGE); // encoder pin on interrupt 1 (pin 1)

  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(pub_encoder);
}

void loop()
{  
  encoder_msg.angular.x = encoderAPos;
  encoder_msg.angular.y = encoderBPos;
  pub_encoder.publish(&encoder_msg);
  nh.spinOnce();
  delay(1);
}

void doEncoderAA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoderAPinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderAPinB) == LOW) {  
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderAPinB) == HIGH) {   
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
//  Serial.println (encoderAPos);
}

void doEncoderAB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoderAPinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderAPinA) == HIGH) {  
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderAPinA) == LOW) {   
      encoderAPos = encoderAPos + 1;
    } 
    else {
      encoderAPos = encoderAPos - 1;
    }
  }
//  Serial.println (encoderAPos);
}

void doEncoderBA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoderBPinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderBPinB) == LOW) {  
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderBPinB) == HIGH) {   
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
//  Serial.println (encoderBPos);
}

void doEncoderBB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoderBPinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderBPinA) == HIGH) {  
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderBPinA) == LOW) {   
      encoderBPos = encoderBPos + 1;
    } 
    else {
      encoderBPos = encoderBPos - 1;
    }
  }
//  Serial.println (encoderBPos);
}

아래의 명령어를 4개의 terminal에 각각 입력한다.

$ roscore #1st Terminal
$ rosrun ros_motor ros_arduino_sub #2nd Terminal
$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard #3rd Terminal
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600 #4th Terminal

동작을 확인할 수 있다.

'한이음 > GPS 좌표 트래킹을 통한 자율주행 로봇' 카테고리의 다른 글

GPS-Waypoint example package 시도(미완성본) (0)	2022.05.01
ROS 삭제 (0)	2022.04.25
ROS 키 입력 받기 (teleop_twist) + 모터 구동 (0)	2022.03.09
OpenCR 사용하기 (0)	2022.02.05
IMU (Myahrs+) 사용하기 (0)	2022.02.03

현재글ROS OpenCR Motor 구동 / Encoder 값 받기

DKU MAZE 26th. STEP Line Tracer. BLDC Line Tracer.

Micro Mouse, MAZE, OpenCR, Melodic, Buck Converter, ubuntu, 라인 트레이서, Norman S. Nise, step, 디지털 시스템 원리 및 응용 제 10판, 전력변환회로설계, 전력전자공학(제4판), ros, TMS320F2809PZA, 18.04.6, 문운당, arduino, DC-DC Converter, Control Systems Engineering, 디지털 디자인 (Digital Design) 제 6판,

Today :
Yesterday :

일	월	화	수	목	금	토
		1	2	3	4	5
6	7	8	9	10	11	12
13	14	15	16	17	18	19
20	21	22	23	24	25	26
27	28	29	30	31

Why always me?! - 왜 맨날 나만 안돼?!