Use UltraBorg to control DiggyBorg

Forums:

I'd like to use an ultra sensor to let my DiggyBorg know if is too near to an object and automatically change its direction. Can I use the UltraBorg to control the speed of motors of DiggyBorg over Raspberry Py instead of controlling a step motor?

piborg's picture

Yes you can do this.

You can use the UltraBorg readings to decide on what the power levels to the motors should be.
Then you can give these values to the PicoBorg Reverse to set the drive output.

There is an example below which should get DiddyBorg to try and stay about 200 mm away from an object in front of him.
This will require a forward facing distance sensor attached to connector #1.
There is a link at the bottom of this post so you can download the script if you wish.

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

# Import the libraries we need
import UltraBorg
import PicoBorgRev
import time
import math
import sys

# Settings
distanceMin = 100.0             # Minimum distance in mm, corresponds to DiddyBorg reversing at 100%
distanceMax = 300.0             # 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)

# Setup the PicoBorg Reverse
PBR = PicoBorgRev.PicoBorgRev()
#PBR.i2cAddress = 0x44                  # Uncomment and change the value if you have changed the board address
PBR.Init()
if not PBR.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:' % (PBR.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 'PBR.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
PBR.SetCommsFailsafe(False)             # Disable the communications failsafe
PBR.ResetEpo()

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

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

# Calculate our divisor
distanceDiv = (distanceMax - distanceMin) / 2.0

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    # Initial settings
    speed = 0.0
    driveLeft = 0.0
    driveRight = 0.0
    while True:
        # Read all four ultrasonic values, we use the filtered values so we get accurate readings
        usm1 = UB.GetDistance1()
        usm2 = UB.GetDistance2()
        usm3 = UB.GetDistance3()
        usm4 = UB.GetDistance4()
        # Convert to the nearest millimeter
        usm1 = int(usm1)
        usm2 = int(usm2)
        usm3 = int(usm3)
        usm4 = int(usm4)
        # Determine the speed of DiddyBorg based on the distance readings
        if usm1 != 0:
            speed = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if speed > 1.0:
                speed = 1.0
            elif speed < -1.0:
                speed = -1.0
        driveLeft = speed
        driveRight = speed
        # Set our new speed
        PBR.SetMotor1(driveRight * maxPower)
        PBR.SetMotor2(-driveLeft * maxPower)
        # Wait between readings
        time.sleep(.1)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    PBR.MotorsOff()
    print 'Done'
Attachments: 

Have been tinkering and have added 3 USM sensors to the front, one in the middle and two either side at an angle (left and right) with the idea that the centre will check distance as in the diddyfromdistance (DFD) script with added left and right helping to avoid obstacle by informing steering?

I appreciate there must be zillions of ways to do this but i,m struggling to even get started
USM1 is middle
USM2 is leff
USM3 is right

Following the idea of

distanceMin = 250.0
distanceMax = 450.0

#Add left and right

LeftMin = 200
RightMin = 200

#snip

        if usm1 != 0:
            speed = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if speed > 1.0:
                speed = 1.0
            elif speed < -1.0:
                speed = -1.0
        driveLeft = speed
        driveRight = speed
        # Set our new speed

# Turn one way while less and less than LeftMin

            elif usm2 < usm1 and usm2 < LeftMin: 
        PBR.SetMotor1(driveRight * maxPower)
        PBR.SetMotor2(driveLeft * maxPower)

# Turn the other way while less and less than RightMin

            elif usm3 < usm1 and usm3 < RightMin:
        PBR.SetMotor1(-driveRight * maxPower)
        PBR.SetMotor2(driveLeft * maxPower)

# Back to standard distance movement

        PBR.SetMotor1(driveLeft * maxPower)
        PBR.SetMotor2(-driveRight * maxPower)
        # Wait between readings
        time.sleep(.05)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    PBR.MotorsOff()
    print 'Done'

I know this does not work but my very limited knowledge is at its limit!

Images: 
piborg's picture

I think you are most of the way there already.

You will probably want a new if block for deciding which way to turn.
The standard movement should probably by in an else block to ensure it only runs if none of the turning conditions are met.

This would look like:


#snip

        if usm1 != 0:
            speed = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if speed > 1.0:
                speed = 1.0
            elif speed < -1.0:
                speed = -1.0
        driveLeft = speed
        driveRight = speed
        # Set our new speed based on all three sensors

        # Turn one way while less and less than LeftMin
        if usm2 < usm1 and usm2 < LeftMin: 
            PBR.SetMotor1(driveRight * maxPower)
            PBR.SetMotor2(driveLeft * maxPower)

        # Turn the other way while less and less than RightMin
        elif usm3 < usm1 and usm3 < RightMin:
            PBR.SetMotor1(-driveRight * maxPower)
            PBR.SetMotor2(driveLeft * maxPower)

        # For anything else use the standard distance movement
        else:
            PBR.SetMotor1(driveLeft * maxPower)
            PBR.SetMotor2(-driveRight * maxPower)

        # Wait between readings
        time.sleep(.05)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    PBR.MotorsOff()
    print 'Done'

