Arduino와 ROS로 DC 모터 x2 제어하기

- 개발 환경 : Rasberry Pi 4B (Ubuntu 18.04 / ROS melodic)

- 준비물 : 아두이노 메가, DC 모터(FIT0450) X 2, 모터 드라이버(JMOD-MOTOR-1)

- 참조 : https://atadiat.com/en/e-rosserial-arduino-introduction/

            https://bildr.org/2012/04/tb6612fng-arduino/

 

 

 

Step1. Raspberry Pi 4B에 Ubuntu 18.04 설치하기

https://do-9un6.tistory.com/2

Raspberry Pi 4B에 Ubuntu 18.04 설치하기

 

 

Step2. Ubuntu 18.04에 ROS melodic 설치하기

https://do-9un6.tistory.com/4

Ubuntu 18.04에 ROS melodic 설치하기

 

 

Step3. Rosserial package 설치

 

- 먼저, PC (Raspberry Pi / Ubuntu 18.04 / ROS melodic)에서 rosserial 메타패키지/클라이언트 패키지를 설치합니다.

$ sudo apt-get install ros-melodic-rosserial
$ sudo apt-get install ros-melodic-rosserial-arduino

 

- 아래의 링크에서 Linux용 아두이노 IDE를 설치합니다.

https://www.arduino.cc/en/software

Software

$ cd /Downloads/arduino-1.8.18
$ sudo ./install.sh
$ sudo apt install fonts-nanurm-coding
$ sudo apt-get update

 

 

- 아두이노 IDE를 실행시켜 File - Preference에 들어가서 Sketchbook location을 설정해줍니다.

Arduino board preference

 

- sketchbook location으로 가서 libraries 폴더가 없다면 생성해줍니다. 모든 Arduino 라이브러리를 보관하는 곳입니다.

 

 

- 터미널 창을 열어 libraries 폴더로 들어가 Arduino용 ros_lib 라이브러리를 생성합니다.

 

$ cd libraries
$ rosrun rosserial_arduino make_libraries.py .

 

- 아두이노 IDE에서 File - Examples 에 보면 ros_lib을 확인하실 수 있습니다.

 

 

Step4. 아두이노에 모터드라이버(TB6612FNG) 연결 및 제어

- 아래의 그림과 표를 참조하여 모터드라이버를 아두이노에 연결합니다.

Motor Driver	Arduino	Motor	모터 외부전력
A0UT	-	Motor A	-
BOUT	-	Motor B	-
STBY	Pin10	-	-
PWMA	Pin4	-	-
AIN1	Pin9	-	-
AIN2	Pin8	-	-
PWMB	Pin5	-	-
BIN1	Pin11	-	-
BIN2	Pin12	-	-
5V	VIN	-	-
GND	GND	-	-
VSEL(외부전력)	-	-	VOUT
-	GND	-	GND

 

 

- 아두이노와 라즈베리파이를 연결 후 아래의 코드를 업로드합니다.

L298N.ino

0.00MB

//Code Authors:
//* Ahmed A. Radwan (author)
//* Maisa Jazba

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

#define EN_L 4
#define IN1_L 9
#define IN2_L 8

#define EN_R 5
#define IN1_R 11
#define IN2_R 12



double w_r = 0, w_l = 0;

//wheel_rad is the wheel radius ,wheel_sep is
double wheel_rad = 0.0325, wheel_sep = 0.295;


ros::NodeHandle nh;
int lowSpeed = 200;
int highSpeed = 50;
double speed_ang = 0, speed_lin = 0;

void messageCb( const geometry_msgs::Twist& msg) {
  speed_ang = msg.angular.z;
  speed_lin = msg.linear.x;
  w_r = (speed_lin / wheel_rad) + ((speed_ang * wheel_sep) / (2.0 * wheel_rad));
  w_l = (speed_lin / wheel_rad) - ((speed_ang * wheel_sep) / (2.0 * wheel_rad));
}


ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb );
void Motors_init();
void MotorL(int Pulse_Width1);
void MotorR(int Pulse_Width2);

void setup() {

  Motors_init();

  nh.initNode();

  nh.subscribe(sub);

}


void loop() {
  MotorL(w_l * 10);

  MotorR(w_r * 10);

  nh.spinOnce();

}

void Motors_init() {

  pinMode(EN_L, OUTPUT);

  pinMode(EN_R, OUTPUT);

  pinMode(IN1_L, OUTPUT);

  pinMode(IN2_L, OUTPUT);

  pinMode(IN1_R, OUTPUT);

  pinMode(IN2_R, OUTPUT);

  digitalWrite(EN_L, LOW);

  digitalWrite(EN_R, LOW);

  digitalWrite(IN1_L, LOW);

  digitalWrite(IN2_L, LOW);

  digitalWrite(IN1_R, LOW);

  digitalWrite(IN2_R, LOW);

}

void MotorL(int Pulse_Width1) {
  if (Pulse_Width1 > 0) {

    analogWrite(EN_L, Pulse_Width1);

    digitalWrite(IN1_L, HIGH);

    digitalWrite(IN2_L, LOW);

  }

  if (Pulse_Width1 < 0) {

    Pulse_Width1 = abs(Pulse_Width1);

    analogWrite(EN_L, Pulse_Width1);

    digitalWrite(IN1_L, LOW);

    digitalWrite(IN2_L, HIGH);

  }

  if (Pulse_Width1 == 0) {

    analogWrite(EN_L, Pulse_Width1);

    digitalWrite(IN1_L, LOW);

    digitalWrite(IN2_L, LOW);

  }
}


void MotorR(int Pulse_Width2) {


  if (Pulse_Width2 > 0) {

    analogWrite(EN_R, Pulse_Width2);

    digitalWrite(IN1_R, LOW);

    digitalWrite(IN2_R, HIGH);

  }

  if (Pulse_Width2 < 0) {

    Pulse_Width2 = abs(Pulse_Width2);

    analogWrite(EN_R, Pulse_Width2);

    digitalWrite(IN1_R, HIGH);

    digitalWrite(IN2_R, LOW);

  }

  if (Pulse_Width2 == 0) {

    analogWrite(EN_R, Pulse_Width2);

    digitalWrite(IN1_R, LOW);

    digitalWrite(IN2_R, LOW);

  }

}

 

imu & motor Arduino_code

motor_imu.ino

0.01MB

- 라즈베리파이에서 터미널 창을 열어 아래의 코드를 입력하여 실행합니다.

$ roscore
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600

 

- 키보드로 모터를 제어하기 위해 아래의 코드를 입력해줍니다.

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

모터 제어 영상.mp4

6.43MB