Thunderborg and UltraBorg

Dear people,

i am fine tuning the monsterJoy.py file. i have 1 servo that i want to control with de d-pad left and right. Ive copied some lines of the diddyborgv2.py file hoping that worked but it doesnt work. Can anyone tell me what i'm doing wrong?

this is the code that im working with:

#!/usr/bin/env python
# coding: Latin-1

# Load library functions we want
import time
import os
import sys
import pygame
import ThunderBorg
import UltraBorg
import RPi.GPIO as GPIO

from time import sleep

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(21, GPIO.OUT, initial=GPIO.LOW)


# 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 ThunderBorgs
TB1 = ThunderBorg.ThunderBorg()
TB2 = ThunderBorg.ThunderBorg()
TB1.i2cAddress = 10
TB2.i2cAddress = 11
TB1.Init()
TB2.Init()
if not TB1.foundChip or not TB2.foundChip:
    boards = ThunderBorg.ScanForThunderBorg()
    if len(boards) == 0:
        print 'No ThunderBorg found, check you are attached :)'
    else:
        print 'ThunderBorg missing at address %02X or %02X, but we did find boards:' % (TB1.i2cAddress, TB2.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 'TB1.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
# Ensure the communications failsafes have been enabled!
failsafe1 = False
failsafe2 = False
for i in range(5):
    TB1.SetCommsFailsafe(True)
    TB2.SetCommsFailsafe(True)
    failsafe1 = TB1.GetCommsFailsafe()
    failsafe2 = TB2.GetCommsFailsafe()
    if failsafe1 and failsafe2:
        break
if not failsafe1:
    print 'Board %02X failed to report in failsafe mode!' % (TB1.i2cAddress)
    sys.exit()
if not failsafe2:
    print 'Board %02X failed to report in failsafe mode!' % (TB2.i2cAddress)
    sys.exit()

# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
    TB1.SetCommsFailsafe(True)
    TB2.SetCommsFailsafe(True)
    failsafe = TB1.GetCommsFailsafe()
    failsafe = TB2.GetCommsFailsafe()
    if failsafe:
        break
if not failsafe:
    print 'Board %02X failed to report in failsafe mode!' % (TB1.i2cAddress)
    print 'Board %02X failed to report in failsafe mode!' % (TB2.i2cAddress)
    sys.exit()
    
# Setup the UltraBorg board
UB = UltraBorg.UltraBorg()
UB.i2cAddress = 24
UB.Init()
if UB.foundChip:
    print 'Yay !'
else:
    print 'Aww...'

# 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
buttonSlow = 6                         # 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 = 7                     # Joystick button number for turning fast (R2)
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time
buttonNew1 = 5 
buttonNew2 = 4
buttonMeArmClawOpen = 13                 # Joystick button number to open the claw (D-Pad Left)
buttonMeArmClawClose = 14                # Joystick button number to close the claw (D-Pad Right)

# 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)
    
# Settings for the MeArm
servoMeArmClaw = 1                      # Servo index used for opening the claw
rateMeArmClaw = 1.0                     # Movement speed for the claw

# Show battery monitoring settings
battMin, battMax = TB1.GetBatteryMonitoringLimits()
battMin, battMax = TB2.GetBatteryMonitoringLimits()
battCurrent = TB1.GetBatteryReading()
battCurrent = TB2.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
TB1.MotorsOff()
TB2.MotorsOff()
TB1.SetLedShowBattery(False)
TB2.SetLedShowBattery(False)
TB1.SetLeds(0,0,1)
TB2.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
                TB1.SetLeds(0,0,1)
                TB2.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
            TB1.SetLeds(0,0,1)
            TB2.SetLeds(0,0,1)
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print '\nUser aborted'
        TB1.SetCommsFailsafe(False)
        TB2.SetCommsFailsafe(False)
        TB1.SetLeds(0,0,0)
        TB2.SetLeds(0,0,0)
        sys.exit()
print 'Joystick found'
joystick.init()
TB1.SetLedShowBattery(True)
TB2.SetLedShowBattery(True)
ledBatteryMode = True

# Make a function to control a specific servo
def SetServoPosition(servo, position):
    if servo == 1:
        UB.SetServoPosition4(position)
    else:
        print 'Servo index %d is not available' % (servo)
        
# Set acceleration values
accelScale = 0.001
accelRate = 1.005
accelMeArmClaw = rateMeArmClaw * accelScale

# Set the initial MeArm positions
SetServoPosition(servoMeArmClaw, meArmClaw)

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
                if event.button == buttonNew1:
                    GPIO.output(21, GPIO.HIGH)
                    print 'Lights on'
                if event.button == buttonNew2:
                    GPIO.output(21, GPIO.LOW)
                    print 'Lights off' 
            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
                # Set the motors to the new speeds
                TB1.SetMotors(driveRight * maxPower)
                TB2.SetMotors(driveLeft * maxPower)
                
                # Claw open / close
            if joystick.get_button(buttonMeArmClawClose):
                meArmClaw += accelMeArmClaw * meArmSpeed
                accelMeArmClaw *= accelRate
                if meArmClaw > 1.0:
                    meArmClaw = 1.0
                SetServoPosition(servoMeArmClaw, meArmClaw)
            elif joystick.get_button(buttonMeArmClawOpen):
                meArmClaw -= accelMeArmClaw * meArmSpeed
                accelMeArmClaw *= accelRate
                if meArmClaw < -1.0:
                    meArmClaw = -1.0
                SetServoPosition(servoMeArmClaw, meArmClaw)
            else:
                accelMeArmClaw = rateMeArmClaw * accelScale                

        # Change LEDs to purple to show motor faults
        if TB1.GetDriveFault1() or TB1.GetDriveFault2() or TB2.GetDriveFault1() or TB2.GetDriveFault2():
            if ledBatteryMode:
                TB1.SetLedShowBattery(False)
                TB2.SetLedShowBattery(False)
                TB1.SetLeds(1,0,1)
                TB2.SetLeds(1,0,1)
                ledBatteryMode = False
        else:
            if not ledBatteryMode:
                TB1.SetLedShowBattery(True)
                TB2.SetLedShowBattery(True)
                ledBatteryMode = True
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    TB1.MotorsOff()
    TB2.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    TB1.MotorsOff()
    TB2.MotorsOff()
    TB1.SetCommsFailsafe(False)
    TB2.SetCommsFailsafe(False)
    TB1.SetLedShowBattery(False)
    TB2.SetLedShowBattery(False)
    TB1.SetLeds(0,0,0)
    TB2.SetLeds(0,0,0)
GPIO.cleanup()
print