Make a ROS package to communicate with Arduino to control motors - mktk1117/six_wheel_robot GitHub Wiki
Arduino Code
Arduino has to receive messages from raspberry pi and decode it. Move to the directory where you want to work with Arduino. Then, download the code.
$ cd /path/to/your/workplace
$ wget https://raw.githubusercontent.com/mktk1117/six_wheel_robot/master/Arduino/motor_driver/motor_driver.ino
or you can copy and paste from here.
Then, write this code to the Arduino.
ROS Code
Then, Raspberry Pi should receive the control command from /cmd_vel
and translate it to the rotation signal of each motor and send it to Arduino.
If you are not familiar with cmd_vel
please see Understanding ROS Topics
Please move to catkin_ws/src
and download and make the code.
pi@raspberry$ cd ~/catkin_ws/src
pi@raspberry$ svn export https://github.com/mktk1117/six_wheel_robot/trunk/ROS/motor_driver
pi@raspberry$ cd ~/catkin_ws
pi@raspberry$ catkin_make
The python script would be like this
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import serial
class driver:
def __init__(self):
# init ros
rospy.init_node('car_driver', anonymous=True)
rospy.Subscriber('/cmd_vel', Twist, self.get_cmd_vel)
self.ser = serial.Serial('/dev/ttyUSB0', 115200)
self.get_arduino_message()
# get cmd_vel message, and get linear velocity and angular velocity
def get_cmd_vel(self, data):
x = data.linear.x
angular = data.angular.z
self.send_cmd_to_arduino(x, angular)
# translate x, and angular velocity to PWM signal of each wheels, and send to arduino
def send_cmd_to_arduino(self, x, angular):
# calculate right and left wheels' signal
right = int((x + angular) * 50)
left = int((x - angular) * 50)
# format for arduino
message = "{},{},{},{},{},{}*".format(right, left, right, left, right, left)
print message
# send by serial
self.ser.write(message)
# receive serial text from arduino and publish it to '/arduino' message
def get_arduino_message(self):
pub = rospy.Publisher('arduino', String, queue_size=10)
r = rospy.Rate(10)
while not rospy.is_shutdown():
message = self.ser.readline()
pub.publish(message)
r.sleep()
if __name__ == '__main__':
try:
d = driver()
except rospy.ROSInterruptException:
pass
This script receives messages from cmd_vel
and translates them to the rotation signal of each motor.
Then, it sends to Arduino. At the same time, it receives the messages from Arduino and publishes to /arduino
.
Run the program
Connect Raspberry Pi and Arduino like Communication between Arduino and Raspberry Pi
And don't forget to prepare Communication between Raspberry Pi and PC
1. roscore
First, run the roscore at Raspberry Pi
pi@raspberry$ roscore
2. Run motor_driver node
Second, login with another terminal with ssh and run the motor_driver node.
pi@raspberry$ rosrun motor_driver motor_driver.py
3. Check from your PC
If Raspberry Pi is successfully connected with PC, rostopic can be seen from your PC
$ rostopic list
It should be like this,
/arduino
/cmd_vel
/rosout
/rosout_agg
Let's check the arduino
message. Do command $ rostopic echo /arduino
It should be like this
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
---
data: 0,0,0,0,0,0
4. Control from your PC
Then, let's control the six-wheel car from your PC. At your PC, run
$rosrun teleop_twist_keyboard teleop_twist_keyboard.py
If it is not installed in your PC, install it with
$sudo apt-get install ros-indigo-teleop-twist-keyboard
Control
Look the instruction on screen
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
anything else : stop
CTRL-C to quit