DoodleBorg - Controlled using a PS3 controller

This is the script we are using to control DoodleBorg using a PS3 controller.
It uses the same idea as the pbrJoystick.py example with the following modifications:
  • It sets up six boards to talk to
  • Each board is set to drive a single motor (SetMotors instead of SetMotor1 and SetMotor2)
  • Checking has been added to ensure the controller is in range and talking, otherwise the motors are turned off
  • The communication failsafes on each board have been enabled using SetCommsFailsafe(True)
  • The script waits for the controller to be attached, this way it can be started before the controller is connected
The bash script is used to run the script whilst hiding some irritating output, the whole thing is started using:
cd ~/picoborgrev
./runDoodleBorg.sh
To download the files click on their names above the source, they should be saved without the ".txt" at the end of the filename.

runDoodleBorg.sh

#!/bin/bash
./DoodleBorg.py > /dev/null

DoodleBorg.py

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

# Load library functions we want
import time
import os
import sys
import pygame
import PicoBorgRev

# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr

# Setup the PicoBorg Reverses
PBR = []
for i in range(6):
    nextPBR = PicoBorgRev.PicoBorgRev()
    nextPBR.i2cAddress = 0x10 + i
    nextPBR.Init()
    if not nextPBR.foundChip:
        boards = PicoBorgRev.ScanForPicoBorgReverse()
        if len(boards) == 0:
            print 'No PicoBorg Reverse found, check you are attached :)'
        else:
            print 'No PicoBorg Reverse at address %02X, but we did find %d boards:' % (nextPBR.i2cAddress, len(boards))
            for board in boards:
                print '    %02X (%d)' % (board, board)
            print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
            print 'PBR.i2cAddress = 0x%02X' % (boards[0])
        sys.exit()
    #nextPBR.SetEpoIgnore(True)                  # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
    nextPBR.ResetEpo()
    # Ensure the communications failsafe has been enabled!
    failsafe = False
    for i in range(5):
        nextPBR.SetCommsFailsafe(True)
        failsafe = nextPBR.GetCommsFailsafe()
        if failsafe:
            break
    if not failsafe:
        print 'Board %02X failed to report in failsafe mode!' % (nextPBR.i2cAddress)
        sys.exit()
    PBR.append(nextPBR)

# Settings for the joystick
axisUpDown = 1                          # Joystick axis to read for up / down position
axisUpDownInverted = False              # Set this to True if up and down appear to be swapped
axisLeftRight = 2                       # Joystick axis to read for left / right position
axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonResetEpo = 3                      # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving slowly whilst held (L2)
slowFactor = 0.5                        # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
buttonFastTurn = 9                      # Joystick button number for turning fast (R2)
interval = 0.02                         # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20                # Number of loops without any joystick events before announcing the joystick as out of range

# Setup pygame and wait for the joystick to become available
for PBRx in PBR:
    PBRx.MotorsOff()
os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
print 'Waiting for joystick... (press CTRL+C to abort)'
while True:
    try:
        try:
            pygame.joystick.init()
            # Attempt to setup the joystick
            if pygame.joystick.get_count() < 1:
                # No joystick attached, toggle the LED
                for PBRx in PBR:
                    PBRx.SetLed(not PBRx.GetLed())
                pygame.joystick.quit()
                time.sleep(0.5)
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick, toggle the LED
            for PBRx in PBR:
                PBRx.SetLed(not PBRx.GetLed())
            pygame.joystick.quit()
            time.sleep(0.5)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print '\nUser aborted'
        for PBRx in PBR:
            PBRx.SetLed(True)
        sys.exit()
print 'Joystick found'
joystick.init()
for PBRx in PBR:
    PBRx.SetLed(False)

