Stew’s Spot

I bought a Lego Mindstorms kit last month to mess with, I’ve got some ideas for robot localisation that I want to try out. Anyway, the kit comes with a sonar range finder but I wanted a little more accuracy and smaller detection area. I’ve got a bunch of Sharp rangefinders from another project and they would work perfectly for what I wanted. A couple of companies sell after market sensors for the NXT, Mindsensors and HiTechnic but it’s much more fun to design and build your own.

For a little background on how the Sharp sensors work, Acroname Robotics has a good article on them.

I based the design on the following basic components. ATtiny45 microcontroller, Sharp (GP2D12, GP2Y0A41, GP2Y0A02, GP2Y0A710) and the NCP1400 Switching Regulator.

NXT Protocol

Pin	Name	Function	Colour	Pin Numbering
1	ANA	Analog interface, +9V Supply	white
2	GND	Ground	black
3	GND	Ground	red
4	IPOWERA	+4.3V Supply	green
5	DIGIAI0	I2C Clock (SCL), RS-485 A	yellow
6	DIGIAI1	I2C Data (SDA), RS-485 B	blue

NXT Connector (wikipedia)

The NXT communicates with intelligent sensors via either a I2C or RS-485 bus, I’m using I2C for the rangefinder. The two images bellow show the prototype setup, AVR Dragon to program the ATtiny, buspirate to sniff and inject I2C communication and a hacked together NXT cable.

System design, Schematics

Click for PDF

Overall a very simple design, there are a few aspects that I’ll touch on.

The Sharp sensor draws around 30mA when running, I’ve included a FDV303N N-Channel FET to allow the sensor to be powered down when not in use.

The NXT brick supplies between 4.3 – 4.7 volts, this varies depending on the battery voltage and what other sensors are attached.  The Sharp sensor starts outputting weird ranges if the voltage drops below 4.5V so I decided to add a boost converter to supply the board with a stable 5V supply. The converter is based around the NCP1400: 100 mA, Fixed Frequency PWM, Step−Up Micropower Switching Regulator. I haven’t tested the efficiency of the converter  but the datasheet specifies it at around 90% with 30mA load.

PCB

The PCBs where manufactured by Betalayout PCB-Pool. Just a tip if you use them, send them the plain Gerber files and not any other formats. I sent the EagleCAD .brd files and there was a bunch of documentation on the tDocu and bDocu layers that got lost in the conversion.

You can buy the funny offset RJ12 jacks from Mindsensors.

Firmware

The firmware is very simple. It consists of of some initialisation code, Interrupts to handle the I2C bus and free running ADC and a main loop running “User Space” code.

The I2C driver was created from Atmel source files for Application Note “AVR312: Using the USI Moduleas an I2C slave” by Donald R. Blake and is very easy to use. See the code snippet below for the interface

void    usiTwiSlaveInit( uint8_t );
void    usiTwiTransmitByte( uint8_t );
uint8_t usiTwiReceiveByte( void );
bool    usiTwiDataInReceiveBuffer( void );

This is a basic flow of the main loop.

initHardware();
usiTwiSlaveInit(I2C_ADDRESS);

while(1)
{
    if(usiTwiDataInReceiveBuffer())
    {
        uint8_t reg = usiTwiReceiveByte();
        switch (reg)
        {
            case (SOMETHING):
            // Do something
            usiTwiTransmitByte(result);
            break;
        }
        nop(); // or we could sleep, the I2C driver is interrupt driven
    }
}

The interface to the Sharp Sensors. A free running ADC updates the raw ADC value and from that the Voltage and Range can be calculated.

void sharpInit();
void sharpEnable();
void sharpDisable();
uint16_t sharpGetVoltage();
uint16_t sharpGetRange();
uint16_t sharpGetRaw();
uint8_t sharpType();

Calibration

The Sharp sensors don’t output a linear voltage/distance, you can either work out a fitted function and recalculate the values on the fly or pre-calculate and use a lookup table.  I’ve gone with the lookup table.

I wrote a small python program (see the calibration directory for the code) to take a set of calibration readings, interpolate the data and compute the lookup table.  You can sellect the type of outputted data.  Output data suitable for graphing the results as seen below or

Generate a .h header file.

PROGMEM  prog_uint16_t adc_to_range[]  = {
    65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535,
    65535, 65535, 65535, 798, 792, 788, 782, 779,
    775, 769, 766, 761, 757, 753, 750, 747,
    742, 740, 736, 734, 730, 728, 726, 724,
    ...
    0, 0, 0, 0, 0, 0, 0, 0};

The 1024 entry lookup table is stored in flash program memory and can be read with the pgm_read_word function.

uint16_t sharpGetRange()
{
    // _adc_value is the current raw ADC value [0..1023]
    return pgm_read_word(adc_to_range + _adc_value);
}

leJOS Driver

I’m running the leJOS firmware replacement on the NXT Brick.

leJOS (pronounced like the Spanish word “lejos” for “far”) is a tiny Java Virtual Machine. In 2006 it was ported to the LEGO NXT brick.

leJOS NXJ includes all the classes in the NXJ API as well as the tools used to upload code to the NXT brick.

I’ve included a driver to interface with the rangefinder.

public static void main(String[] args)
{
	OpticalRangefinder range = new OpticalRangefinder(SensorPort.S1);

	System.out.println("Ver: " + range.getVersion());
	System.out.println("ID: " + range.getProductID());
	System.out.println("Type: " + range.getSensorType());
	System.out.println("Mod: 0x" + Integer.toHexString(range.getSensorModule()));

	while (!Button.ESCAPE.isPressed())
	{
		LCD.drawString("Range: " + range.getDistance() + "mm", 0, 0);
	}

	range.powerOff();
}

Results

The range data seems to be quite accurate, I’ve not had time to calibrate all the sensor types yet, but will be doing that as I start using them.

Excuse the ADC: 0194mmm line in the image above, it’s supposed to read Range: 0194mm.

Download

You can download the Schematics, PCB layout, Firmware and leJOS Driver from the git repository.

git clone git://stewartallen.org/mindstorms_optical_rangefinder.git

or you can just browse the repository.

Licenses

Optical Rangefinder schematics and PCB layouts are licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.

The firmware and drivers are licensed under the GNU GENERAL PUBLIC LICENSE version 3.0 or later.