Following your very speedy help...

It works! except it will turn left to avoid but not right?

Also I had to add usm2 and usm3 to the if usm1 != 0: part of it kept crashing leaving the motors still running!

piborg's picture

Good call with the != 0 change :)

I think you want to see if left or right is smaller.
With the current code it will always turn the same way if both are below their minimums.

I would check if a turn is needed first, then see if sensor 2 or 3 is the lower reading to make the choice.

This would look like:

#snip

		# Set our new speed based on all three sensors

		# See if we should try and turn
		if usm2 < usm1 or usm3 < usm1:

			# Turn one way while less and less than LeftMin
			if usm2 < usm3 and usm2 < LeftMin: 
				PBR.SetMotor1(driveRight * maxPower)
				PBR.SetMotor2(driveLeft * maxPower)

			# Turn the other way while less and less than RightMin
			elif usm3 < usm2 and usm3 < RightMin:
				PBR.SetMotor1(-driveRight * maxPower)
				PBR.SetMotor2(driveLeft * maxPower)

			# For anything else use the standard distance movement
			else:
				PBR.SetMotor1(driveLeft * maxPower)
				PBR.SetMotor2(-driveRight * maxPower)

		# No need to turn
		else:
			PBR.SetMotor1(driveLeft * maxPower)
			PBR.SetMotor2(-driveRight * maxPower)

		# Wait between readings
		time.sleep(.05)
except KeyboardInterrupt:
	# User has pressed CTRL+C
	PBR.MotorsOff()
	print 'Done'

I still cant get him to turn right, he just backs off?

The code ammended DFD python is here

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

# Import the libraries we need
import UltraBorg
import PicoBorgRev
import time
import math
import sys

#  Settings
distanceMin = 200.0             # Minimum distance in mm, corresponds to DiddyBorg reversing at 100%
distanceMax = 500.0             # Maximum distance in mm, corresponds to DiddyBorg driving at 100%
#LeftMin = 200
#RightMin = 200
#Replace with TurnMin
TurnMin = 200

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

# Setup the PicoBorg Reverse
PBR = PicoBorgRev.PicoBorgRev()
#PBR.i2cAddress = 0x44                  # Uncomment and change the value if you have changed the board address
PBR.Init()
if not PBR.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:' % (PBR.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 'PBR.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
PBR.SetCommsFailsafe(False)             # Disable the communications failsafe
PBR.ResetEpo()

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

print voltageIn

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

# Calculate our divisor
distanceDiv = (distanceMax - distanceMin) / 2.0

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    # Initial settings
    speed = 0.0
    driveLeft = 0.0
    driveRight = 0.0
    while True:
        # Read all four ultrasonic values, we use the filtered values so we get accurate readings
        usm1 = UB.GetDistance1()
        usm2 = UB.GetDistance2()
        usm3 = UB.GetDistance3()
        #usm4 = UB.GetDistance4()
        # Convert to the nearest millimeter
        usm1 = int(usm1)
        usm2 = int(usm2)
        usm3 = int(usm3)
        #usm4 = int(usm4)
        # Determine the speed of DiddyBorg based on the distance readings
        print '     LEFT     MIDDLE     RIGHT'
        print 'USM2 ',usm2, 'USM1 ',usm1, ' USM3 ',usm3
        if usm1 and usm2 and usm3 != 0:
            speed = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if speed > 1.0:
                speed = 1.0
            elif speed < -1.0:
                speed = -1.0
        driveLeft = speed
        driveRight = speed
        # Set our new speed based on all three sensors
 
        # Turn one way while less and less than LeftMin
        if usm2 < usm1 or usm3 < usm1:
 
            # Turn one way while less and less than TurnMin
            if usm2 < usm3 and usm2 < TurnMin: 
                PBR.SetMotor1(driveRight * maxPower)
                PBR.SetMotor2(driveLeft * maxPower)
 
            # Turn the other way while less and less than TurnMin
            elif usm3 < usm2 and usm3 < TurnMin:
                PBR.SetMotor1(-driveRight * maxPower)
                PBR.SetMotor2(driveLeft * maxPower)
 
            # For anything else use the standard distance movement
            else:
                PBR.SetMotor1(driveLeft * maxPower)
                PBR.SetMotor2(-driveRight * maxPower)
 
        # No need to turn
        else:
            PBR.SetMotor1(driveLeft * maxPower)
            PBR.SetMotor2(-driveRight * maxPower)
 
        # Wait between readings
        time.sleep(.05)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    PBR.MotorsOff()
    print 'Done'

