Using UltraBorg and two Ultrasonic sensors to drive Diablo's two motors

Forums:

Please help! my code is not working
See my code below:-

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

# Import the libraries we need
import UltraBorg
import time
import Diablo

# Settings
distanceMin = 100.0             # Minimum distance in mm, corresponds to servo at -100%
distanceMax = 300.0             # Maximum distance in mm, corresponds to servo at +100%

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

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


# Motor sequence script

# Setup the Diablo
DIABLO = Diablo.Diablo()            # Create a new Diablo object
DIABLO.Init()                       # Set the board up (checks the board is connected)
DIABLO.ResetEpo()                   # Reset the stop switch (EPO) state
                                    # if you do not have a switch across the two pin header then fit the jumper

# Power settings
voltageIn = 7.4        # Total battery voltage to the Diablo
voltageOut = 7.2        # Maximum motor voltage
 
# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    # Initial settings
    servo1 = 0.0
    servo4 = 0.0
    while True:
        # Read all two ultrasonic values, we use the raw values so we respond quickly
        usm1 = UB.GetRawDistance1()
        usm4 = UB.GetRawDistance4()
        # Convert to the nearest millimeter
        usm1 = int(usm1)
        usm4 = int(usm4)
        # Generate the servo positions based on the distance readings
        if usm1 != 0:
            servo1 = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if servo1 > 1.0:
                servo1 = 1.0
            elif servo1 < -1.0:
                servo1 = -1.0
        if usm4 != 0:
            servo4 = ((usm4 - distanceMin) / distanceDiv) - 1.0
            if servo4 > 1.0:
                servo4 = 1.0
            elif servo4 < -1.0:
               servo4 = -1.0
        # Display our readings
        print '%4d mm -> %.1f %%' % (usm1, servo1 * 100.0)
        print '%4d mm -> %.1f %%' % (usm4, servo4 * 100.0)
        print
        # Set our new servo positions
        DIABLO.SetMotor1(maxPower * servo1)
        DIABLO.SetMotor2(maxPower * servo4)
      
        # Wait between readings
        time.sleep(.1)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    print 'Done'
Images: 
piborg's picture

This error usually indicates a problem talking to the Diablo itself.

The first thing to check is if you have an old or new version of Diablo.py. If you open the script in a text editor and find the import lines towards the top you should see:

# Import the libraries we need
import io
import fcntl
import types
import time

which means you have the new version which is fixed for recent copies of Raspbian.

If you see this instead:

# Import the libraries we need
import smbus
import types
import time

you are running the older version which does not work correctly with recent copies of Raspbian. In this case you can get the updated version of the script from here: https://forum.piborg.org/downloads/diablo/examples.zip

We already have the new version of the Diablo code which is fixed for recent copies of Rasbian. We are connecting the UltraBorg with the Diablo.

The Ultrasonic sensors connected to the UltraBorg are responding but the Diablo (motors) are not running. The servo pins connector are not connected to anything. On the terminal it is still reading (Failed sending motor 1 drive level) please help.

piborg's picture

Don't worry, we will figure out what the problem is :)

It would help if we can get the output from the initial connection code.

If you start Python from the Diablo directory:

cd ~/diablo
python

then run the following commands:

import Diablo
DIABLO = Diablo.Diablo()
DIABLO.Init()
print DIABLO.foundChip

what output do you get?

>>> import Diablo
>>> DIABLO = Diablo.Diablo()
>>> DIABLO.Init()
Loading Diablo on bus 1, address 37
Missing Diablo at 37
Diablo was not found
Are you sure your Diablo is properly attached, the correct address is used, and the I2C drivers are running?
>>> yes
Traceback (most recent call last):
File "", line 1, in
NameError: name 'yes' is not defined
>>> print DIABLO.foundChip
False
>>>

Images: 

Hi! the code works well, everything now works. I found out that there was a loosed 5v from a BattBorg to the Ultraborg. I believe this denied power to the I2C connection to the Diablo??
The only thing now is how do we use a keyboard command to stop the Diablo

piborg's picture

I am glad to hear you found the problem :)

The Diablo uses one of the 5V pins to power the logic chips on the board, which is why it would not communicate without it.

To stop the Diablo you need to call the DIABLO.MotorsOff() command. If you add it to the last section of the script like this:

except KeyboardInterrupt:
    # User has pressed CTRL+C
    DIABLO.MotorsOff()
    print 'Done'

then you can press CTRL+C and the script should stop both motors before printing done.

Can you help in modifying the code slightly please:-
I am using all 3 ultrasonic sensors to scan for the distance.
Ultrasonic no.2 is the main sensor.
Ultrasonic sensor 1,2 or 3 can stop the motors, the program then compare the distances collected from ultrasonic sensor 1 and 3.
If Ultrasonic sensor no.1 is closer to the obstacle, then the robot reverses and turns forward in the direction of ultrasonic sensor no.3
The robot then moves forward.
Else if Ultrasonic sensor no.3 is closer to the obstacle, then the robot reverses and turns forward in the direction of ultrasonic sensor no.1
The robot then moves forward.

Images: 

Any help here will be greatly appreciated. Thanks!

piborg's picture

What you can do is read the sensor values and then use if tests to determine what to do. There is a good explanation of how they work here: https://www.programiz.com/python-programming/if-elif-else

You can get the reverse and turn by performing a sequence of motor outputs with time delays. For example:

# Reverse at half-speed for 1 second
DIABLO.SetMotors(-maxPower * 0.5)
time.sleep(1.0)
DIABLO.MotorsOff()
# Turn left at full speed for half a second (toward sensor 3)
DIABLO.SetMotor1(-maxPower) # Left motors
DIABLO.SetMotor2(+maxPower) # Right motors
time.sleep(0.5)
DIABLO.MotorsOff()

The times and speeds can be adjusted to get the reverse distance and turn angle correct :)

Thank you for the steer in the right direction

Subscribe to Comments for &quot;Using UltraBorg  and two Ultrasonic sensors to drive Diablo&amp;#039;s two motors&quot;