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 (
SetMotors
instead ofSetMotor1
andSetMotor2
) - 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.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
1 2 | #!/bin/bash . /DoodleBorg .py > /dev/null |
DoodleBorg.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 | #!/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 |