Create a gist now

Instantly share code, notes, and snippets.

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

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