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
To download the files click on their names above the source, they should be saved without the "
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 (
SetMotorsinstead ofSetMotor1andSetMotor2) - 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
cd ~/picoborgrev./runDoodleBorg.shTo 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
