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 (
- 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
- 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:
To download the files click on their names above the source, they should be saved without the "
.txt" at the end of the filename.
#!/bin/bash ./DoodleBorg.py > /dev/null
#!/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) 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 ' User 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.SetMotors(-driveLeft) PBR.SetMotors(-driveLeft) PBR.SetMotors(-driveLeft) PBR.SetMotors(driveRight) PBR.SetMotors(driveRight) PBR.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 ' User shutdown' for PBRx in PBR: PBRx.MotorsOff() except: # Unexpected error, shut down! e = sys.exc_info() print print e print ' Unexpected 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