Joystick control of DiddyBorg v2 with a MeArm claw

Ever wanted your robot to have claw for picking things up or fighting other robots? Why not add a MeArm to your DiddyBorg v2 to do just that.

In this example we will add an UltraBorg to our DiddyBorg v2 so that it can control the servos used to make the MeArm do its thing :)

Parts

In order to run this script you will need:

• A DiddyBorg v2 with the multifunction top plate
• A MeArm to provide the robotic claw
• An UltraBorg to control the MeArm
• A BattBorg to power the MeArm servos
• A wireless controller, such as a PS3 controller

The multipurpose top plate has two servo mounting positions, one at the front and one at the back. Either can be used for the MeArm base.

Servo connections

Connect the MeArm servos to these ports on the UltraBorg:

1. left / right
2. up / down
3. forward / backward
4. claw open / close

The BattBorg should be used to connect 5v from the 6-pin header into the 2-pin screw terminals on the UltraBorg. Remember to make sure the jumper on the UltraBorg is removed! Both the UltraBorg and BattBorg can be fixed onto the underside of the multipurpose top plate using the various mounting holes. You may wish to grab some mounting posts to help here. After the everything is attached and the robot is re-assembled use the UltraBorg tuning GUI to setup the MeArm movement ranges. Do not leave the servos "under strain" for long periods of time, otherwise you may damage them!

Controls

The controls are based on the joystick control example (diddy2Joy.py) with some new additions:

Tapping the MeArm movement buttons will make small movements. Holding the MeArm movement buttons will make faster movements. For other controllers you will probably need to change the values in "Settings for the joystick" to work properly. You can also change the values to swap buttons around to your preferred layout :) When the "move slowly" button (L2) is held both the main DiddyBorg v2 motors and the MeArm servos will move slower. Again do not leave the servos "under strain" for long periods of time, otherwise you may damage them.

Get the example

The example is part of the standard set of DiddyBorg v2 examples installed during the getting started instructions: bash <(curl https://www.piborg.org/installer/install-diddyborg-v2.txt) To setup the MeArm servos you will also want the UltraBorg examples installed: bash <(curl https://www.piborg.org/installer/install-ultraborg.txt)

Run once

Go to the DiddyBorg v2 code directory: cd ~/diddyborgv2 and run the script using the simple launcher: ./runDiddy2Claw.sh

Run at startup

Open /etc/rc.local to make an addition using: sudo nano /etc/rc.local Then add this line just above the exit 0 line: /home/pi/diddyborgv2/runDiddy2Claw.sh & Finally press CTRL+O, ENTER to save the file followed by CTRL+X to exit nano. Next time you power up the Raspberry Pi it should start the script for you :)

Full code listing - diddy2Claw.py

#!/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 = 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()

# Setup the UltraBorg board
UB = UltraBorg.UltraBorg()              # Create a new UltraBorg object
UB.Init()                               # Set the board up (checks the board is connected)

# 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
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
buttonMeArmClawOpen = 7                 # Joystick button number to open the claw (D-Pad Left)
buttonMeArmClawClose = 5                # Joystick button number to close the claw (D-Pad Right)
buttonMeArmForward = 4                  # Joystick button number to move the MeArm forwards (D-Pad Up)
buttonMeArmBackward = 6                 # Joystick button number to move the MeArm backwards (D-Pad Down)
buttonMeArmUp = 12                      # Joystick button number to move the MeArm upwards (Triangle)
buttonMeArmDown = 14                    # Joystick button number to move the MeArm downwards (Cross)
buttonMeArmLeft = 15                    # Joystick button number to move the MeArm left (Square)
buttonMeArmRight = 13                   # Joystick button number to move the MeArm right (Circle)

# Power settings
voltageIn = 12.0                        # 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 = 4                      # Servo index used for opening the claw
servoMeArmForwardBackward = 3           # Servo index used for moving the MeArm forward / backward
servoMeArmUpDown = 2                    # Servo index used for moving the MeArm up / down
servoMeArmLeftRight = 1                 # Servo index used for moving the MeArm left / right
meArmClaw = 0.0                         # Starting position for the claw
meArmForwardBackward = 0.0              # Starting position for the MeArm forward / backward
meArmUpDown = 0.0                       # Starting position for the MeArm up / down
meArmLeftRight = 0.0                    # Starting position for the MeArm left / right
rateMeArmClaw = 1.0                     # Movement speed for the claw
rateMeArmForwardBackward = 1.0          # Movement speed for the MeArm forward / backward
rateMeArmUpDown = 1.0                   # Movement speed for the MeArm up / down
rateMeArmLeftRight = 1.0                # Movement speed for the MeArm left / right
rateMeArmSlow = 0.2                     # Reduced movement speed when the drive slowly button is held

