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:
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

No comments:

Post a Comment