Tuesday, May 27, 2014

My first working robot, It’s Alive – Part 3

In Part 1 and Part 2 of this series I showed how to connect the BeagleBone Black to the Rover 5 Tracked Chassis, using the Rover 5 motor controller and also wrote a Python module to help control the robot.  In this third and final post I will show how I connected the Rocketfish MicroBluetooth Adapter to the BeagleBone Black.  I will then show my Bluetooth client and server scripts, written in Python, that lets me control my robot remotely to make it truly mobile.  Also added one more video of my robot in action.



Adding the Rocketfish Bluetooth adapter
Remember to always turn the power off on the BeagleBone prior to plugging (or unplugging) anything into it.  The Rocketfish Bluetooth adapter simply plugs into a USB port.  If you are using a USB hub you can plug it into the hub or directly into the BeagleBone if you do not have a hub.  Once the Bluetooth adapter is plugged in, power on the BeagleBone.

When the BeagleBone comes up, log into the BeagleBone and run the following command:

lsusb

You should see a line similar to this if the BeagleBone recognizes the RocketFish Bluetooth adapter:

Bus 001 Device 009: ID 0461:4d75 Primax Electronics, Ltd Rocketfish RF-FLBTAD Bluetooth Adapter

When I saw this line, my first thought was that Angstrom Linux recognized my adapter and I was good to go, but that is not the case.  By default, Bluetooth is not enabled with the default BeagleBone Black Angstrom Linux distribution.

Enable Bluetooth support in Angstrom Linux
To enable Bluetooth support we will need to edit a configuration files, enable the Bluetooth service and then start the Bluetooth service.  In your favorite editor (I prefer vi) open the /var/lib/connman/settings file.  You should see something similar to this:

[global]
OfflineMode=false

[Wired]
Enable=true

[WiFi]
Enable=true

[Bluetooth]
Enable=false

Notice that Bluetooth is disabled.  To enable Bluetooth, change the Enable=false line to Enabled=true under the [Bluetooth] heading and then save the file.  Now we will want to enable and start the Bluetooth service.  To do this, run the following commands:

systemctl enable bluetooth.service
systemctl start bluetooth.service

Next restart the BeagleBone Black and the Bluetooth service should be running.  We now need to pair our BeagleBone with the device that we will use to control it.

I have the ability to connect my BeagleBone Black to a USB keyboard/touchpad and monitor so I paired my BeagleBone Black to my laptop from the GUI interface.  How you pair your devices will vary depending on what device you pair your BeagleBone with.  You can initiate the pairing from either the BeagleBone or the other device. 

If you do not have your BeagleBone black connected to a Monitor, there are command line options but I was unable to get any of them to pair properly.  This post (http://pfalcon-oe.blogspot.com/2011/12/pairing-bluetooth-devices-from-command.html) seems to be the closes one but it still requires a GUI to set the PIN.  If anyone is able to get the BeagleBone to pair purely from command line, please post the code (or a link) in the comment section below for others.  Another option would be to install and configure VNC on the BeagleBone Black so you can access the Desktop remotely. 

Using Python to connect over Bluetooth
To begin, we need to install a couple of packages, so connect your BeagleBone to the Internet and run the following commands:

opkg install bluez4 bluez4-dev
pip install pybluez

This will install bluez and the bluez development libraries.  We then installed the pybluez python module.  The pybluez module is a python wrapper around the system Bluetooth resources that allows us to easy creates python scripts that communicate over Bluetooth.  

I used the rfcomm-server.py and rfcomm-client.py examples from the pybluez documentation as the base for my rover 5 client/server scripts.  You can take a look at the examples and the documentation on how the Bluetooth communication works. 

Rover 5 rfcomm server
Lets begin by writing the server script that will run on the BeagleBone Black and listen for incoming Bluetooth connections.  This script must be located in a directory that also contains the rover_beaglebone.py module that we wrote in part 2 of this series. Here is the code for the rover5-rfcomm-server.py script:

from bluetooth import *
import rover_beaglebone as rover
import time

#open Bluetooth socket and listen for connections
server_sock=BluetoothSocket( RFCOMM )
server_sock.bind(("",PORT_ANY))
server_sock.listen(2)

port = server_sock.getsockname()[1]

uuid = "34B1CF4D-1069-4AD6-89B6-E161D79BE4D8"

#advertise our Bluetooth service so other devices can find it
advertise_service( server_sock, "BeagleBoneService",
                   service_id = uuid,
                   service_classes = [ uuid, SERIAL_PORT_CLASS ],
                   profiles = [ SERIAL_PORT_PROFILE ]
                    )
                  
print("Waiting for connection on RFCOMM channel %d" % port)

#wait for an incoming connection
client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)