try:
    print 'Press CTRL+C to quit'
    driveLeft = 0.0
    driveRight = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    loopsWithoutEvent = 0
    controllerLost = False
    # Loop indefinitely
    while running:
        # Get the latest events from the system
        hadEvent = False
        events = pygame.event.get()
        # Handle each event individually
        for event in events:
            if event.type == pygame.QUIT:
                # User exit
                running = False
            elif event.type == pygame.JOYBUTTONDOWN:
                # A button on the joystick just got pushed down
                hadEvent = True
            elif event.type == pygame.JOYAXISMOTION:
                # A joystick has been moved
                hadEvent = True
            if hadEvent:
                # Read axis positions (-1 to +1)
                if axisUpDownInverted:
                    upDown = -joystick.get_axis(axisUpDown)
                else:
                    upDown = joystick.get_axis(axisUpDown)
                if axisLeftRightInverted:
                    leftRight = -joystick.get_axis(axisLeftRight)
                else:
                    leftRight = joystick.get_axis(axisLeftRight)
                # Apply steering speeds
                if not joystick.get_button(buttonFastTurn):
                    leftRight *= 0.5
                # Determine the drive power levels
                driveLeft = -upDown
                driveRight = -upDown
                if leftRight < -0.05:
                    # Turning left
                    driveLeft *= 1.0 + (2.0 * leftRight)
                elif leftRight > 0.05:
                    # Turning right
                    driveRight *= 1.0 - (2.0 * leftRight)
                # Check for button presses
                if joystick.get_button(buttonResetEpo):
                    for PBRx in PBR:
                        PBRx.ResetEpo()
                if joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                # Set the motors to the new speeds
                PBR[0].SetMotors(-driveLeft)
                PBR[1].SetMotors(-driveLeft)
                PBR[2].SetMotors(-driveLeft)
                PBR[3].SetMotors(driveRight)
                PBR[4].SetMotors(driveRight)
                PBR[5].SetMotors(driveRight)
        if hadEvent:
            # Reset the controller lost counter
            loopsWithoutEvent = 0
            if controllerLost:
                # We had lost the controller, we have now found it again
                print 'Controller re-connected, move joystick to resume operation'
                for PBRx in PBR:
                    PBRx.SetLed(False)
                controllerLost = False
        elif controllerLost:
            # Controller has been lost, pulse the LED at a regular loop count
            loopsWithoutEvent += 1
            if (loopsWithoutEvent % (controllerLostLoops / 10)) == 0:
                for PBRx in PBR:
                    PBRx.SetLed(not PBRx.GetLed())
                # Attempt to reset the joystick module
                del joystick
                pygame.joystick.quit()
                pygame.joystick.init()
                if pygame.joystick.get_count() < 1:
                    # Controller has been disconnected, poll for reconnection
                    print 'Controller disconnected!'
                    while pygame.joystick.get_count() < 1:
                        time.sleep(interval * (controllerLostLoops / 10))
                        pygame.joystick.quit()
                        pygame.joystick.init()
                        for PBRx in PBR:
                            PBRx.SetLed(not PBRx.GetLed())
                # Grab the joystick again
                joystick = pygame.joystick.Joystick(0)
                joystick.init()
                continue
            # Skip to the next loop after the interval
            time.sleep(interval)
            continue
        else:
            # No events this loop, check if it has been too long since we saw an event
            loopsWithoutEvent += 1
            if loopsWithoutEvent > controllerLostLoops:
                # It has been too long, disable control!
                print 'Controller lost!'
                for PBRx in PBR:
                    PBRx.MotorsOff()
                    PBRx.SetLed(True)
                controllerLost = True
                # Skip to the next loop after the interval
                time.sleep(interval)
                continue
        # Change the LED to reflect the status of the EPO latch
        for PBRx in PBR:
            PBRx.SetLed(PBRx.GetEpo())
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    for PBRx in PBR:
        PBRx.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    print '\nUser shutdown'
    for PBRx in PBR:
        PBRx.MotorsOff()
except:
    # Unexpected error, shut down!
    e = sys.exc_info()[0]
    print
    print e
    print '\nUnexpected error, shutting down!'
    for PBRx in PBR:
        try:
            PBRx.MotorsOff()
        except:
            pass
for PBRx in PBR:
    try:
        PBRx.SetCommsFailsafe(False)
        PBRx.SetLed(True)
    except:
        pass
print
Subscribe to Comments for &quot;DoodleBorg - Controlled using a PS3 controller&quot;