# Load library functions we want from __future__ import print_function from diablo import * from time import sleep from os import environ from sys import exit, stdout, stderr, exc_info import pygame import RPi.GPIO as GPIO GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW) # Power settings voltageIn = 24.0 # Total battery voltage to the Diablo voltageOut = 18.5 # Maximum motor voltage # Setup the power limits if voltageOut > voltageIn: maxPower = 1.0 else: maxPower = voltageOut / float(voltageIn) # Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame stdout = stderr # Setup the left Diablo DIABLO1 = Diablo() DIABLO1.i2cAddress = 38 DIABLO1.Init() if not DIABLO1.foundChip: boards = ScanForDiablo() if len(boards) == 0: print('No Diablo found, check you are attached :)') else: print('No Diablo at address %02X, but we did find boards:' % (DIABLO1.i2cAddress)) for board in boards: print(' %02X (%d)' % (board, board)) print('If you need to change the I2C address change the set-up line so it is correct, e.g.') print('DIABLO1.i2cAddress = 0x%02X' % (boards[0])) exit() #DIABLO1.SetEpoIgnore(True) # Uncomment to disable EPO latch, needed if you do not have a switch / jumper DIABLO1.ResetEpo() # Setup the right Diablo DIABLO2 = Diablo() DIABLO2.i2cAddress = 55 DIABLO2.Init() if not DIABLO2.foundChip: boards = ScanForDiablo() if len(boards) == 0: print('No Diablo found, check you are attached :)') else: print('No Diablo at address %02X, but we did find boards:' % (DIABLO2.i2cAddress)) for board in boards: print(' %02X (%d)' % (board, board)) print('If you need to change the I2C address change the set-up line so it is correct, e.g.') print('DIABLO2.i2cAddress = 0x%02X' % (boards[0])) exit() #DIABLO2.SetEpoIgnore(True) # Uncomment to disable EPO latch, needed if you do not have a switch / jumper DIABLO2.ResetEpo() # 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 = 3 # Joystick axis to read for left / right position axisLeftRightInverted = False # Set this to True if left and right appear to be swapped buttonResetEpo = 16 # Joystick button number to perform an EPO reset (Start) buttonSlow = 8 # Joystick button number for driving fast whilst held (L2) slowFactor = 1.0 # Speed to slow to when the drive fast button is not held, e.g. 0.5 would be half speed buttonFastTurn = 7 # Joystick button number for turning fast (R2) buttoncross = 0 # Button cross for lights on buttoncircle = 1 # Button circle for lights off buttonsquare = 3 # Button square for claxon on buttontriangle = 2 # Button triangle for claxon off interval = 0.1 # Time between updates in seconds, smaller responds faster but uses more processor time controllerLostLoops = 20 # Number of loops til diablos shut down # Setup pygame and wait for the joystick to become available 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: pygame.joystick.quit() else: # We have a joystick, attempt to initialise it! joystick = pygame.joystick.Joystick(0) break except pygame.error: # Failed to connect to the joystick pygame.joystick.quit() time.sleep(0.1) except KeyboardInterrupt: # CTRL+C exit, give up print('\nUser aborted') exit() print('Joystick found') joystick.init() try: print('Motors are ready to drive.') print('Press CTRL+C to quit') driveLeft = 0.0 driveRight = 0.0 running = True hadEvent = False upDown = 0.0 leftRight = 0.0 # 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 if event.button == buttoncross: GPIO.output(8, GPIO.HIGH) if event.button == buttonsquare: GPIO.output(10, GPIO.HIGH) elif event.type == pygame.JOYBUTTONUP: # A button on the joystick just got released hadEvent = True if event.button == buttoncross: GPIO.output(8, GPIO.LOW) if event.button == buttonsquare: GPIO.output(10, GPIO.LOW) 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): DIABLO1.ResetEpo() DIABLO2.ResetEpo() if not joystick.get_button(buttonSlow): driveLeft *= slowFactor driveRight *= slowFactor # Set the motors to the new speeds DIABLO1.SetMotors(driveLeft * maxPower) DIABLO2.SetMotors(driveRight * maxPower) # 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 # Skip to the next loop after the interval time.sleep(interval) 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!') DIABLO1.MotorsOff() DIABLO2.MotorsOff() controllerLost = True # Wait for the interval period sleep(interval) # Disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() except KeyboardInterrupt: # CTRL+C exit, disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() print('Terminated') except: # Unexpected error, disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() print("Unexpected error:", sys.exc_info()[0]) print() GPIO.cleanup()