It is possible, we usually do this by checking how long it has been since the controller has provided an update for the inputs.
If your code is polling the controller inputs (checking them at a regular interval), you can check how long it has been since an input change by counting how many times you have polled without any update.
In the DoddleBorg code we keep a loopsWithoutEvent counter. If we do not see an event for a polling interval the counter is increased by one. Whenever we see an event the counter is reset to zero. When loopsWithoutEvent exceeds controllerLostLoops we decide the controller has lost contact and we stop the motors.
If your code blocks until a new event happens you will need a watchdog thread instead. This runs in the background and will turn motors off if the time since the last update is too long.
Our WebUI examples have this feature: monsterWeb.py.
In this code the class Watchdog is a thread that turns the motors off when its event property has not been set recently. The code sets the event by calling watchdog.event.set() when it gets an update, where watchdog is an instance of the Watchdog class. The time delay is set by the if self.event.wait(1): line, where 1 means 1 second.
Dear Piborg, i changed my code with doodleborg code but it seems that is doesnt work it gives a few faults but i dont know what. Here is the code that im working with it works with 2 diablos. Greetings Thieu.
# 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
import pygame
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(7, GPIO.OUT, initial=GPIO.LOW)
# Power settings
voltageIn = 24.0 # Total battery voltage to the Diablos
voltageOut = 18.0 # 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 = 2 # 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 = 9 # Joystick button number for turning fast (R2)
buttoncross = 14 # Button cross for lights on
buttoncircle = 13 # Button circle for lights off
buttonsquare = 15 # Button square for claxon on
buttontriangle = 12 # Button triangle for claxon off
interval = 0.00 # 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 quit)')
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')
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 == buttoncircle:
GPIO.output(8, GPIO.LOW)
if event.button == buttonsquare:
GPIO.output(7, GPIO.HIGH)
if event.button == buttontriangle:
GPIO.output(7, GPIO.LOW)
if event.button == r1:
axisUpDownInverted = 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):
DIABLO.ResetEpo()
if not joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
# Set the motors to the new speeds
# Set the motors to the new speeds
DIABLO1.SetMotors(driveLeft * maxPower)
DIABLO2.SetMotors(driveRight * maxPower)
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 DIABLO1 in Diablo:
controllerLost = False
for DIABLO2 in Diablo:
controllerLost = False
# 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!')
for DIABLO1 in Diablo:
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
for DIABLO2 in Diablo:
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.ResetEpo()
DIABLO2.ResetEpo()
print('Terminated')
print()
GPIO.cleanup()
# 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
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.00 # 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')
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):
DIABLO.ResetEpo()
if not joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
# Set the motors to the new speeds
# Set the motors to the new speeds
DIABLO1.SetMotors(driveLeft * maxPower)
DIABLO2.SetMotors(driveRight * maxPower)
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 Diablox in DIABLO:
controllerLost = False
# 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!')
for Diablox in DIABLO:
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.ResetEpo()
DIABLO2.ResetEpo()
print('Terminated')
print()
GPIO.cleanup()
I think you are close, but there are a few things that need to be sorted to get things working :)
The first is that the indenting is a bit mixed up, probably because of cutting and pasting from another script. Unfortunately Python does not like a mixture of spaces and tabs for the indenting. I have attached an image of where the tabs are below (shown by the red dots) so you can see what I mean.
The second thing is not really a problem, simply that you do not need the if hadEvent: line twice. The inner one can be removed as it has already been checked at that point in the code :)
The third issue is the for Diablox in DIABLO: lines. In both cases these are invalid (there is no DIABLO), but the lines under them are correct ones. Also the DIABLO.ResetEpo() line should be two lines for DIABLO1 and DIABLO2.
The fourth part is the # Attempt to reset the joystick module section. This is intended to handle a pygame errors losing the controller entirely and recover, but it requires other detection code to work. Instead we can have the script turn the motors off and quit in the event of any unexpected error by adding another except block at the end without any error type.
The final part is just a value change. Currently the value of interval is 0, meaning it will check for events as fast as possible. This means the 20 loops set by controllerLostLoops may be very quick, making the lost detection too sensitive. I would set interval to 0.1 (ten checks per second maximum) to mean at least 2 seconds (20 × 0.1 = 2) without any update from the controller will trigger the detection. Once it works you can adjust either of these values to change the minimum delay.
I have made these changes (they sound worse then they actually are) and have attached the updated script below. Give it a try and let me know if it works or if there are still problems :)
Dear Piborg, the code almost works there is only one issue.
When i leave the interval on 0.1 it seems that the ps3 controller reacts very slow when i change it to 0.05 it reacts better but the motors sometimes stop for a few millisecs and than turns again.
It looks like I missed a couple of problems in the code :(
The first is that it is updating the motors and waiting for each input changed, it should be just once per interval. There are also two sleep calls instead of just one.
The second is that the else block on line 183 does not line up with an if block like it is intended to.
This version should work better and without the strange behaviour from before. You can lower the interval again if the controls respond too slowly like before.
Dear Piborg, i have still a fault in the program. When i connect the controller it immediatly gives the controller lost message on the terminal window, i dont know whats wrong. Ive attached the code.
Greetings Thieu
# 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
import pygame
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW)
# Power settings
voltageIn = 24.0 # Total battery voltage to the Diablo
voltageOut = 12.0 # 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 = True # 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 # Joystick button cross
buttoncircle = 1 # Joystick button circle
buttontriangle = 2 # Joystick button triangle
buttonsquare = 3 # Joystick button square
buttonL1 = 4 # Joystick button L1
buttonR1 = 5 # Joystick button R1
interval = 0.1 # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults
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')
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 == buttoncircle:
GPIO.output(13, GPIO.HIGH)
if event.button == buttoncross:
GPIO.output(8, GPIO.HIGH)
if event.button == buttonsquare:
GPIO.output(10, GPIO.HIGH)
if event.button == buttontriangle:
GPIO.output(11, GPIO.HIGH)
if event.button == buttonL1:
axisUpDownInverted = True
axisLeftRightInverted = False
if event.button == buttonR1:
axisUpDownInverted = False
axisLeftRightInverted = True
elif event.type == pygame.JOYBUTTONUP:
# A button on the joystick just got released
hadEvent = True
if event.button == buttoncircle:
GPIO.output(13, GPIO.LOW)
if event.button == buttoncross:
GPIO.output(8, GPIO.LOW)
if event.button == buttonsquare:
GPIO.output(10, GPIO.LOW)
if event.button == buttontriangle:
GPIO.output(11, 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):
DIABLO.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)
if controllerLost:
controllerLost = False
else:
loopsWithoutEvent += 1
if loopsWithoutEvent > controllerLostLoops:
print('Controller lost!')
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
controllerLost = True
sleep(interval)
# Disable all drives
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
DIABLO1.ResetEpo()
DIABLO2.ResetEpo()
print('Terminated')
except:
# Unexpected error, disable all drives
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
print("Unexpected error:", exc_info()[0])
print()
GPIO.cleanup()
1. Indentation below the "Set the motors to the new speeds" comment.
The lines below the comment should be shifted to the left so that the DIABLO1.SetMotors(driveLeft * maxPower) line starts in the same column as the comment itself.
2. The sleep(interval) line is in the wrong place.
You want the outer while running: loop to have a delay at the end every time, not just in some cases. Move the sleep(interval) line down so it is the last line in the loop and change the indentation so it starts in the same column as the if hadEvent: line further up.
Dear Piborg, ive tried what you said and a few other things but its still not working fine. When i push the joystick forward so that the motors are turning and i walk away to disconnect the controller while the motors are turning, the controllers disconnect but the motors keep on turning and dont stop unless i restart the progam again. i also dont get the controller lost message on the terminal.
# 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
import pygame
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW)
# Power settings
voltageIn = 24.0 # Total battery voltage to the Diablo
voltageOut = 12.0 # 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 = True # 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 # Joystick button cross
buttoncircle = 1 # Joystick button circle
buttontriangle = 2 # Joystick button triangle
buttonsquare = 3 # Joystick button square
buttonL1 = 4 # Joystick button L1
buttonR1 = 5 # Joystick button R1
interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults
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')
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 == buttoncircle:
GPIO.output(13, GPIO.HIGH)
if event.button == buttoncross:
GPIO.output(8, GPIO.HIGH)
if event.button == buttonsquare:
GPIO.output(10, GPIO.HIGH)
if event.button == buttontriangle:
GPIO.output(11, GPIO.HIGH)
if event.button == buttonL1:
axisUpDownInverted = True
axisLeftRightInverted = False
if event.button == buttonR1:
axisUpDownInverted = False
axisLeftRightInverted = True
elif event.type == pygame.JOYBUTTONUP:
# A button on the joystick just got released
hadEvent = True
if event.button == buttoncircle:
GPIO.output(13, GPIO.LOW)
if event.button == buttoncross:
GPIO.output(8, GPIO.LOW)
if event.button == buttonsquare:
GPIO.output(10, GPIO.LOW)
if event.button == buttontriangle:
GPIO.output(11, 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):
DIABLO.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)
if hadEvent:
# Reset the controller lost counter
loopsWithoutEvent = 0
controllerLost = False
else:
loopsWithoutEvent += 1
if loopsWithoutEvent > controllerLostLoops:
print('Controller lost!')
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
controllerLost = True
# Disable all drives
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
sleep(interval)
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
DIABLO1.ResetEpo()
DIABLO2.ResetEpo()
print('Terminated')
except:
# Unexpected error, disable all drives
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
print("Unexpected error:", exc_info()[0])
print()
GPIO.cleanup()
The check to see if you have had input is within the larger if hadEvent: section, this means it cannot run when there is no input!
I have had a go at fixing things. Give this version a try:
# 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
import pygame
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW)
# Power settings
voltageIn = 24.0 # Total battery voltage to the Diablo
voltageOut = 12.0 # 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 = True # 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 # Joystick button cross
buttoncircle = 1 # Joystick button circle
buttontriangle = 2 # Joystick button triangle
buttonsquare = 3 # Joystick button square
buttonL1 = 4 # Joystick button L1
buttonR1 = 5 # Joystick button R1
interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults
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')
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 == buttoncircle:
GPIO.output(13, GPIO.HIGH)
if event.button == buttoncross:
GPIO.output(8, GPIO.HIGH)
if event.button == buttonsquare:
GPIO.output(10, GPIO.HIGH)
if event.button == buttontriangle:
GPIO.output(11, GPIO.HIGH)
if event.button == buttonL1:
axisUpDownInverted = True
axisLeftRightInverted = False
if event.button == buttonR1:
axisUpDownInverted = False
axisLeftRightInverted = True
elif event.type == pygame.JOYBUTTONUP:
# A button on the joystick just got released
hadEvent = True
if event.button == buttoncircle:
GPIO.output(13, GPIO.LOW)
if event.button == buttoncross:
GPIO.output(8, GPIO.LOW)
if event.button == buttonsquare:
GPIO.output(10, GPIO.LOW)
if event.button == buttontriangle:
GPIO.output(11, GPIO.LOW)
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
hadEvent = True
# Update the motor output if any events occurred
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):
DIABLO.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
controllerLost = False
elif not controllerLost:
# No input, increment no event counter
loopsWithoutEvent += 1
if loopsWithoutEvent > controllerLostLoops:
# Counter limit exceeded, disable all drives
print('Controller lost!')
DIABLO1.MotorsOff()
DIABLO2.MotorsOff()
controllerLost = True
sleep(interval)
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()
piborg
Wed, 11/18/2020 - 10:50
Permalink
Turning motors off when controller is out of range
It is possible, we usually do this by checking how long it has been since the controller has provided an update for the inputs.
If your code is polling the controller inputs (checking them at a regular interval), you can check how long it has been since an input change by counting how many times you have polled without any update.
The early DoodleBorg code has this feature: DoodleBorg - Controlled using a PS3 controller.
In the DoddleBorg code we keep a
loopsWithoutEventcounter. If we do not see an event for a polling interval the counter is increased by one. Whenever we see an event the counter is reset to zero. WhenloopsWithoutEventexceedscontrollerLostLoopswe decide the controller has lost contact and we stop the motors.If your code blocks until a new event happens you will need a watchdog thread instead. This runs in the background and will turn motors off if the time since the last update is too long.
Our WebUI examples have this feature: monsterWeb.py.
In this code the
class Watchdogis a thread that turns the motors off when itseventproperty has not been set recently. The code sets the event by callingwatchdog.event.set()when it gets an update, wherewatchdogis an instance of theWatchdogclass. The time delay is set by theif self.event.wait(1):line, where1means 1 second.Thieu
Wed, 11/25/2020 - 18:24
Permalink
code eddited
Dear Piborg, i changed my code with doodleborg code but it seems that is doesnt work it gives a few faults but i dont know what. Here is the code that im working with it works with 2 diablos. Greetings Thieu.
# 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 import pygame import RPi.GPIO as GPIO GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(7, GPIO.OUT, initial=GPIO.LOW) # Power settings voltageIn = 24.0 # Total battery voltage to the Diablos voltageOut = 18.0 # 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 = 2 # 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 = 9 # Joystick button number for turning fast (R2) buttoncross = 14 # Button cross for lights on buttoncircle = 13 # Button circle for lights off buttonsquare = 15 # Button square for claxon on buttontriangle = 12 # Button triangle for claxon off interval = 0.00 # 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 quit)') 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') 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 == buttoncircle: GPIO.output(8, GPIO.LOW) if event.button == buttonsquare: GPIO.output(7, GPIO.HIGH) if event.button == buttontriangle: GPIO.output(7, GPIO.LOW) if event.button == r1: axisUpDownInverted = 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): DIABLO.ResetEpo() if not joystick.get_button(buttonSlow): driveLeft *= slowFactor driveRight *= slowFactor # Set the motors to the new speeds # Set the motors to the new speeds DIABLO1.SetMotors(driveLeft * maxPower) DIABLO2.SetMotors(driveRight * maxPower) 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 DIABLO1 in Diablo: controllerLost = False for DIABLO2 in Diablo: controllerLost = False # 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!') for DIABLO1 in Diablo: DIABLO1.MotorsOff() DIABLO2.MotorsOff() for DIABLO2 in Diablo: 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.ResetEpo() DIABLO2.ResetEpo() print('Terminated') print() GPIO.cleanup()Thieu
Wed, 11/25/2020 - 19:48
Permalink
other code
Maybe this code is a little better
Greetings
# 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 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.00 # 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') 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): DIABLO.ResetEpo() if not joystick.get_button(buttonSlow): driveLeft *= slowFactor driveRight *= slowFactor # Set the motors to the new speeds # Set the motors to the new speeds DIABLO1.SetMotors(driveLeft * maxPower) DIABLO2.SetMotors(driveRight * maxPower) 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 Diablox in DIABLO: controllerLost = False # 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!') for Diablox in DIABLO: 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.ResetEpo() DIABLO2.ResetEpo() print('Terminated') print() GPIO.cleanup()piborg
Thu, 11/26/2020 - 17:06
Permalink
Just needs a little tweaking
I think you are close, but there are a few things that need to be sorted to get things working :)
The first is that the indenting is a bit mixed up, probably because of cutting and pasting from another script. Unfortunately Python does not like a mixture of spaces and tabs for the indenting. I have attached an image of where the tabs are below (shown by the red dots) so you can see what I mean.
The second thing is not really a problem, simply that you do not need the
if hadEvent:line twice. The inner one can be removed as it has already been checked at that point in the code :)The third issue is the
for Diablox in DIABLO:lines. In both cases these are invalid (there is noDIABLO), but the lines under them are correct ones. Also theDIABLO.ResetEpo()line should be two lines forDIABLO1andDIABLO2.The fourth part is the
# Attempt to reset the joystick modulesection. This is intended to handle a pygame errors losing the controller entirely and recover, but it requires other detection code to work. Instead we can have the script turn the motors off and quit in the event of any unexpected error by adding anotherexceptblock at the end without any error type.The final part is just a value change. Currently the value of
intervalis 0, meaning it will check for events as fast as possible. This means the 20 loops set bycontrollerLostLoopsmay be very quick, making the lost detection too sensitive. I would setintervalto 0.1 (ten checks per second maximum) to mean at least 2 seconds (20 × 0.1 = 2) without any update from the controller will trigger the detection. Once it works you can adjust either of these values to change the minimum delay.I have made these changes (they sound worse then they actually are) and have attached the updated script below. Give it a try and let me know if it works or if there are still problems :)
Thieu
Fri, 11/27/2020 - 18:37
Permalink
Code
Hallo there i tested the code but it seems to be that line 216 is wrong. I have a picture of it attached
Greetings
piborg
Fri, 11/27/2020 - 20:15
Permalink
Oops
Line 216 should be:
print("Unexpected error:", exc_info()[0])Thieu
Sat, 11/28/2020 - 06:44
Permalink
Error
Hello, i removed the "sys" but still getting an error
piborg
Sat, 11/28/2020 - 10:40
Permalink
Another coding bug
NameError suggests that there is another mistake in the code earlier on.
Comment out the unexpected error handling code:
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()This will let Python handle the error itself, which should give us the line number for the problem :)
Thieu
Mon, 11/30/2020 - 05:58
Permalink
Fine tuning
Dear Piborg, the code almost works there is only one issue.
When i leave the interval on 0.1 it seems that the ps3 controller reacts very slow when i change it to 0.05 it reacts better but the motors sometimes stop for a few millisecs and than turns again.
Greetings Thieu
piborg
Mon, 11/30/2020 - 11:03
Permalink
I missed a couple of problems
It looks like I missed a couple of problems in the code :(
The first is that it is updating the motors and waiting for each input changed, it should be just once per interval. There are also two
sleepcalls instead of just one.The second is that the
elseblock on line 183 does not line up with anifblock like it is intended to.This version should work better and without the strange behaviour from before. You can lower the interval again if the controls respond too slowly like before.
Thieu
Sun, 12/27/2020 - 18:15
Permalink
Project pictures
Maybe intresting for some other raspbery enthusiasts, some pictures of my covid project.
Thieu
Sun, 12/27/2020 - 18:19
Permalink
Rest of the pictures.
Rest of the pictures.
Thieu
Mon, 03/29/2021 - 18:42
Permalink
fault
Dear Piborg, i have still a fault in the program. When i connect the controller it immediatly gives the controller lost message on the terminal window, i dont know whats wrong. Ive attached the code.
Greetings Thieu
# 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 import pygame import RPi.GPIO as GPIO GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW) # Power settings voltageIn = 24.0 # Total battery voltage to the Diablo voltageOut = 12.0 # 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 = True # 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 # Joystick button cross buttoncircle = 1 # Joystick button circle buttontriangle = 2 # Joystick button triangle buttonsquare = 3 # Joystick button square buttonL1 = 4 # Joystick button L1 buttonR1 = 5 # Joystick button R1 interval = 0.1 # Time between updates in seconds, smaller responds faster but uses more processor time controllerLostLoops = 20 # Setup pygame and wait for the joystick to become available environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults 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') 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 == buttoncircle: GPIO.output(13, GPIO.HIGH) if event.button == buttoncross: GPIO.output(8, GPIO.HIGH) if event.button == buttonsquare: GPIO.output(10, GPIO.HIGH) if event.button == buttontriangle: GPIO.output(11, GPIO.HIGH) if event.button == buttonL1: axisUpDownInverted = True axisLeftRightInverted = False if event.button == buttonR1: axisUpDownInverted = False axisLeftRightInverted = True elif event.type == pygame.JOYBUTTONUP: # A button on the joystick just got released hadEvent = True if event.button == buttoncircle: GPIO.output(13, GPIO.LOW) if event.button == buttoncross: GPIO.output(8, GPIO.LOW) if event.button == buttonsquare: GPIO.output(10, GPIO.LOW) if event.button == buttontriangle: GPIO.output(11, 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): DIABLO.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) if controllerLost: controllerLost = False else: loopsWithoutEvent += 1 if loopsWithoutEvent > controllerLostLoops: print('Controller lost!') DIABLO1.MotorsOff() DIABLO2.MotorsOff() controllerLost = True sleep(interval) # Disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() except KeyboardInterrupt: # CTRL+C exit, disable all drives DIABLO1.ResetEpo() DIABLO2.ResetEpo() print('Terminated') except: # Unexpected error, disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() print("Unexpected error:", exc_info()[0]) print() GPIO.cleanup()piborg
Tue, 03/30/2021 - 09:51
Permalink
Two problems
I think there are two problems with the code.
1. Indentation below the "Set the motors to the new speeds" comment.
The lines below the comment should be shifted to the left so that the
DIABLO1.SetMotors(driveLeft * maxPower)line starts in the same column as the comment itself.2. The
sleep(interval)line is in the wrong place.You want the outer
while running:loop to have a delay at the end every time, not just in some cases. Move thesleep(interval)line down so it is the last line in the loop and change the indentation so it starts in the same column as theif hadEvent:line further up.Thieu
Tue, 03/30/2021 - 18:48
Permalink
no succes
Dear Piborg, ive tried what you said and a few other things but its still not working fine. When i push the joystick forward so that the motors are turning and i walk away to disconnect the controller while the motors are turning, the controllers disconnect but the motors keep on turning and dont stop unless i restart the progam again. i also dont get the controller lost message on the terminal.
# 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 import pygame import RPi.GPIO as GPIO GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW) # Power settings voltageIn = 24.0 # Total battery voltage to the Diablo voltageOut = 12.0 # 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 = True # 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 # Joystick button cross buttoncircle = 1 # Joystick button circle buttontriangle = 2 # Joystick button triangle buttonsquare = 3 # Joystick button square buttonL1 = 4 # Joystick button L1 buttonR1 = 5 # Joystick button R1 interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time controllerLostLoops = 20 # Setup pygame and wait for the joystick to become available environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults 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') 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 == buttoncircle: GPIO.output(13, GPIO.HIGH) if event.button == buttoncross: GPIO.output(8, GPIO.HIGH) if event.button == buttonsquare: GPIO.output(10, GPIO.HIGH) if event.button == buttontriangle: GPIO.output(11, GPIO.HIGH) if event.button == buttonL1: axisUpDownInverted = True axisLeftRightInverted = False if event.button == buttonR1: axisUpDownInverted = False axisLeftRightInverted = True elif event.type == pygame.JOYBUTTONUP: # A button on the joystick just got released hadEvent = True if event.button == buttoncircle: GPIO.output(13, GPIO.LOW) if event.button == buttoncross: GPIO.output(8, GPIO.LOW) if event.button == buttonsquare: GPIO.output(10, GPIO.LOW) if event.button == buttontriangle: GPIO.output(11, 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): DIABLO.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) if hadEvent: # Reset the controller lost counter loopsWithoutEvent = 0 controllerLost = False else: loopsWithoutEvent += 1 if loopsWithoutEvent > controllerLostLoops: print('Controller lost!') DIABLO1.MotorsOff() DIABLO2.MotorsOff() controllerLost = True # Disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() sleep(interval) except KeyboardInterrupt: # CTRL+C exit, disable all drives DIABLO1.ResetEpo() DIABLO2.ResetEpo() print('Terminated') except: # Unexpected error, disable all drives DIABLO1.MotorsOff() DIABLO2.MotorsOff() print("Unexpected error:", exc_info()[0]) print() GPIO.cleanup()piborg
Tue, 03/30/2021 - 23:14
Permalink
The last part does not run when their is no input
The check to see if you have had input is within the larger
if hadEvent:section, this means it cannot run when there is no input!I have had a go at fixing things. Give this version a try:
# 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 import pygame import RPi.GPIO as GPIO GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW) # Power settings voltageIn = 24.0 # Total battery voltage to the Diablo voltageOut = 12.0 # 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 = True # 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 # Joystick button cross buttoncircle = 1 # Joystick button circle buttontriangle = 2 # Joystick button triangle buttonsquare = 3 # Joystick button square buttonL1 = 4 # Joystick button L1 buttonR1 = 5 # Joystick button R1 interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time controllerLostLoops = 20 # Setup pygame and wait for the joystick to become available environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults 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') 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 == buttoncircle: GPIO.output(13, GPIO.HIGH) if event.button == buttoncross: GPIO.output(8, GPIO.HIGH) if event.button == buttonsquare: GPIO.output(10, GPIO.HIGH) if event.button == buttontriangle: GPIO.output(11, GPIO.HIGH) if event.button == buttonL1: axisUpDownInverted = True axisLeftRightInverted = False if event.button == buttonR1: axisUpDownInverted = False axisLeftRightInverted = True elif event.type == pygame.JOYBUTTONUP: # A button on the joystick just got released hadEvent = True if event.button == buttoncircle: GPIO.output(13, GPIO.LOW) if event.button == buttoncross: GPIO.output(8, GPIO.LOW) if event.button == buttonsquare: GPIO.output(10, GPIO.LOW) if event.button == buttontriangle: GPIO.output(11, GPIO.LOW) elif event.type == pygame.JOYAXISMOTION: # A joystick has been moved hadEvent = True # Update the motor output if any events occurred 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): DIABLO.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 controllerLost = False elif not controllerLost: # No input, increment no event counter loopsWithoutEvent += 1 if loopsWithoutEvent > controllerLostLoops: # Counter limit exceeded, disable all drives print('Controller lost!') DIABLO1.MotorsOff() DIABLO2.MotorsOff() controllerLost = True sleep(interval) 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()Thieu
Wed, 03/31/2021 - 17:26
Permalink
Working
This code works much better, many thanks. The only thing is that i have to restart the program when the controller is lost.
Greetings Thieu