Skip to content

Instantly share code, notes, and snippets.

@fvdbosch fvdbosch/tbJoystick.py Secret
Created Jul 16, 2017

Embed
What would you like to do?
Code for ThunderBorg Devastator Tank robot
#!/usr/bin/env python
# coding: Latin-1
# Code by PiBorg, modified by Frederick Vandenbosch
# Load library functions we want
import time
import os
import sys
import pygame
import ThunderBorg
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
# Setup the ThunderBorg
TB = ThunderBorg.ThunderBorg()
#TB.i2cAddress = 0x15 # Uncomment and change the value if you have changed the board address
TB.Init()
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
print 'No ThunderBorg found, check you are attached :)'
else:
print 'No ThunderBorg at address %02X, but we did find boards:' % (TB.i2cAddress)
for board in boards:
print ' %02X (%d)' % (board, board)
print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
print 'TB.i2cAddress = 0x%02X' % (boards[0])
sys.exit()
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
TB.SetCommsFailsafe(True)
failsafe = TB.GetCommsFailsafe()
if failsafe:
break
if not failsafe:
print 'Board %02X failed to report in failsafe mode!' % (TB.i2cAddress)
sys.exit()
# 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 = 0 # Joystick axis to read for left / right position
axisLeftRightInverted = True # Set this to True if left and right appear to be swapped
buttonSlow = 0 # Joystick button number for driving slowly whilst held (L2)
slowFactor = 0.5 # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
buttonFast = 4 # Joystick button number for driving fast whilst held (L2)
fastFactor = 1.5 # Speed to accelerate to when the drive fast button is held, e.g. 0.5 would be 1.5 speed
buttonFastTurn = 1 # Joystick button number for turning fast (R2)
interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time
buttonShutdown = 6 # Graceful shutdown using button
# Power settings
voltageIn = 12.0 # Total battery voltage to the ThunderBorg
voltageOut = 7.0 # Maximum motor voltage
# Setup the power limits
if voltageOut > voltageIn:
maxPower = 1.0
else:
maxPower = voltageOut / float(voltageIn)
# Show battery monitoring settings
battMin, battMax = TB.GetBatteryMonitoringLimits()
battCurrent = TB.GetBatteryReading()
print 'Battery monitoring settings:'
print ' Minimum (red) %02.2f V' % (battMin)
print ' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2)
print ' Maximum (green) %02.2f V' % (battMax)
print
print ' Current voltage %02.2f V' % (battCurrent)
print
# Setup pygame and wait for the joystick to become available
TB.MotorsOff()
TB.SetLedShowBattery(False)
TB.SetLeds(0,0,1)
os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
#pygame.display.set_mode((1,1))
print 'Waiting for joystick... (press CTRL+C to abort)'
while True:
try:
try:
pygame.joystick.init()
# Attempt to setup the joystick
if pygame.joystick.get_count() < 1:
# No joystick attached, set LEDs blue
TB.SetLeds(0,0,1)
pygame.joystick.quit()
time.sleep(0.1)
else:
# We have a joystick, attempt to initialise it!
joystick = pygame.joystick.Joystick(0)
break
except pygame.error:
# Failed to connect to the joystick, set LEDs blue
TB.SetLeds(0,0,1)
pygame.joystick.quit()
time.sleep(0.1)
except KeyboardInterrupt:
# CTRL+C exit, give up
print '\nUser aborted'
TB.SetCommsFailsafe(False)
TB.SetLeds(0,0,0)
sys.exit()
print 'Joystick found'
joystick.init()
TB.SetLedShowBattery(True)
ledBatteryMode = True
try:
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
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
hadEvent = True
if hadEvent:
# Read axis positions (-1 to +1)
if axisUpDownInverted:
upDown = -joystick.get_axis(axisUpDown)
else:
upDown = joystick.get_axis(axisUpDown)
if axisLeftRightInverted:
leftRight = -joystick.get_axis(axisLeftRight)
else:
leftRight = joystick.get_axis(axisLeftRight)
# Apply steering speeds
if not joystick.get_button(buttonFastTurn):
leftRight *= 0.5
# Determine the drive power levels
driveLeft = -upDown
driveRight = -upDown
if leftRight < -0.05:
# Turning left
driveLeft *= 1.0 + (2.0 * leftRight)
elif leftRight > 0.05:
# Turning right
driveRight *= 1.0 - (2.0 * leftRight)
# Check for button presses
if joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
if joystick.get_button(buttonFast):
driveLeft *= fastFactor
driveRight *= fastFactor
# Set the motors to the new speeds
TB.SetMotor1(driveRight * maxPower)
TB.SetMotor2(driveLeft * maxPower)
if joystick.get_button(buttonShutdown):
os.system("sudo halt")
# Change LEDs to purple to show motor faults
if TB.GetDriveFault1() or TB.GetDriveFault2():
if ledBatteryMode:
TB.SetLedShowBattery(False)
TB.SetLeds(1,0,1)
ledBatteryMode = False
else:
if not ledBatteryMode:
TB.SetLedShowBattery(True)
ledBatteryMode = True
# Wait for the interval period
time.sleep(interval)
# Disable all drives
TB.MotorsOff()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
TB.MotorsOff()
TB.SetCommsFailsafe(False)
TB.SetLedShowBattery(False)
TB.SetLeds(0,0,0)
print
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.