Running two PicoBorg Reverse for different uses in one unit

Submitted by METTAUK on Wed, 11/02/2016 - 16:38

Forums:

I've managed to assign different addresses as per the instructions so have address 10 and 11 with 10 as the mian standard wheel driver.

I want to use one board as wheel driver (10) and am using MJoy.py to drive a home made version of diddyborg. I now want to use the second PicoBorg Reverse to drive (11) on board motors (not driving the wheels) with a 'simple' full on or full of switch assigned to a PS3 remote using say Triangle and Square as on off switching.

My questions relate to modifying the MJoy scrip to include the second board but am a bit lost how to intergrate the two. This is what i've tried so far, it seems to work the same as MJoy.py but cant even get the LED flashing on board 11?

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

# Load library functions we want
import time
import os
import sys
import pygame
import PicoBorgRev

# Add no crash system
#import UltraBorg

#distanceMin = 300               # Minimum distance in mm, corresponds to DiddyBorg reversing at 100%
#distanceMax = 400               # Maximum distance in mm, corresponds to DiddyBorg driving at 100%

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





# 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 PicoBorg Reverse
PBR1 = PicoBorgRev.PicoBorgRev()
PBR2 = PicoBorgRev.PicoBorgRev()
PBR1.i2cAddress = 10                  # Uncomment and change the value if you have changed the board address
PBR2.i2cAddress = 11
PBR1.Init()
PBR2.Init()
if not PBR1.foundChip:
    boards = PicoBorgRev.ScanForPicoBorgReverse()
    if len(boards) == 0:
        print 'No PicoBorg Reverse found, check you are attached :)'
    else:
        print 'No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR1.i2cAddress),(PBR2.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 'PBR1.i2cAddress = 0x%02X' % (boards[0]),'PBR2.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
 # PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
 # Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
    PBR1.SetCommsFailsafe(True)
    PBR2.SetCommsFailsafe(True)
    failsafe = PBR1.GetCommsFailsafe()
    failsafe = PBR2.GetCommsFailsafe()
    if failsafe:
        break
if not failsafe:
    print 'Board %02X failed to report in failsafe mode!' % (PBR1.i2cAddress),(PBR2.i2cAddress)
    sys.exit()
PBR1.ResetEpo()
PBR2.ResetEpo()

# 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
buttonResetEpo = 3                      # Joystick button number to perform an EPO reset (Start)
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 = 3.7 * 4                    # Total battery voltage to the PicoBorg Reverse
voltageOut = 12.4                       # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Setup pygame and wait for the joystick to become available
PBR1.MotorsOff()
PBR2.MotorsOff()
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, toggle the LED
                PBR1.SetLed(not PBR1.GetLed())
                PBR2.SetLed(not PBR2.GetLed())
                pygame.joystick.quit()
                time.sleep(0.5)
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick, toggle the LED
            PBR1.SetLed(not PBR.GetLed())
            PBR2.SetLed(not PBR.GetLed())
            pygame.joystick.quit()
            time.sleep(0.5)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print '\nUser aborted'
        PBR1.SetLed(True)
        PBR2.SetLed(True)
        sys.exit()
print 'Joystick found'
joystick.init()
PBR1.SetLed(False)
PBR2.SetLed(False)

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(buttonResetEpo):
                    PBR1.ResetEpo()
                if joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                # Set the motors to the new speeds
                PBR1.SetMotor1(driveRight * maxPower)
                PBR1.SetMotor2(-driveLeft * maxPower)
        # Change the LED to reflect the status of the EPO latch
        PBR1.SetLed(PBR1.GetEpo())
        PBR2.SetLed(PBR2.GetEpo())
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    PBR1.MotorsOff()
    PBR2.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    PBR1.MotorsOff()
    PBR2.MotorsOff()
print

Any help would be much apreciated

piborg's picture

piborg

Wed, 11/02/2016 - 19:52

The first thing you want to do is replace the # Setup the PicoBorg Reverse section in the script with the code you have above.

Next replace all of the references to PBR with PBR1.
At this point the script should run and work the same way it used to.

Next we want to ensure the second Reverse has it's motors disabled when the script is done.
Find all the lines which look like this:

PBR1.MotorsOff()

and add a duplicate for PBR2:

