Problem Ultrasonic Sensor

ubik75 · January 29, 2014, 12:56pm

Hello,

I tried out this program:

from BrickPi import * #import BrickPi.py file to use BrickPi operations

BrickPiSetup() # setup the serial port for communication

BrickPi.SensorType[PORT_1] = TYPE_SENSOR_ULTRASONIC_CONT #Set the type of sensor at PORT_1

BrickPiSetupSensors() #Send the properties of sensors to BrickPi

while True:
result = BrickPiUpdateValues() # Ask BrickPi to update values for sensors/motors
if not result :
print BrickPi.Sensor[PORT_1] #BrickPi.Sensor[PORT] stores the value obtained from sensor
time.sleep(.01) # sleep for 10 ms

The values that I got, seems ok but some of them are looking strange : they are more than *4 the real value.
For example:
177
177
49 <- real distance
49
49
49
49
177
177
49
49

Is my sensor break or Is my BrickPi looking bad or something else ?
ps : I’m sorry for my english, I’m french and i have some diificulties to speak english very well.

Thanks

Ubik75

frans · January 31, 2014, 7:36am

Hello Ubik75,
The ability of the ultrasonic sensor to correctly measure the distance to an object in front of it depends greatly on that object. Hard flat surface objects will generally give good results. Soft or small objects may not be ‘seen’ at all.
Also, the ultrasonic sensor needs 9V to work properly. How is your BrickPi powered?

ubik75 · February 2, 2014, 5:18am

Hello Frans

I checked all you told me with the BrickPi and i tested the sensor with NXT controller, the sensor looks right with the NXT controller. Plus my BrickPi is powered with 8X 1,5V and the Raspberry is powered with a 5V battery.

You could go and see my site to watch my NXT: http://ubik75.com/?p=1217 (this is a french site).

In my minds, the acquisition of the measure by the program doesn’t run correctly.

PS: If you want to correct my english, do it, it will improve myself.

Thanks
Ubik75

frans · February 3, 2014, 10:11am

Hello Ubik75,
Have you tried a slower polling rate by increasing the sleep time in the test program? The firmware code ‘BrickPiUS.h’ contains the following comment:

// does indeed require 15ms between readings.

I have two US sensors. I'll test both of them soon and let you know how they behave.

janjachnik · February 5, 2014, 4:39am

I have also noticed this problem with multiple Ultrasonic sensors and multiple BrickPi’s.

I have noticed that the error is always 128. Notice the example above: 177 - 49 =128.

I believe this must be some kind of data corruption. I’ve checked the message received by the Pi and it corresponds to the value outputted. Therefore, I believe it is a problem in the firmware of the BrickPi.

If I had time I would compile the I2C part of the BrickPi into a simple Arduino sketch and see if I get the same behaviour. I know the I2C for the US is not standard protocol, maybe something is not functioning quite right there? Something to do with the most significant bit not being received properly.

frans · February 5, 2014, 4:13pm

Hello again,
I did some tests with my US sensors and found that both indeed return the wrong distance occasionally. However, in my case these are not the small distances returned too large, but the 128+ distances returned too small. The difference, as janjachnik point out, being 128 (-128 in my case).

I took a look at the firmware code to see how the US sensor is controlled. It is basically a I2C sensor with some special timing requirements and non-standard clock-pulse between write and read. Fortunately, the firmware has been designed to support this kind of special I2C sensors without a dedicated firmware implementation. So, you can write your own driver in C or Python running on the RPi.

Here is my python implementation for the Lego US sensor:

from BrickPi import *
import sys

I2C_PORT = PORT_1
I2C_SPEED = 8        #define US_I2C_WAIT 7
I2C_DEVICE_INDEX = 0

LEGO_US_I2C_ADDR = 0x02

LEGO_US_CMD_REG = 0x41
LEGO_US_CMD_OFF = 0x00
LEGO_US_CMD_SS = 0x01
LEGO_US_CMD_CONT = 0x02
LEGO_US_CMD_EVNT = 0x03
LEGO_US_CMD_RST = 0x04