#initialize rover
rover.init_rover()

try:
                 #wait for incoming commands and execute appropriate function
    while True:
        data = client_sock.recv(1024)
        if len(data) == 0: break
        print("received [%s]" % data)
             
        if data == 'u':
            rover.all_full_speed()
       
        elif data == 'j':
            rover.all_stop()
       
        elif data == 'i':
            rover.increase_speed()
       
        elif data == 'k':
            rover.decrease_speed()
       
        elif data == 'o':
            rover.forward_dir()
           
        elif data == 'l':
            rover.reverse_dir()
           
        elif data == 'z':
            rover.spin_left(40)
           
        elif data == 'x':
            rover.spin_right(40)
             
         elif data == 'c':
              rover.spin_right(40)
        else:
            break
       
                           
except IOError:
    pass

#disconnection socket and stop robot
print("disconnected")
rover.stop_rover()

client_sock.close()
server_sock.close()
print("all done")

This script begins by setting up a Bluetooth socket to listen on and also advertises the service using SDP.  Once a connection is made it then listens for incoming text and executes the appropriate functions depending on the incoming text.  If there is no function tied to the incoming text, the script disconnects the socket and stops the robot.

Now we need to configure this script to start up automatically when the BeagleBone boots up since we will not have the robot connected to anything (network or another computer) when we control it remotely.  This means we will want to configure the script to run as a service when the BeagleBone first boots up.  To do this we will create a file called rover5.service in the /lib/systemd/system directory and put the following code in it (Note:  You will need to change the path of the WorkingDirectory and ExecStart variables to match the path to your rover5-rfcomm-server.py file.

[Unit]
Description=Rover 5 Bluetooth Service

[Service]
WorkingDirectory=/home/root/bluetooth
ExecStart=/usr/bin/python /home/root/bluetooth/rover5-rfcomm-server.py
SyslogIdentifier=rover5
Restart=on-failure
RestartSec=2

[Install]
WantedBy=multi-user.target

Now run the following commands to enable and start the Rover5 service:

systemctl enable rover5.service
systemctl start rover5.service

The service is now started and will start each time the BeagleBone starts up.

Rover 5 rfcomm client
Now that we have our Rover 5 Bluetooth server created and started, let write our client application that will run on the device that we will use to control the robot.  Create a file called rover5-rfcomm-client.py and add the following code:

from bluetooth import *
import sys, tty, termios

if sys.version < '3':
    input = raw_input

addr = None

if len(sys.argv) < 2:
    print("no device specified.  Searching all nearby bluetooth devices for")
    print("the BeagleBoneService")
else:
    addr = sys.argv[1]
    print("Searching for BeagleBoneServic e on %s" % addr)

# search for the Rover 5 service
uuid = "34B1CF4D-1069-4AD6-89B6-E161D79BE4D8"
service_matches = find_service( uuid = uuid, address = addr )

if len(service_matches) == 0:
    print("couldn't find the Rover 5 service =(")
    sys.exit(0)

#If we found a match
first_match = service_matches[0]
port = first_match["port"]
name = first_match["name"]
host = first_match["host"]

print("connecting to \"%s\" on %s" % (name, host))

# Create the client socket
sock=BluetoothSocket( RFCOMM )
sock.connect((host, port))
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
tty.setraw(sys.stdin.fileno())

#we are connected, now we can type commands
print("connected.  type stuff")
r=True
while r:
    data = sys.stdin.read(1)
    if len(data) == 0: break
    sock.send(data)
    if data == 'x':
        r=False

sock.close()

If your run this scripts, it will search for the Rover 5 service using the UUID as the key.  If the service is found it will attempt to connect.  If the connection is successful it will display the message “connected.  type stuff”.  At this point we can begin typing commands to control the robot.  Before we do too much with this, we will want to disconnect the rover from external power supplies, monitors, USB hubs…. and make it truly mobile.

Making the robot mobile
The final piece of the puzzle is to free our robot of external dependencies like USB hubs, power source, network cables… that limit the range that we can go.  For this we will need to get an external power source.  I am using the portable external cell phone charger from EasyAcc.

To make the robot mobile, begin by powering off the BeagleBone and unplug all cables that connect the BeagleBone to a resource external to the robot like power, network, USB hub… (Note:  leave the BeagleBone connected to the motor controller).  The Beaglebone black’s connections will look like this:



Now plug the Rocketfish Bluetooth adapter into the USB port on your BeagleBone Black and run the USB power connector on the BeagleBone Black to the external cell phone charger as pictured below:



If everything is connected correctly the robot should power up.  Once the robot is powered up; run the client script by running this command “python rover5-rfcomm-client.py”.  I normally give the BeagleBone about 30 seconds to power up before I run the client script.

If all is well, you should see the prompt “connected.  type stuff”.  At this point you can enter the any of the one character commands defined in the rover5-rfcomm-server.py script to make your robot go.

This ends the three part series on my first working robot.  Now it is time to expand the robot and/or experiment some more with the BeagleBone Black.  Either way I will document what I am learning here.  I hope this series of posts helped/inspired others.

Monday, May 26, 2014

My first working robot, It’s Alive – Part 2

In my last post “My first working robot, It’s Alive”, I showed how to connect the BeagleBone Black to the Rover 5 Tracked Chassis, using the Rover 5 motor controller, and ran a quick test to verify everything was working properly.  In this post I will share the code for the Python module that I wrote to help control the robot and share some sample scripts that demonstrate how to use the module.  I also included a new video and images of my robot since I added an expansion plate and mounted the BeagleBone and the Motor Driver Controller (no more cardboard).

Video and images

Video:

Back of Robot:


Side of Robot:


Top of Robot:




My BeagleBone Black Rover 5 module
In my last post I ended it with a simple test script that verified everything was properly connected.  If we really want to write applications in Python to control our robot, we need to start off by creating a good Python module that defines the basic movements of our robot like setting the speed of the tracks, changing direction, keeping track of the current speed and spinning.  This section shows the rover_beaglebone.py module that I wrote and also explains the functions that it exposes.

Here is the code for the rover_beaglebone.py module that I wrote:

import Adafruit_BBIO.PWM as PWM
import Adafruit_BBIO.GPIO as GPIO
import time

PIN_SPEED_RIGHT = "P8_13"
PIN_SPEED_LEFT = "P8_19"
PIN_DIR_LEFT = "P8_14"
PIN_DIR_RIGHT = "P8_16"
MAX_SPEED=100
MIN_SPEED=25
CHANGE_RATE=5
STOP_SPEED=0
FORWARD_DIR=1
REVERSE_DIR=0

current_speed_right=STOP_SPEED
current_speed_left=STOP_SPEED
current_dir_right = FORWARD_DIR
current_dir_left = FORWARD_DIR

#init rover to prepare it for movement
def init_rover():
    PWM.start(PIN_SPEED_RIGHT,0)
    PWM.start(PIN_SPEED_LEFT,0)
    GPIO.setup(PIN_DIR_RIGHT, GPIO.OUT)
    GPIO.setup(PIN_DIR_LEFT, GPIO.OUT)
   
def stop_rover():
    all_stop()
   
#Helper functions to verify the speed stays in acceptable range
def check_speed(speed):
    if speed < MIN_SPEED and speed != STOP_SPEED:
        speed=MIN_SPEED
    if speed > MAX_SPEED:
        speed = MAX_SPEED
    return speed

def fastest_speed():
    global current_speed_right, current_speed_left
    ret_speed = current_speed_right
    if current_speed_left > current_speed_right:
        ret_speed = current_speed_left
    return ret_speed

def slowest_speed():
    global current_speed_right, current_speed_left
    ret_speed = current_speed_right
    if current_speed_left < current_speed_left:
        ret_speed = current_speed_left
    return ret_speed;
   
#Sets speed
def set_right_speed(speed):
    global current_speed_right, current_speed_left
    new_speed = check_speed(speed)
    PWM.set_duty_cycle(PIN_SPEED_RIGHT,new_speed)
    current_speed_right = new_speed
 
def set_left_speed(speed):
    global current_speed_right, current_speed_left
    new_speed = check_speed(speed)
    PWM.set_duty_cycle(PIN_SPEED_LEFT,new_speed)
    current_speed_left = new_speed
   
def set_speed(speed):
    set_left_speed(speed)
    set_right_speed(speed)
 

#increase speed
def increase_right_speed():
    global current_speed_right, current_speed_left
    set_right_speed(current_speed_right + CHANGE_RATE)

def increase_left_speed():
    global current_speed_right, current_speed_left
    set_left_speed(current_speed_left + CHANGE_RATE)
   
def increase_speed():
    increase_right_speed()
    increase_left_speed()
   
#decrease speed
def decrease_right_speed():
    global current_speed_right, current_speed_left
    set_right_speed(current_speed_right - CHANGE_RATE)
   
def decrease_left_speed():
    global current_speed_right, current_speed_left
    set_left_speed(current_speed_left -CHANGE_RATE)
   
def decrease_speed():
    decrease_right_speed()
    decrease_left_speed()

#set direction forward
def forward_right_dir():
    global current_dir_right, current_dir_left
    if current_dir_right == REVERSE_DIR:
        all_stop()
    GPIO.output(PIN_DIR_RIGHT, GPIO.LOW)
    current_dir_right = FORWARD_DIR
   
def forward_left_dir():
    global current_dir_right, current_dir_left
    if current_dir_left == REVERSE_DIR:
        all_stop()
    GPIO.output(PIN_DIR_LEFT, GPIO.LOW)
    current_dir_left = FORWARD_DIR

def forward_dir():
    forward_right_dir()
    forward_left_dir()

#set reverse direction
def reverse_right_dir():
    global current_dir_right, current_dir_left
    if current_dir_right == FORWARD_DIR:
        all_stop()
    GPIO.output(PIN_DIR_LEFT, GPIO.HIGH)
    current_dir_right = REVERSE_DIR
   
def reverse_left_dir():
    global current_dir_right, current_dir_left
    if current_dir_left == FORWARD_DIR:
        all_stop()
    GPIO.output(PIN_DIR_RIGHT, GPIO.HIGH) 
    current_dir_left = REVERSE_DIR

def reverse_dir():
    reverse_right_dir()
    reverse_left_dir()

#stop rover  
def stop_left():
    set_left_speed(STOP_SPEED)
   
def stop_right():
    set_right_speed(STOP_SPEED)
   
def all_stop():
    stop_left()
    stop_right()

#Full speed
def full_speed_left():
    set_left_speed(MAX_SPEED)

def full_speed_right():
    set_right_speed(MAX_SPEED)
   
def all_full_speed():
    full_speed_left()
    full_speed_right()

#spin rover
def spin_right(speed):
    all_stop()
    forward_dir()
    forward_right_dir()
    reverse_left_dir()
    set_right_speed(speed)
    set_left_speed(speed)

def spin_left(speed):
    all_stop()
    forward_dir()
    forward_left_dir()
    reverse_right_dir()
    set_right_speed(speed)
    set_left_speed(speed) 

This module begins by importing the Adafruit modules that we discussed in the last post.  I use these modules to control the BeagleBone Black’s expansion ports. 

We then define a number of constants.  These constants are:
PIN_SPEED_RIGHT:  Defines that pin 13 of the P8 expansion port controls the speed of the right track.
PIN_SPEED_LEFT:  Defines that pin 19 of the P8 expansion port controls the speed of the left track.
PIN_DIR_LEFT:  Defines that pin 14 of the P8 expansion port controls the direction of the left track.
PIN_DIR_RIGHT:  Defines that pin 16 of the P8 expansion port controls the direction of the right track.
MAX_SPEED:  Is the maximum speed that we can set for the robot
MIN_SPEED:  Is the minimum speed that we can set for the robot.  Anything below 25 and the track does not move.
CHANGE_RATE:  Is the rate that we increase or decrease the speed by.
STOP_SPEED:  Is the speed that we set when we want to stop the robot
FORWARD_DIR:  Defines what forward direction is
REVERSE_DIR:  Defines what reverse direction is.
After we define our constants, we set the current speed and current direction global variables.  These variables, obviously, keep track of the current speed and direction of our robot.

The first function is pretty important because it initialized the pins on the expansion ports that we will use to control our robot.  Notice that we use the PWM.start() function for the PIN_SPEED_RIGHT and PIN_SPEED_LEFT pins to initialize them as PWM pins and we use the GPIO.setup() function for the PIN_DIR_RIGHT and PIN_DIR_LEFT pins to initialize them as digitial I/O pins.  We need to call the init_rover() function prior to calling any other function in this module.  Here is a list of the remaining functions and what they do:
stop_rover():  Stops the rover
check_speed(speed):  Verifies that the speed is within the acceptable ranges.  This function returns the speed that was passed in if it was within the acceptable range otherwise it returns the MAX_SPEED or MIN_SPEED depending on if the speed that was passed in was too high or too low.
fastest_speed():  Returns the speed of the track that is currently going the fastest.
slowest_speed():  Returns the speed of the track that is currently going the slowest.

set_right_speed():  Sets the speed of the right track.
set_left_speed():  Sets the speed of the left track.
set_speed():  Sets the speed of both tracks.

increase_right_speed():  Increases the speed of the right track.
increase_left_speed():  Increases the speed of the left track.
increase_speed():  increases the speed of both tracks.

decrease_right_speed():  Decreases the speed of the right track.
decrease_left_speed():  Decrease the speed of the left track.
decrease_speed():  Decrease the speed of both tracks.

forward_right_dir():  Sets the direction of the right track to forward.
forward_left_dir():  Sets the direction of the left track to forward.
forward_dir():  Sets the direction of both tracks to forward.

reverse_right_dir():  Sets the direction of the right track to reverse.
reverse_left_dir():  Sets the direction of the left track to reverse.
reverse_dir():  Sets the direction of both tracks to reverse.

stop_left():  Stops the left track.
stop_right():  Stops the right track.
all_stop():  Stops both tracks.

full_speed_left():  Sets the left track to full speed.
full_speed_right():  Sets the right track to full speed.
all_full_speed():  Sets both tracks to full speed.

spin_right(speed):  Spins the robot in the right direction at the speed passed in.
spin_left(speed):  Spins the robot in the left direction at the speed passed in.

Now lets write a simple test script to test the module.  Create a file called module_test.py in the same directory that contains the rover_beaglebone.py module and add the following code:
import rover_beaglebone as rover
import sys, time

rover.init_rover()

rover.reverse_dir()
rover.set_speed(40)

time.sleep(1)

rover.all_stop()
rover.forward_dir()
rover.set_speed(50)
time.sleep(1)

rover.spin_left(40)
time.sleep(1)

rover.all_stop()
Save the script and run it using the following command “python module_test.py”.  If everything is working correctly, the rover should go backwards for one second, stop and go forward for 1 second, and finally spin left for one second before stopping.

Demo Script
In the video at the beginning of this post, I used the rover5-rfcomm-server.py module to orchestrate the robots movement.  The code below is the code that I used to create the video and is a good example of what you can do with the rover5-rfcomm-server.py module.

import rover_beaglebone as rover
import time

rover.init_rover()
rover.reverse_dir()
rover.set_speed(80)
time.sleep(3.5)
rover.all_stop()

time.sleep(2)
mytime=2

rover.forward_dir()
rover.set_speed(80)
time.sleep(4.5)
rover.all_stop()

rover.spin_left(30)
time.sleep(mytime)
rover.set_speed(40)
time.sleep(mytime)
rover.set_speed(50)
time.sleep(mytime)
rover.set_speed(60)
time.sleep(mytime)
rover.set_speed(70)
time.sleep(mytime)
rover.set_speed(80)
time.sleep(mytime)
rover.set_speed(90)
time.sleep(2.9)

rover.all_stop()

rover.reverse_dir()
rover.set_speed(90)
time.sleep(15)
rover.all_stop()

Third and final post in this series
In the next couple of days I will be posting the third and final installment in this series.  This post will show how I added a USB Bluetooth adapter and added an external power source to free the robot of the need to connect to my computer or a power outlet.  I will also post my Python code that allows me to control the robot 

Part 1:  http://myroboticadventure.blogspot.com/2014/05/my-first-working-robot-its-alive.html
Part 3:  http://myroboticadventure.blogspot.com/2014/05/my-first-working-robot-its-alive-part-3.html