# 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 '
User aborted'
        TB.SetCommsFailsafe(False)
        TB.SetLeds(0,0,0)
        sys.exit()
print 'Joystick found'
joystick.init()
TB.SetLedShowBattery(True)
ledBatteryMode = True

# Make a function to control a specific servo
def SetServoPosition(servo, position):
    if servo == 1:
        UB.SetServoPosition1(position)
    elif servo == 2:
        UB.SetServoPosition2(position)
    elif servo == 3:
        UB.SetServoPosition3(position)
    elif servo == 4:
        UB.SetServoPosition4(position)
    else:
        print 'Servo index %d is not available' % (servo)

# Set acceleration values
accelScale = 0.001
accelRate = 1.005
accelMeArmClaw = rateMeArmClaw * accelScale
accelMeArmForwardBackward = rateMeArmForwardBackward * accelScale
accelMeArmUpDown = rateMeArmUpDown * accelScale
accelMeArmLeftRight = rateMeArmLeftRight * accelScale

# Set the initial MeArm positions
SetServoPosition(servoMeArmClaw, meArmClaw)
SetServoPosition(servoMeArmForwardBackward, meArmForwardBackward)
SetServoPosition(servoMeArmUpDown, meArmUpDown)
SetServoPosition(servoMeArmLeftRight, meArmLeftRight)

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
                # Set the motors to the new speeds
                TB.SetMotor1(driveLeft  * maxPower)
                TB.SetMotor2(driveRight * maxPower)
            # Adjust the MeArm positions every loop
            if joystick.get_button(buttonSlow):
                meArmSpeed = rateMeArmSlow
            else:
                meArmSpeed = 1.0
            # 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
            # MeArm forward / backward
            if joystick.get_button(buttonMeArmForward):
                meArmForwardBackward += accelMeArmForwardBackward * meArmSpeed
                accelMeArmForwardBackward *= accelRate
                if meArmForwardBackward > 1.0:
                    meArmForwardBackward = 1.0
                SetServoPosition(servoMeArmForwardBackward, meArmForwardBackward)
            elif joystick.get_button(buttonMeArmBackward):
                meArmForwardBackward -= accelMeArmForwardBackward * meArmSpeed
                accelMeArmForwardBackward *= accelRate
                if meArmForwardBackward < -1.0:
                    meArmForwardBackward = -1.0
                SetServoPosition(servoMeArmForwardBackward, meArmForwardBackward)
            else:
                accelMeArmForwardBackward = rateMeArmForwardBackward * accelScale
            # MeArm up / down
            if joystick.get_button(buttonMeArmUp):
                meArmUpDown += accelMeArmUpDown * meArmSpeed
                accelMeArmUpDown *= accelRate
                if meArmUpDown > 1.0:
                    meArmUpDown = 1.0
                SetServoPosition(servoMeArmUpDown, meArmUpDown)
            elif joystick.get_button(buttonMeArmDown):
                meArmUpDown -= accelMeArmUpDown * meArmSpeed
                accelMeArmUpDown *= accelRate
                if meArmUpDown < -1.0:
                    meArmUpDown = -1.0
                SetServoPosition(servoMeArmUpDown, meArmUpDown)
            else:
                accelMeArmUpDown = rateMeArmUpDown * accelScale
            # MeArm left / right
            if joystick.get_button(buttonMeArmLeft):
                meArmLeftRight += accelMeArmLeftRight * meArmSpeed
                accelMeArmLeftRight *= accelRate
                if meArmLeftRight > 1.0:
                    meArmLeftRight = 1.0
                SetServoPosition(servoMeArmLeftRight, meArmLeftRight)
            elif joystick.get_button(buttonMeArmRight):
                meArmLeftRight -= accelMeArmLeftRight * meArmSpeed
                accelMeArmLeftRight *= accelRate
                if meArmLeftRight < -1.0:
                    meArmLeftRight = -1.0
                SetServoPosition(servoMeArmLeftRight, meArmLeftRight)
            else:
                accelMeArmLeftRight = rateMeArmLeftRight * accelScale
        # 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)
except KeyboardInterrupt:
    # CTRL+C exit
    print '
User shutdown'
finally:
    # Disable all drives
    TB.MotorsOff()
    TB.SetCommsFailsafe(False)
    TB.SetLedShowBattery(False)
    TB.SetLeds(0,0,0)
    print