TurtleBorg - Turn your PicoBorg based robot into a turtle

For those of you who did not grow up with these devices, a turtle is a robot consisting of two wheels, a coaster and a pen, sound familiar?

This was used as a learning tool when some of us were at school, as such it seems like a great idea to bring it back, with or without the pen.
The idea here was to implement some of the basic control commands which were used by Logo, a language which was often used with turtles, and make them drive a PicoBorg based robot instead, such as PiCy
The commands implemented differ slightly from those originally provided by Logo, but should allow budding roboteers to take their first steps easily, they are:

• FORWARD n
  Moves forward for n seconds (shorthand FD)
• LEFT n
  Moves left for n seconds (shorthand LT)
• RIGHT n
  Moves right for n seconds (shorthand RT)
• PENUP
  Removes the pen from the paper if attached (shorthand PU)
• PENDOWN
  Places the pen on the paper if attached (shorthand PD)

All of these commands accept fractional seconds, e.g. FD 0.25 would move forward for a quarter of a second.
The script has been designed so that it may be called directly from the terminal (or a Bash script) as well as being usable by importing it into another python script and calling the underlying functions.
If you are intending to use the pen functions you will want to attach a drawing device (pen, pencil, crayon) to a solenoid such that when powered the pen is touching the paper/floor, and when not powered it is clear.

The main script is TurtleBorg.py, a Python script which takes a command to execute or is imported into another Python script
Here's the code, you can download the TurtleBorg script file as text here
Save the text file on your pi as TurtleBorg.py
Make the script executable using
chmod +x TurtleBorg.py
and run using
sudo ./TurtleBorg.py command time_if_needed

To get you started we have also put up a pair of examples in Bash and Python which both move the turtle in a square, you may need to change the time on the left turns to make it square on your robot!
The Bash example is here and should be saved as TurtleBorgTest.sh on your pi.
The Python example is here and should be saved as TurtleBorgTest.py on your pi.
Make the test scripts executable using
chmod +x TurtleBorgTest.*
and run using
sudo ./TurtleBorgTest.py
sudo ./TurtleBorgTest.sh

TurtleBorg.py

#!/usr/bin/env python
# coding: Latin-1

# Load library functions we want
import sys
import time
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# Set which GPIO pins the drive outputs are connected to
DRIVE_1 = 4
DRIVE_2 = 18
DRIVE_3 = 8
DRIVE_4 = 7

# Set all of the drive pins as output pins
GPIO.setup(DRIVE_1, GPIO.OUT)
GPIO.setup(DRIVE_2, GPIO.OUT)
GPIO.setup(DRIVE_3, GPIO.OUT)
GPIO.setup(DRIVE_4, GPIO.OUT)

# Map of functions to drive pins
leftDrive = DRIVE_1                     # Drive number for left motor
rightDrive = DRIVE_4                    # Drive number for right motor
penDrive = DRIVE_3                      # Drive number for pen solenoid

# Functions for the robot to perform
def MoveForward(n):
    """Move forward for 'n' seconds"""
    GPIO.output(leftDrive, GPIO.HIGH)
    GPIO.output(rightDrive, GPIO.HIGH)
    time.sleep(n)
    GPIO.output(leftDrive, GPIO.LOW)
    GPIO.output(rightDrive, GPIO.LOW)

def MoveLeft(n):
    """Move left for 'n' seconds"""
    GPIO.output(leftDrive, GPIO.HIGH)
    GPIO.output(rightDrive, GPIO.LOW)
    time.sleep(n)
    GPIO.output(leftDrive, GPIO.LOW)

def MoveRight(n):
    """Move right for 'n' seconds"""
    GPIO.output(leftDrive, GPIO.LOW)
    GPIO.output(rightDrive, GPIO.HIGH)
    time.sleep(n)
    GPIO.output(rightDrive, GPIO.LOW)

def PenUp(n):
    """Lift the pen up"""
    GPIO.output(penDrive, GPIO.LOW)

def PenDown(n):
    """Place the pen down"""
    GPIO.output(penDrive, GPIO.HIGH)

def HelpMessage(n):
    """Display a list of available commands"""
    print ''
    print 'Available commands:'
    commands = dCommands.keys()
    commands.sort()
    for command in commands:
        print '% 10s - %s, %s' % (command, dCommands[command].func_name, dCommands[command].__doc__)
    print ''

# Map of command names to functions
dCommands = {
        'FORWARD':MoveForward,
        'FD':MoveForward,
        'LEFT':MoveLeft,
        'LT':MoveLeft,
        'RIGHT':MoveRight,
        'RT':MoveRight,
        'PENUP':PenUp,
        'PU':PenUp,
        'PENDOWN':PenDown,
        'PD':PenDown,
        'HELP':HelpMessage,
        '?':HelpMessage
        }

# If we have been run directly then look at command line
if __name__ == "__main__":
    # Process command
    if len(sys.argv) > 1:
        # Extract the command name and value (if there is any)
        command = sys.argv[1].upper()
        if len(sys.argv) > 2:
            sValue = sys.argv[2].upper()
        else:
            sValue = '0'
        try:
            fValue = float(sValue)
        except:
            fValue = 0.0
    
        # Select the appropriate function and call it
        if dCommands.has_key(command):
            dCommands[command](fValue)
        else:
            print 'Command "%s" not recognised' % (command)
            HelpMessage(fValue)
    else:
        # No command, display the help message
        print 'Usage: %s command [n]' % (sys.argv[0])
        HelpMessage(0)

TurtleBorgTest.sh

#!/bin/bash

/home/pi/TurtleBorg.py fd 1
/home/pi/TurtleBorg.py lt 0.85
/home/pi/TurtleBorg.py fd 1
/home/pi/TurtleBorg.py lt 0.85
/home/pi/TurtleBorg.py fd 1
/home/pi/TurtleBorg.py lt 0.85
/home/pi/TurtleBorg.py fd 1
/home/pi/TurtleBorg.py lt 0.85

TurtleBorgTest.py

#!/usr/bin/env python
# coding: Latin-1

import TurtleBorg

TurtleBorg.MoveForward(1)
TurtleBorg.MoveLeft(0.85)
TurtleBorg.MoveForward(1)
TurtleBorg.MoveLeft(0.85)
TurtleBorg.MoveForward(1)
TurtleBorg.MoveLeft(0.85)
TurtleBorg.MoveForward(1)
TurtleBorg.MoveLeft(0.85)