Maxbotix analog programming

[tuXguy15]

Does anyone have any example code of getting the distance to something and driving that distance with a macho tis range finder?

[irvingc]

What you're asking is actually not so simple! Just getting the distance reading from the sensor can be somewhat complicated, depending on the interface. Here's some code I dug up from the last time we used an ultrasonic sensor that connected to the I2C bus (this was before we switched to the command-based framework):

import edu.wpi.first.wpilibj.*;
import java.util.TimerTask;

public class NRGUltrasonic
{
    public static final boolean USING_ULTRASONIC = false;
    private Timer timer;
    private java.util.Timer controlLoop;
    private DigitalModule digMod;
    private I2C i2c;
    private static final int ULTRASONIC_SLOT = 4;
    private static final int ULTRASONIC_ADDRESS = 0xE0;
    private static final int ECHO_ADDRESS_HIGH = 2;
    private static final int ECHO_ADDRESS_LOW = 3;
    private int dist;
    private double period;
    private boolean writeFail;

    private class UltrasonicTask extends TimerTask
    {
	private NRGUltrasonic u;

	public UltrasonicTask(NRGUltrasonic ultrasonic)
	{
	    if (ultrasonic == null)
	    {
		throw new NullPointerException("Given Ultrasonic was null");
	    }
	    this.u = ultrasonic;
	}

	public void run()
	{
	    u.setDist();
	}
    }

    public NRGUltrasonic()
    {
	dist = 0;
	writeFail = false;
	if(USING_ULTRASONIC)
	{
	    digMod = DigitalModule.getInstance(ULTRASONIC_SLOT);
	    i2c = digMod.getI2C(ULTRASONIC_ADDRESS);
	    timer = new Timer();
	    controlLoop = new java.util.Timer();
	    if (i2c.addressOnly())
	    {
		NRGLCD.println4("ERR: No Ultrasonic!");
		Debug.println(Debug.FATAL_EXCEPTIONS, "ERROR: Could not find Ultrasonic.");
	    }
	    //Range: Three meters.
	    i2c.write(0x02, 0x46);
	    //Analogue Gain: 317
	    i2c.write(0x01, 0x18);
	    period = .05;
	    setDist();
	}
    }

    /**
     * Ends the control loop.
     */
    public void free()
    {
	controlLoop.cancel();
	controlLoop = null;
    }

    public int getDist()
    {
	return dist;
    }

    public boolean getWriteFail()
    {
	return writeFail;
    }

    //gets distance
    private void setDist()
    {
	byte high, low;

	try
	{
	    //setting range to centimeters
	    if (i2c.write(0x00, 0x51))
	    {
		writeFail = true;
		Debug.println(Debug.ULTRASONIC, "Attemted write to ultrasonic, failed.");
	    }
	    else
	    {
		writeFail = false;
		Debug.println(Debug.ULTRASONIC, "Wrote to ultrasonic, began rangefinding in centimeters.");
		timer.reset();
		timer.start();

		try
		{
		    boolean done = false;
		    Timer.delay(.065);
		    for (int i = 0; i < 10; i++)
		    {
			byte[] buffer = new byte[1];
			if (i2c.read(ECHO_ADDRESS_HIGH, 1, buffer))
			{
			    //Debug.println(Debug.ULTRASONIC, "Attempted read to ultrasonic high, failed.");
			}
			else
			{
			    high = buffer[0];
			    if (i2c.read(ECHO_ADDRESS_LOW, 1, buffer))
			    {
				//Debug.println(Debug.ULTRASONIC, "Attempted read to ultrasonic low, failed.");
			    }
			    else
			    {
				low = buffer[0];
				dist = high * 256 + ((int) low & 0xFF);
				Debug.println(Debug.ULTRASONIC, "Try: " + i + " Time: " + timer.get() + " High: " + high + " Low: " + low + " Dist: " + dist);
				done = true;
			    }
			}
			if (done)
			    break;
			//Debug.println(Debug.ULTRASONIC, "Try "+i+" failed.");
			if (i == 9)
			    Debug.println(Debug.ULTRASONIC, "TRIED 10 TIMES!");
			Timer.delay(.02);
		    }
		}
		catch (Exception e)
		{
		    Debug.println(Debug.FATAL_EXCEPTIONS, "Attempt to read ultrasonic caused a fatal exception.");
		}
	    }
	}
	catch (Exception e)
	{
	    Debug.println(Debug.FATAL_EXCEPTIONS, "Attempt to write to ultrasonic caused a fatal exception");
	}
	//Schedule next loop.
	controlLoop.schedule(new UltrasonicTask(this), (long) (period * 1000));

    }
}

And even after you know your distance, it's really not so easy to just drive directly at it. What type of drive train are you running? If it's a tank drive - does your robot drive straight if you just feed the same power to left and right motors?