LEGO_US_DATA_REG = 0x42

BrickPiSetup()

BrickPi.SensorType[I2C_PORT] = TYPE_SENSOR_I2C_9V
BrickPi.SensorI2CSpeed[I2C_PORT] = I2C_SPEED
BrickPi.SensorI2CDevices[I2C_PORT] = 1

BrickPi.SensorSettings[I2C_PORT][I2C_DEVICE_INDEX] = BIT_I2C_MID | BIT_I2C_SAME
BrickPi.SensorI2CAddr[I2C_PORT][I2C_DEVICE_INDEX] = LEGO_US_I2C_ADDR

BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_INDEX]    = 1
BrickPi.SensorI2CRead  [I2C_PORT][I2C_DEVICE_INDEX]    = 1
BrickPi.SensorI2COut   [I2C_PORT][I2C_DEVICE_INDEX][0] = LEGO_US_DATA_REG

if(BrickPiSetupSensors()):
  print &quot;BrickPiSetupSensors failed.&quot;
  sys.exit( 0 )

while True:
    result = BrickPiUpdateValues()
    if not result:
        if(BrickPi.Sensor[I2C_PORT] &amp; (0x01 &lt;&lt; I2C_DEVICE_INDEX)):
            dist = BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_INDEX][0]
            print dist
    time.sleep(.01)

Note that the I2C_SPEED value is actually a ‘speed limit’. A higher value increases the wait time between writes and reads from the sensor. In the firmware implementation it is set to 7. By increasing it to 8, I found that the wrong distance readings were gone.
I hope this helps for you too. Please, post your results.

ubik75 · February 6, 2014, 12:10pm

Hello

Thanks Frans for your program, i tested I2C_SPEED = 8 (it’s OK) and I2C_SPEED = 7 (it’n not OK).
Where did you found the LEGO_CMD values ?

Thanks
Ubik75

frans · February 6, 2014, 5:05pm

Hi Ubik75,
Good to see that it solved your problem
The command values are listed in the firmware header file BrickPiUS.h.
Note that I didn’t send a mode command to the US sensor in the test program. It looks like continuous mode is the default mode of the sensor, and as far as I know it is the only mode implemented by the firmware.

I think SS stands for ‘single shot’ or ‘signal source’, making it operate like a US source, so send out a pulse, but don’t listen for echos. However I’m not sure if or how this works.

EVNT then probably means ‘event’ and I guess that puts the sensor in a listening mode, so only picking up US signals, not sending them out. Again this is just a guess and I don’t know how it works. It would make sense though, because it would allow two nxt robots to communicate by sending US pulses to and receiving US pulses from each other. I’ll give it a try soon with my two US sensors.

RST probably means ‘reset’. But again I’m not sure.

janjachnik · February 7, 2014, 1:49pm

That is great Frans, I now have working US sensors!

I modified BrickPi.py so that TYPE_SENSOR_ULTRASONIC_CONT actually implements TYPE_SENSOR_I2C using the the code you posted.

This is great because you don’t need to change any code in your existing programs!

Here is the link if you want to try it out:
hacked BrickPi.py

It’s a shame that the value on the BrickPi is hard coded. I’m not going to try updating the firmware to fix it. This workaround works just fine.

Matt · February 7, 2014, 5:02pm

According to the comments I made in the file, it was tested to work with a US_I2C_WAIT value of 3, so I set it to 7 to play it safe. Perhaps there is a difference between the HW I was using, and the HW you are having trouble with (e.g. Ultrasonic sensor revision).

Using the I2C drivers instead of the built in Ultrasonic sensor drivers certainly does offer more configuration options.

Please note that the 15ms time between Ultrasonic sensor updates is specified in the Lego NXT Ultrasonic sensor documentation. If you try to update faster than that, you will get inaccurate results.

ubik75 · February 26, 2014, 1:58pm

Hello

My first robot with Bricki; Robot pilotable avec une caméra

frans · February 28, 2014, 6:24am

Nice 1. The smooth camera stream will probably be very useful to others with a pi-cam as well.