PBR1.MotorsOff()
PBR2.MotorsOff()

In order to "grab" button presses we want to put some code to handle button press events.

First look for this bit of code:

            elif event.type == pygame.JOYBUTTONDOWN:
                # A button on the joystick just got pushed down
                hadEvent = True

What we want to do is check what the button was and change the values for the second Reverse.
You can find out what the button code is from event.button, the codes for a PS3 controller are listed on our PlayStation 3 controller help sheet in the button index column.

As an example this will turn motor 1 on with triangle, and off with square:

            elif event.type == pygame.JOYAXISMOTION:
                # A joystick has been moved
                hadEvent = True
				if event.button == 12:
					# Triangle pressed
					PBR2.SetMotor1(1.0)
				elif event.button == 15:
					# Square pressed
					PBR2.SetMotor1(0.0)

You can keep adding additional elif lines for more buttons until you run out :)

METTAUK

Thu, 11/03/2016 - 01:34

Thank you for your help

On line 32 I have 

if not PBR1.foundChip:

With nothing for PBR2. As PBR2 to will be spinning a motor at 8000rpm I want to make sure fail safes are working? I have the jumper wired to cut switches but am a little worried if the BT to PS3 remote fails, will it stop after x time with no connection? WiFi is less of an issue at the moment (Only used to get the script going then WiFi is lost anyway!

Line 176 and 179 turn the spinner on and off

I'm a bit worried that I've just duplicated lines to add in the second PicoBorg Reverse without really knowing what i'm doing! Its not dangerous but further down the line I might make something else and it could be IF i'M MESSING UP HERE!

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

# Load library functions we want
import time
import os
import sys
import pygame
import PicoBorgRev

# Add no crash system
#import UltraBorg

#distanceMin = 300               # Minimum distance in mm, corresponds to DiddyBorg reversing at 100%
#distanceMax = 400               # Maximum distance in mm, corresponds to DiddyBorg driving at 100%

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

# 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 PicoBorg Reverse
PBR1 = PicoBorgRev.PicoBorgRev()
PBR2 = PicoBorgRev.PicoBorgRev()
PBR1.i2cAddress = 10                  # Uncomment and change the value if you have changed the board address
PBR2.i2cAddress = 11
PBR1.Init()
PBR2.Init()
if not PBR1.foundChip:
    boards = PicoBorgRev.ScanForPicoBorgReverse()
    if len(boards) == 0:
        print 'No PicoBorg Reverse found, check you are attached :)'
    else:
        print 'No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR1.i2cAddress),(PBR2.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 'PBR1.i2cAddress = 0x%02X' % (boards[0]),'PBR2.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
 # PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
 # Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
    PBR1.SetCommsFailsafe(True)
    PBR2.SetCommsFailsafe(True)
    failsafe = PBR1.GetCommsFailsafe()
    failsafe = PBR2.GetCommsFailsafe()
    if failsafe:
        break
if not failsafe:
    print 'Board %02X failed to report in failsafe mode!' % (PBR1.i2cAddress),(PBR2.i2cAddress)
    sys.exit()
PBR1.ResetEpo()
PBR2.ResetEpo()

# 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
buttonResetEpo = 3                      # Joystick button number to perform an EPO reset (Start)
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

SPINNERgo = 12                           #Cross to spin PBR2
SPINNERstop = 14                         #Trinage STOP spin PBR2

# Power settings
voltageIn = 3.7 * 4                    # Total battery voltage to the PicoBorg Reverse
voltageOut = 12.0                       # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Setup pygame and wait for the joystick to become available
PBR1.MotorsOff()
PBR2.MotorsOff()
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, toggle the LED
                PBR1.SetLed(not PBR1.GetLed())
                PBR2.SetLed(not PBR2.GetLed())
                pygame.joystick.quit()
                time.sleep(0.5)
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick, toggle the LED
            PBR1.SetLed(not PBR1.GetLed())
            PBR2.SetLed(not PBR2.GetLed())
            pygame.joystick.quit()
            time.sleep(0.5)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print '\nUser aborted'
        PBR1.SetLed(True)
        PBR2.SetLed(True)
        sys.exit()
print 'Joystick found'
joystick.init()
PBR1.SetLed(False)
PBR2.SetLed(False)

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(buttonResetEpo):
                    PBR1.ResetEpo()
                if joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor

                if joystick.get_button(SPINNERgo):
                    print "TRIANGLE PRESSED"
                    PBR2.SetMotor2(1.0)
                elif joystick.get_button(SPINNERstop):
                    print "CROSS PRESSED"
                    PBR2.SetMotor2(0.0)

                # Set the motors to the new speeds
                PBR1.SetMotor1(driveRight * maxPower)
                PBR1.SetMotor2(-driveLeft * maxPower)
        # Change the LED to reflect the status of the EPO latch
        PBR1.SetLed(PBR1.GetEpo())
        PBR2.SetLed(PBR2.GetEpo())
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    PBR1.MotorsOff()
    PBR2.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    PBR1.MotorsOff()
    PBR2.MotorsOff()
print
piborg's picture

piborg

Thu, 11/03/2016 - 11:00

Firstly I would recommend changing:

if not PBR1.foundChip:

to:

if (not PBR1.foundChip) or (not PBR2.foundChip):

so that the script will stop before doing anything if it cannot find both Reverses.

The failsafe option is a little confusing, but in short you probably want it on for both boards.

Basically with the failsafe on the Reverse will automatically shut both motors down if it has been too long since it has heard from the Raspberry Pi.
This should protect against a number of possible problems:

  1. The Raspberry Pi is restarted for any reason, such as power failure.
  2. The script is forcibly terminated by something
  3. A script error causes it to crash
  4. The script itself stops sending commands

From memory the Reverse needs to see at least 4 messages each second to prevent the failsafe from activating.

To get the motors to turn off when the PS3 controller is out-of-range or disconnected we can either:

  1. Stop sending commands when there are no joystick events
  2. Add some code to check how long it has been since we saw an event and turn the motors off in the script

The easiest is option 1 and you can try it by indenting lines 186 to 188 once to the right.
This will mean the LEDs will only be changed when the PS3 controller sends a message.
The controller should send messages frequently enough to keep the failsafe happy from memory.

You can test by running the script with the robot lifted off the ground so it cannot move.
Run the script and hold/press something so the motors are running.
Now walk out of range with the motors still running, they should stop when the controller is too far away.
If you have someone else watching the robot the LEDs on both Reverses should be flashing to indicate the failsafe is engaged.

If you have trouble with option 1 or would rather try option 2 instead take a look at the code we used for running DoodleBorg:
DoodleBorg - Controlled using a PS3 controller
This has a specific set of checks to shut the motors down in the event that communication with the controller is lost.

METTAUK

Thu, 11/03/2016 - 18:48

Thanks again you have been really helpful

I now have it up and running and am impressed! But realise it moves way to fast! so want to reduce the power to the drive motors on PBR1 to around max 60% but not to PBR2 which needs to stay at 97-100%

The power setting seem to apply to both?

# Power settings
voltageIn = 3.7 * 4                    # Total battery voltage to the PicoBorg Reverse
voltageOut = 12.0                       # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

Or is it just a simple change like 

                # Set the motors to the new speeds
                PBR1.SetMotor1(driveRight * maxPower * 0.70)
                PBR1.SetMotor2(-driveLeft * maxPower * 0.70)
piborg's picture

piborg

Thu, 11/03/2016 - 19:04

The power setting should only apply if the motor setting line includes * maxPower.
Your alternative suggestion should also work fine.

METTAUK

Thu, 11/03/2016 - 19:24

I tried second option, the problem is with reduced power, turning becomes difficult! Is there a way to increase power only when turning, either tank or just turn?

piborg's picture

piborg

Fri, 11/04/2016 - 10:53

A simple fix is to choose the reduction depending on the outputs, for example replace:

# Set the motors to the new speeds
PBR1.SetMotor1(driveRight * maxPower * 0.70)
PBR1.SetMotor2(-driveLeft * maxPower * 0.70)

with

# Choose the current power level
if driveLeft == driveRight:
	# No turning applied, reduce power
	powerLevel = 0.70
else:
	# Turning, use more power
	powerLevel = 1.00
# Set the motors to the new speeds
PBR1.SetMotor1(driveRight * maxPower * powerLevel)
PBR1.SetMotor2(-driveLeft * maxPower * powerLevel)

METTAUK

Fri, 11/04/2016 - 15:27

Thank you so much, it works like a dream for tank steering, However, for minor direction changes it sort of takes off (200rpm motors with 135mm dia tyres so can hit about 1.4m/s or 3mph+.

Is there a way to modify the steering so that if say steer left or right is less than x maintain powerLevel for straight. Perhaps something with axisLeftRight without buttonFastTurn only allows say 0.75 power where buttonFastTurn of full turn keeps power at 100%. Is there a way to know what level of steering is being input from the PS3? full lock or minor adjustment?

Currently it woks well with;
0.55 for straight, still quite fast but OK
1.0 is good for tank steering or full lock turn
would like to add 0.9 for full turns depending on terrain
with
0.55 or a bit more power, say 0.65 for minor direction changes

I know my syntax is wrong but hope it explains what I'm trying to get to work but suspect i'm on the wrong track?

something like 

                # Choose the current power level
                if driveLeft == driveRight:
                   # No turning applied, reduce power
                   powerLevel = 0.55
                elif:
                  (axisLeftRight=>1) and (buttonFastTurn=<0):
                  powerLevel = 0.75
                else:
                   # Turning, use more power
                   powerLevel = 1.00
                # Set the motors to the new speeds
                PBR1.SetMotor1(driveRight * maxPower * powerLevel)
                PBR1.SetMotor2(-driveLeft * maxPower * powerLevel)
piborg's picture

piborg

Fri, 11/04/2016 - 16:29

You are on the right track, but the leftRight variable would be the best thing to test.
Putting the value through the abs function will remove the +/- differences and make it easier to check against.

For example:

# Choose the current power level
if abs(leftRight) < 0.1:
   # Driving nearly straight
   powerLevel = 0.55
elif abs(leftRight) < 0.5:
    # Simple steering
    powerLevel = 0.75
elif abs(leftRight) < 0.75:
    # Weak tank steering
    powerLevel = 0.85
else:
    # Hard tank steering
    powerLevel = 1.00
# Set the motors to the new speeds
PBR1.SetMotor1(driveRight * maxPower * powerLevel)
PBR1.SetMotor2(-driveLeft * maxPower * powerLevel)

METTAUK

Sat, 11/05/2016 - 10:35

Thank you so much, it works really well and I think I learnt something, which brings another question...

You helped me add a 2nd PicoBorg Reverse to run a separate motor from the main wheels, now I want to change the speed of that motor in relation to PBR1 so it slows down when PBR1 is turning. Something like...

If PBR1 turning then PBR2 left slows to 50%

piborg's picture

piborg

Sun, 11/06/2016 - 10:09

This is quite easy, we simply need to change the spinner output to behave like the left and right motors.

First make a new value for the spinner drive:

driveLeft = 0.0
driveRight = 0.0
driveSpinner = 0.0
running = True
hadEvent = False
upDown = 0.0
leftRight = 0.0

Next change the button code to change this value instead of the motor itself:

if joystick.get_button(SPINNERgo):
    print "TRIANGLE PRESSED"
    driveSpinner = 1.0
elif joystick.get_button(SPINNERstop):
    print "CROSS PRESSED"
    driveSpinner = 0.0

Now make a new power value using the same if block as the left/right power value:

# Choose the current power level
if abs(leftRight) < 0.1:
    # Driving nearly straight
    powerLevel = 0.55
    powerLevelSpinner = 1.00
elif abs(leftRight) < 0.5:
    # Simple steering
    powerLevel = 0.75
    powerLevelSpinner = 0.85
elif abs(leftRight) < 0.75:
    # Weak tank steering
    powerLevel = 0.85
    powerLevelSpinner = 0.75
else:
    # Hard tank steering
    powerLevel = 1.00
    powerLevelSpinner = 0.50

Finally set the motor using both values in the same way as the left and right motors get set:

# Set the motors to the new speeds
PBR1.SetMotor1(driveRight * maxPower * powerLevel)
PBR1.SetMotor2(-driveLeft * maxPower * powerLevel)
PBR2.SetMotor2(driveSpinner * powerLevelSpinner)

Now the spinner speed should update with how the robot is turning :)

Subscribe to Comments for &quot;Running two PicoBorg Reverse for different uses in one unit&quot;