# 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
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
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
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:", exc_info()[0])
print()
GPIO.cleanup()