JoyBorgPwm
Uses PiCy.
Allows a joystick / gamepad to drive PiCy at controllable speeds.
The default settings are setup for a PS3 remote with the following controls:
See JoyBorg for instructions on setting up other joysticks.
Here's the code, you can download the JoyBorgPwm script file as text here
Save the text file on your Raspberry Pi as JoyBorgPwm.py
You may need to install WiringPi2, do this using
Make the script executable using
and run using
Allows a joystick / gamepad to drive PiCy at controllable speeds.
The default settings are setup for a PS3 remote with the following controls:
- Left stick controls speed
- Right stick controls turning
- L2 enables drive slowly mode
See JoyBorg for instructions on setting up other joysticks.
Here's the code, you can download the JoyBorgPwm script file as text here
Save the text file on your Raspberry Pi as JoyBorgPwm.py
You may need to install WiringPi2, do this using
sudo easy_install wiringpi2Make the script executable using
chmod +x JoyBorgPwm.pyand run using
sudo bash -c './JoyBorgPwm.py > /dev/null'
#!/usr/bin/env python
# coding: Latin-1
# Load library functions we want
import time
import os
import sys
import pygame
import wiringpi2 as wiringpi
# 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
# Set which GPIO pins the drive outputs are connected to
DRIVE_1 = 7 # GPIO 4
DRIVE_2 = 1 # GPIO 18
DRIVE_3 = 10 # GPIO 8
DRIVE_4 = 11 # GPIO 7
# Setup the software PWMs for 0 to 100
wiringpi.wiringPiSetup()
wiringpi.softPwmCreate(DRIVE_1, 0, 100)
wiringpi.softPwmWrite(DRIVE_1, 0)
wiringpi.softPwmCreate(DRIVE_2, 0, 100)
wiringpi.softPwmWrite(DRIVE_2, 0)
wiringpi.softPwmCreate(DRIVE_3, 0, 100)
wiringpi.softPwmWrite(DRIVE_3, 0)
wiringpi.softPwmCreate(DRIVE_4, 0, 100)
wiringpi.softPwmWrite(DRIVE_4, 0)
# Settings for the joystick
leftDrive = DRIVE_1 # Drive number for left motor
rightDrive = DRIVE_4 # Drive number for right motor
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
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
# Function to set all drives off
def MotorsOff():
wiringpi.softPwmWrite(DRIVE_1, 0)
wiringpi.softPwmWrite(DRIVE_2, 0)
wiringpi.softPwmWrite(DRIVE_3, 0)
wiringpi.softPwmWrite(DRIVE_4, 0)
# Setup pygame and wait for the joystick to become available
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, wait
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, wait
pygame.joystick.quit()
time.sleep(0.5)
except KeyboardInterrupt:
# CTRL+C exit, give up
print '\nUser aborted'
sys.exit()
print 'Joystick found'
joystick.init()
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(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
# Set the motors to the new speeds
driveLeft = int(driveLeft * 100)
if driveLeft < 0:
# Cannot reverse, go to stopped
driveLeft = 0
elif driveLeft > 100:
# Beyond full speed, go to full speed
driveLeft = 100
driveRight = int(driveRight * 100)
if driveRight < 0:
# Cannot reverse, go to stopped
driveRight = 0
elif driveRight > 100:
# Beyond full speed, go to full speed
driveRight = 100
wiringpi.softPwmWrite(leftDrive, driveLeft)
wiringpi.softPwmWrite(rightDrive, 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'
controllerLost = False
elif controllerLost:
# Controller has been lost, re-initialise joystick module at a regular loop count
loopsWithoutEvent += 1
if (loopsWithoutEvent % (controllerLostLoops / 10)) == 0:
# 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()
# 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!'
MotorsOff()
controllerLost = True
# Skip to the next loop after the interval
time.sleep(interval)
continue
# Wait for the interval period
time.sleep(interval)
# Disable all drives
MotorsOff()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
print '\nUser shutdown'
MotorsOff()
except:
# Unexpected error, shut down!
e = sys.exc_info()[0]
print
print e
print '\nUnexpected error, shutting down!'
MotorsOff()
print
