nxtOSEK/lejos_nxj/src/java/classes/lejos/nxt/UltrasonicSensor.java

   1 package lejos.nxt;
   2
   3 /**
   4  *
   5  * Abstraction for a NXT Ultrasonic Sensor.
   6  *
   7  */
   8 public class UltrasonicSensor extends I2CSensor
   9 {
  10         /* Device control locations */
  11         private static final byte MODE = 0x41;
  12         private static final byte DISTANCE = 0x42;
  13         private static final byte FACTORY_DATA = 0x11;
  14         private static final byte UNITS = 0x14;
  15         private static final byte CALIBRATION = 0x4a;
  16         private static final byte PING_INTERVAL = 0x40;
  17         /* Device modes */
  18         private static final byte MODE_OFF = 0x0;
  19         private static final byte MODE_SINGLE = 0x1;
  20         private static final byte MODE_CONTINUOUS = 0x2;
  21         private static final byte MODE_CAPTURE = 0x3;
  22         private static final byte MODE_RESET = 0x4;
  23         /* Device timing */
  24         private static final int DELAY_CMD = 0x5;
  25         private static final int DELAY_AVAILABLE = 0xf;
  26
  27         private byte[] buf = new byte[1];
  28         private byte[] inBuf = new byte[8];
  29         private String units = "       ";
  30         private char [] unitsChars = StringUtils.getCharacters(units);
  31         private int nextCmdTime;
  32         private int dataAvailableTime;
  33         private int currentDistance;
  34         private byte mode;
  35
  36         /*
  37          * Return the current time in milliseconds
  38          */
  39         private int now()
  40         {
  41                 return (int)System.currentTimeMillis();
  42         }
  43
  44         /*
  45          * Wait until the specified time
  46          *
  47          */
  48         private void wait(int when)
  49         {
  50                 int delay = when - now();
  51                 if (delay > 0)
  52                         try
  53                         {
  54                                 Thread.sleep(delay);
  55                         }
  56                         catch (Exception e)
  57                         {}
  58         }
  59
  60         /*
  61          * Over-ride standard get function to ensure correct inter-command timing
  62          * when using the ultrasonic sensor. The Lego Ultrasonic sensor uses a
  63          * "bit-banged" i2c interface and seems to require a minimum delay between
  64          * commands otherwise the commands fail.
  65          */
  66         public int getData(int register, byte [] buf, int len)
  67         {
  68                 wait(nextCmdTime);
  69                 int ret = super.getData(register, buf, len);
  70                 nextCmdTime = now() + DELAY_CMD;
  71                 return ret;
  72         }
  73
  74         /*
  75          * Over-ride the standard send function to ensure the correct inter-command
  76          * timing for the ultrasonic sensor.
  77          *
  78          */
  79         public int sendData(int register, byte [] buf, int len)
  80         {
  81                 wait(nextCmdTime);
  82                 int ret = super.sendData(register, buf, len);
  83                 nextCmdTime = now() + DELAY_CMD;
  84                 return ret;
  85         }
  86
  87         public UltrasonicSensor(SensorPort port)
  88         {
  89                 super(port);
  90                 // Set correct sensor type, default is TYPE_LOWSPEED
  91                 port.setType(TYPE_LOWSPEED_9V);
  92                 // Default mode is continuous
  93                 mode = MODE_CONTINUOUS;
  94                 // Set initial inter-command delays
  95                 nextCmdTime = now() + DELAY_CMD;
  96                 dataAvailableTime = now() + DELAY_AVAILABLE;
  97                 currentDistance = 255;
  98         }
  99
 100         /**
 101          * Return distance to an object. To ensure that the data returned is valid
 102          * this method may have to wait a short while for the distance data to
 103          * become available.
 104          *
 105          * @return distance or 255 if no object in range
 106          */
 107         public int getDistance()
 108         {
 109                 // If we are in continuous mode and new data will not yet be available
 110                 // simply return the current reading (since this is what the sensor
 111                 // will do anyway.)
 112                 if (mode == MODE_CONTINUOUS && now() < dataAvailableTime)
 113                         return currentDistance;
 114                 wait(dataAvailableTime);
 115                 int ret = getData(DISTANCE, buf, 1);
 116                 currentDistance = (ret == 0 ? (buf[0] & 0xff) : 255);
 117                 // Make a note of when new data should be available.
 118                 if (mode == MODE_CONTINUOUS)
 119                         dataAvailableTime = now() + DELAY_AVAILABLE;
 120                 return currentDistance;
 121         }
 122
 123         /**
 124          * Return an array of 8 echo distances. These are generated when using ping
 125          * mode. A value of 255 indicates that no echo was obtained. The array must
 126          * contain at least 8 elements, if not -1 is returned. If the distnace data
 127          * is not yet available the method will wait until it is.
 128          *
 129          * @return 0 if ok <> 0 otherwise
 130          */
 131         public int getDistances(int dist[])
 132         {
 133                 if (dist.length < inBuf.length || mode != MODE_SINGLE) return -1;
 134                 wait(dataAvailableTime);
 135                 int ret = getData(DISTANCE, inBuf, inBuf.length);
 136                 for(int i = 0; i < inBuf.length; i++)
 137                         dist[i] = (int)inBuf[i] & 0xff;
 138                 return ret;
 139         }
 140
 141         /*
 142          * Set the sensor into the specified mode. Keep track of which mode we are
 143          * operating in. Make a note of when any distance data will become available
 144          *
 145          */
 146         private int setMode(byte mode)
 147         {
 148                 buf[0] = mode;
 149                 int ret = sendData(MODE, buf, 1);
 150                 // Make a note of when the data will be available
 151                 dataAvailableTime = now() + DELAY_AVAILABLE;
 152                 if (ret == 0) this.mode = mode;
 153                 return ret;
 154         }
 155
 156         /**
 157          * Send a single ping.
 158          * The sensor operates in two modes, continuous and ping. When in continuous
 159          * mode the sensor sends out pings as often as it can and the most recently
 160          * obtained result is available via a call to getDistance. When in ping mode
 161          * a ping is only transmitted when a call is made to ping. This sends a
 162          * single ping and up to 8 echoes are captured. These may be read by making
 163          * a call to getDistance and passing a suitable array. A delay of
 164          * approximately 20ms is required between the call to ping and getDistance.
 165          * This delay is not included in the method. Calls to getDistance before
 166          * this period may result in an error or no data being returned. The normal
 167          * getDistance call may also be used with ping, returning information for
 168          * the first echo. Calling this method will disable teh default continuous
 169          * mode, to switch back to continuous mode call continuous.
 170          *
 171          * @return 0 if ok <> 0 otherwise
 172          *
 173          */
 174         public int ping()
 175         {
 176                 return setMode(MODE_SINGLE);
 177         }
 178
 179         /**
 180          * Switch to continuous ping mode.
 181          * This method enables continuous ping and capture mode. This is the default
 182          * operating mode of the sensor. Please the notes for ping for more details.
 183          *
 184          * @return 0 if ok <> 0 otherwise
 185          *
 186          */
 187         public int continuous()
 188         {
 189                 return setMode(MODE_CONTINUOUS);
 190         }
 191         /**
 192          * Turn off the sensor.
 193          * This call disables the sensor. No pings will be issued after this call,
 194          * until either ping, continuous or reset is called.
 195          *
 196          * @return 0 if ok <> 0 otherwise
 197          *
 198          */
 199         public int off()
 200         {
 201                 return setMode(MODE_OFF);
 202         }
 203
 204         /**
 205          * Set capture mode
 206          * Set the sensor into capture mode. The Lego documentation states:
 207          * "Within this mode the sensor will measure whether any other ultrasonic
 208          * sensors are within the vicinity. With this information a program can
 209          * evaluate when it is best to make a new measurement which will not
 210          * conflict with other ultrasonic sensors."
 211          * I have no way of testing this. Perhaps someone with a second NXT could
 212          * check it out!
 213          *
 214          * @return 0 if ok <> 0 otherwise
 215          *
 216          */
 217         public int capture()
 218         {
 219                 return setMode(MODE_CAPTURE);
 220         }
 221
 222         /**
 223          * Reset the device
 224          * Performs a "soft reset" of the device. Restores things to the default
 225          * state. Following this call the sensor will be operating in continuous
 226          * mode.
 227          *
 228          * @return 0 if ok <> 0 otherwise
 229          *
 230          */
 231         public int reset()
 232         {
 233                 int ret = setMode(MODE_RESET);
 234                 // In continuous mode after a reset;
 235                 if (ret == 0) mode = MODE_CONTINUOUS;
 236                 return ret;
 237         }
 238
 239         private int getMultiBytes(int reg, byte data[], int len)
 240         {
 241                 /*
 242                  * For some locations that are adjacent in address it is not possible
 243                  * to read the locations in a single read, instead we must read them
 244                  * using a series of individual reads. No idea why this should be, but
 245                  * that is how it is!
 246                  */
 247                 int ret;
 248                 for(int i = 0; i < len; i++)
 249                 {
 250                         ret = getData(reg+i, buf, 1);
 251                         if (ret != 0) return ret;
 252                         data[i] = buf[0];
 253                 }
 254                 return 0;
 255         }
 256
 257         private int setMultiBytes(int reg, byte data[], int len)
 258         {
 259                 /*
 260                  * For some locations that are adjacent in address it is not possible
 261                  * to read the locations in a single write, instead we must write them
 262                  * using a series of individual writes. No idea why this should be, but
 263                  * that is how it is!
 264                  */
 265                 int ret;
 266                 for(int i = 0; i < len; i++)
 267                 {
 268                         buf[0] = data[i];
 269                         ret = sendData(reg+i, buf, 1);
 270                         if (ret != 0) return ret;
 271                 }
 272                 return 0;
 273         }
 274
 275         /**
 276          * Return 10 bytes of factory calibration data. The bytes are as follows
 277          * data[0] : Factory zero (cal1)
 278          * data[1] : Factory scale factor (cal2)
 279          * data[2] : Factory scale divisor.
 280          *
 281          * @return 0 if ok <> 0 otherwise
 282          */
 283         public int getFactoryData(byte data[])
 284         {
 285                 if (data.length < 3) return -1;
 286                 return getMultiBytes(FACTORY_DATA, data, 3);
 287         }
 288         /**
 289          * Return a string indicating the type of units in use by the unit.
 290          * The default response is 10E-2m indicating centimetres in use.
 291          *
 292          * @return 7 byte string
 293          */
 294         public String getUnits()
 295         {
 296                 int ret = getData(UNITS, inBuf, 7);
 297                 for(int i = 0; i < 7; i++)
 298                         unitsChars[i] = (ret == 0 ? (char)inBuf[i] : ' ');
 299                 return units;
 300         }
 301
 302         /**
 303          * Return 3 bytes of calibration data. The bytes are as follows
 304          * data[0] : zero (cal1)
 305          * data[1] : scale factor (cal2)
 306          * data[2] : scale divisor.
 307          *
 308          * @return 0 if ok <> 0 otherwise
 309          */
 310         public int getCalibrationData(byte data[])
 311         {
 312                 /* Note the lego documentation says this is at loacation 0x50, however
 313                  * it looks to me like this is a hex v decimal thing and it should be
 314                  * location 0x49 + 1 which is 0x4a not 0x50! There certainly seems to be
 315                  * valid data at 0x4a...
 316                  */
 317                 if (data.length < 3) return -1;
 318                 return getMultiBytes(CALIBRATION, data, 3);
 319         }
 320
 321         /**
 322          * Set 3 bytes of calibration data. The bytes are as follows
 323          * data[0] : zero (cal1)
 324          * data[1] : scale factor (cal2)
 325          * data[2] : scale divisor.
 326          *
 327          * This does not currently seem to work.
 328          *
 329          * @return 0 if ok <> 0 otherwise
 330          */
 331         public int setCalibrationData(byte data[])
 332         {
 333                 if (data.length < 3) return -1;
 334                 return setMultiBytes(CALIBRATION, data, 3);
 335         }
 336
 337         /**
 338          * Return the interval used in continuous mode.
 339          * This seems to be in the range 1-15. It can be read and set. However tests
 340          * seem to show it has no effect. Others have reported that this does vary
 341          * the ping interval (when used in other implementations). Please report
 342          * any new results.
 343          *
 344          * @return -1 if error otherwise the interval
 345          */
 346         public byte getContinuousInterval()
 347         {
 348                 int ret = getData(PING_INTERVAL, buf,1);
 349                 return (ret == 0 ? buf[0] : -1);
 350         }
 351
 352         /**
 353          * Set the ping inetrval used when in continuous mode.
 354          * See getContinuousInterval for more details.
 355          *
 356          * @return 0 if 0k <> 0 otherwise.
 357          */
 358         public int setContinuousInterval(byte interval)
 359         {
 360                 buf[0] = interval;
 361                 int ret = sendData(PING_INTERVAL, buf, 1);
 362                 return ret;
 363         }
 364
 365         /**
 366          * Returns the current operating mode of the sensor.
 367          * 0 : sensor is off
 368          * 1 : Single shot ping mode
 369          * 2 : continuous ping mode (default)
 370          * 3 : Event capture mode
 371          *
 372          * @return -1 if error otherwise the operating mode
 373          */
 374         public byte getMode()
 375         {
 376                 int ret = getData(MODE, buf,1);
 377                 return (ret == 0 ? buf[0] : -1);
 378         }
 379 }