#!/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 slowFactor = 0.5 # Speed to slow motors to, e.g. 0.5 would be half speed / voltage 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) # Set the motor maximum speed 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