I might have left and right a bit mixed but either way he won't turn right or perhaps better put as only clearly turning one way. But does 'randomly'? turn the other way a bit!!

Also think I might not be dealing with if all are below minimum he can try to keep going forward?

piborg's picture

It might be a simple case of the two bits of logic behave strangely together.

I would try simply setting a fixed speed and see if the turning logic works on its own.
To do this comment out the line:
speed = ((usm1 - distanceMin) / distanceDiv) - 1.0
and in its place set a fixed value:
speed = 1.0

If the turning works with the change above then it is just the two methods of control fighting each other.
If it does not work at all then the steering logic is probably not doing its job.

Thanks tried that and nothing changes aside from speed.

I added in some feedback

        # Turn while usm2 or usm3 are less than usm1 (middle)
        if usm2 < usm1 or usm3 < usm1:
 
            # Turn 1, USM2 less than USM3 and less than TurnMin
            if usm2 < usm3 and usm2 < TurnMin: 
                PBR.SetMotor1(-driveRight * maxPower)
                PBR.SetMotor2(driveLeft * maxPower)
                print 'turn 1 USM2 less than USM3'

            # Turn 2, turn the other way while USM3 less than USM2 and less than TurnMin
            elif usm3 < usm2 and usm3 < TurnMin:
                PBR.SetMotor1(driveRight * maxPower)
                PBR.SetMotor2(driveLeft * maxPower)
                print 'turn 2 USM3 less than USM2'
 
            # For anything else use the standard distance movement
            else:
                PBR.SetMotor1(driveLeft * maxPower)
                PBR.SetMotor2(-driveRight * maxPower)
                print 'go straight 1'
 
        # No need to turn
        else:
            PBR.SetMotor1(driveLeft * maxPower)
            PBR.SetMotor2(-driveRight * maxPower)
            print 'go straight 2'
        # Wait between readings
        time.sleep(.03)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    PBR.MotorsOff()
    print 'Done'

When running him screen output is;

pi@METTA:~/metal2 $ python DFD05.py
Loading UltraBorg on bus 1, address 36
Found UltraBorg at 36
UltraBorg loaded on bus 1
Loading PicoBorg Reverse on bus 1, address 44
Found PicoBorg Reverse at 44
PicoBorg Reverse loaded on bus 1
14.4
Press CTRL+C to finish

     LEFT     MIDDLE     RIGHT
USM2  136 USM1  328  USM3  388
turn 1 USM2 less than USM3
     LEFT     MIDDLE     RIGHT
USM2  136 USM1  328  USM3  388
turn 1 USM2 less than USM3
snip
     LEFT     MIDDLE     RIGHT
USM2  368 USM1  414  USM3  162
go straight 1
     LEFT     MIDDLE     RIGHT
USM2  368 USM1  414  USM3  162
go straight 1
snip
     LEFT     MIDDLE     RIGHT
USM2  387 USM1  426  USM3  145
turn 2 USM3 less than USM2
     LEFT     MIDDLE     RIGHT
USM2  503 USM1  497  USM3  52
turn 2 USM3 less than USM2
^CDone
pi@METTA:~/metal2 $

it runs backwards (retreats) rather than turning? to the section #Turn 1 seems to be the problem?

Is
PBR.SetMotor1(-driveRight * maxPower)
PBR.SetMotor2(driveLeft * maxPower)
correct?

It seems

                PBR.SetMotor1(driveRight * maxPower)
                PBR.SetMotor2(driveLeft * maxPower)

works fine, he turns, but

                PBR.SetMotor1(-driveRight * maxPower)
                PBR.SetMotor2(driveLeft * maxPower)

simply sends him backwards????

piborg's picture

I think the correct versions will be:

# Forwards
PBR.SetMotor1(+driveLeft  * maxPower)
PBR.SetMotor2(-driveRight * maxPower)

# Backwards
PBR.SetMotor1(-driveLeft  * maxPower)
PBR.SetMotor2(+driveRight * maxPower)

# Spin Left
PBR.SetMotor1(-driveLeft  * maxPower)
PBR.SetMotor2(-driveRight * maxPower)

# Spin Right
PBR.SetMotor1(+driveLeft  * maxPower)
PBR.SetMotor2(+driveRight * maxPower)

The + symbols are not actually needed, but I think they probably make the code a bit clearer.

PBR.SetMotor1(driveRight * maxPower)
PBR.SetMotor2(driveLeft * maxPower)

