Code Update!

Code Update!

After a while of working on writing code, I have finally done it! This script below is adapted from the MonsterJoy code by PiBorg, yet it has a number of important changes. I will probably also tweak the code slightly (ie turn magnitude).

#!/usr/bin/env python
	# coding: Latin-1
	# Load library functions we want
	import time
	import os
	import sys
	import pygame
	import ThunderBorg
	import UltraBorg
	# 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 = 0xA # 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
	axisUp = 2
	axisDown = 5 # Joystick axis to read for up / down position
	axisUpDownInverted = True # 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 = 8 # 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
	buttonFastTurn = 9 # Joystick button number for turning fast (R2)
	interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time
	# Power settings
	voltageIn = 1.2 * 10 # Total battery voltage to the ThunderBorg
	voltageOut = 12.0 * 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power
	# Setup the power limits
	if voltageOut > voltageIn:
	maxPower = 1.0
	else:
	maxPower = voltageOut / float(voltageIn)
	powerReductionFactor = 0.6
	maxPower *= powerReductionFactor
	# 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
	print("Initializing ultraborg")
	UB = UltraBorg.UltraBorg()
	UB.i2cAddress = 0xB
	UB.Init()
	servo1Min = -1
	servo1Max = 1
	servo2Min = -1
	servo2Max = 1
	servo3Min = -1
	servo3Max = 1
	servoRate = -0.05
	servoRatePincer = 0.05
	servoUD = 4
	servoLR = 3
	pincerClose = 5
	pincerOpen = 4
	print("returning to start position...")
	servo1 = UB.GetServoStartup1()
	servo2 = UB.GetServoStartup2()
	servo3 = 0
	UB.SetServoPosition1(servo1)
	UB.SetServoPosition2(servo2)
	UB.SetServoPosition3(servo3)
	# 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.JOYBUTTONUP:
	hadEvent = True
	elif event.type == pygame.JOYAXISMOTION:
	# A joystick has been moved
	hadEvent = True
	if hadEvent:
	#Pincer
	close = joystick.get_button(pincerClose)
	open = joystick.get_button(pincerOpen)
	if close:
	servo3 -= 0.2
	if servo3 < servo3Min:
	servo3 = servo3Min
	UB.SetServoPosition3(servo3)
	if open:
	servo3 += 0.2
	if servo3 > servo3Max:
	servo3 = servo3Max
	UB.SetServoPosition3(servo3)
	close = False
	open = False
	#Camera Servo
	servo1 += servoRate * joystick.get_axis(servoLR)
	servo2 -= servoRate * joystick.get_axis(servoUD)
	if servo1 > servo1Max:
	servo1 = servo1Max
	if servo1 < servo1Min:
	servo1 = servo1Min
	if servo2 > servo2Max:
	servo2 = servo1Max
	if servo2 < servo2Min:
	servo2 = servo1Min
	UB.SetServoPosition1(servo1)
	UB.SetServoPosition2(servo2)
	# Read axis positions (-1 to +1)
	up = (1 + joystick.get_axis(axisUp))/2
	down = (1 + joystick.get_axis(axisDown))/2
	upDown = up - down
	leftRight = joystick.get_axis(axisLeftRight)
	# Apply steering speeds
	if not joystick.get_button(buttonFastTurn):
	pass
	#leftRight *= 0.5
	# Determine the drive power levels
	driveLeft = upDown
	driveRight = upDown
	if leftRight > 0.05:
	# Turning left
	driveLeft *= 1.0 + (2.0 * leftRight)
	driveRight *= 1.0 - (1.0 * leftRight)
	elif leftRight < -0.05:
	# Turning right
	driveRight *= 1.0 - (2.0 * leftRight)
	driveLeft *= 1.0 + (1.0 * leftRight)
	# Check for button presses
	if joystick.get_button(buttonSlow):
	driveLeft *= slowFactor
	driveRight *= slowFactor
	# Set the motors to the new speeds
	TB.SetMotor1(driveRight * maxPower)
	TB.SetMotor2(driveLeft * maxPower)
	# 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