works fine, he turns left

PBR.SetMotor1(-driveRight * maxPower)
PBR.SetMotor2(-driveLeft * maxPower)

Turns him RIGHT!

How can I wire the UltraBord with Raspberry Py without using the Daisy Chain? The BattBorg Board and the PicoBorg Reverse already use these port (1 to 6). (Typo)

piborg's picture

You can still use the daisy-chain connection with all three boards.
Basically the BattBorg needs to be connected to the last board in the chain.

Option 1, Connect the UltraBorg between the Raspberry Pi and the PicoBorg Reverse:

  • Raspberry Pi GPIO → UltraBorg 6-pin header
  • UltraBorg daisy-chain connector → PicoBorg Reverse 6-pin header
  • PicoBorg Reverse daisy-chain connector → BattBorg mounted with post

Option 2, mount the BattBorg on the UltraBorg instead:

  • Raspberry Pi GPIO → PicoBorg Reverse 6-pin header
  • PicoBorg Reverse daisy-chain connector → UltraBorg 6-pin header
  • UltraBorg daisy-chain connector → BattBorg mounted with post
sigpaw@hotmail.com's picture

I notice a couple of extra holes in the Diddy base plate. But I suspect those are left over to allow different RasPi models to be mounted...

I'm thinking I will mount my "larghish" battery on the Diddy top panel (using Velcro) to facilitate easier/quicker battery swaps. My battery pack has a HXT 4mm connector (http://www.amazon.com/Turnigy-5000mAh-4S1P-14-8v-hardcase/dp/B00FXL7XJC?...), so I ordered some spare, blank HXT connectors from Amazon and made up a cable that replaces the 9v connector that came with Diddy.

This leaves the main deck for mounting all my accessories. I'm still waiting for my GPS module to be delivered (although I suspect I will have to mount it on the top panel to give the antenna best line of sight to geo-orbit). I'm thinking I will mount a small stepper motor and control it from a Pico or perhaps a Pico Reverse. The stepper shaft will go up through the top panel to a TMP007 IR Thermopile sensor mounted on a parabolic reflector - the top of the same shaft will also have a HC-SR04 on it.

So I need to think out where everything will best fit. I guess the first order of business is how to mount the stepper.
I need to mount it bottom down so that the stepper shat is perpendicular to the base plate, able to stick up through and perpendicular to the top plate.

Any ideas?

Hy
Is my wiring with the option 1 correct?

And were are the servo connections going to?
What im doing with the jumper?

Images: 
piborg's picture

The wiring between the UltraBorg and PicoBorg Reverse is correct.

Without being able to see the connections on the Raspberry Pi I cannot be sure if the wiring between the UltraBorg and Raspberry Pi is correct or not.

I hope this helps!

Images: 
piborg's picture

That helps, thanks :)
It all looks good to me, everything should work correctly wired like that.

Hello
Somethings seems not correctly.
If i power it with this wiring, the Raspi doesnt start. If i disconect the UltraBorg the RP3 starts.
Could it bee that im not load the UltraBorg script yet!

piborg's picture

I am guessing there is too much connected to power everything from a single BattBorg.
In particular servos can demand a fair amount of power even when they are not moving.

I would remove the 5V link jumper on the UltraBorg (green dashed line in the image below) and see if everything powers up.
If it does you will need another 5V power source for the servos / ultrasonics connected to the V+ and GND on the UltraBorg itself.

Do i need now two power sources 12V for Diddyborg and 5V for the Ultra Borg at the same time?
Do i need to disconnect a V+ cable from a board?

piborg's picture

If you are trying to drive servos from the UltraBorg then you will need a separate 5V source for the servos / ultrasonic modules.
This is because the BattBorg cannot supply enough current to supply everything.

You can use a UBEC to power the servos, or even a second BattBorg may work for lower power servos.
I have attached a diagram below for how the power would need to be connected.

Images: 

Hello
I use two different sources without BEC!
One 12V for the Diddyborg and one 5V for the Ultra Borg!

Images: 

It works like a charm, although I have some troubles by as I try to fix the UltraBorg inside the DiddyBorg.
I fixed it by binding the two rings, or what every is called in English, with the think black cable tie.
Thank you for your very fast response, and I wish I a nice holiday.

Images: 

Hy
I will know if i need a Ultra Borg to make the same Vehicle? I still have an DiddyBorg!
Greats from Switzerland.

Yes, I use a Ultra Borg.

Hello bui_hong_phuc
Is it possible that you upload your script?
Thank you most kindly!

Greats from Switzerland 🇨🇭

Hello
Does someone have the hole functionall script?

Subscribe to Comments for &quot;Use UltraBorg to control DiggyBorg&quot;