Please comment: MindSensors accelerometer

Post your NXJ projects, project ideas, etc here!

Moderators: 99jonathan, roger, imaqine

Please comment: MindSensors accelerometer

Postby Aswin » Sun Jun 05, 2011 12:40 pm

Hi,

I have, more or less, finished my first java project. I would like to have some feedback from the experts to prevent me developing bad habits.

The project is an implementation of a MindSensors accelerometer. It is compatible with the AccelMindSensors class from V0.90 but provide some extra's:
- It can return acceleration nd tilt values over three axes in one call.
- It uses user selectable units for acceleration (milli-G, G, m/s^2, mm/s^2) and tilt (radians, degrees, cosine).
- Sensitivity levels can be set.
- It has an interface to examine the sensor and its output and to change settings like sensitivity and units for acceleration and tilt.

I have used enums for the units, the enums are implemented as class members, but I have the feeling that these should be moved to a different level. So if you have any ideas on that please respond.

Aswin

This is the class, AccelMindSensor3D
Code: Select all
package nl.totan.test;

import lejos.nxt.Button;
import lejos.nxt.I2CPort;
import lejos.nxt.I2CSensor;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.Sound;
import lejos.robotics.Accelerometer;
import lejos.util.TextMenu;
import lejos.util.EndianTools;

/**
 * This class works with the Mindsensors acceleration sensor
 * @author Aswin
 */
public class AccelMindSensor3D extends I2CSensor implements Accelerometer, AccelerometerAdvanced {

  byte[] buf = new byte[12];
  private static byte X_TILT = 0x42;
  private static byte Y_TILT = 0x43;
  private static byte Z_TILT = 0x44;
  private static byte X_ACCEL = 0x45;
  private static byte Y_ACCEL = 0x47;
  private static byte Z_ACCEL = 0x49;
  private static byte CALSET = 0x4b;
  private static byte YCALSET = 0x4f;
  private static byte RESET = 0x52;
  private static byte GETSENS = 0x19;
  private static byte SETSENS = 0x41;
  private AccelUnits accelUnit = AccelUnits.MILLIG;
  private TiltUnits tiltUnit = TiltUnits.DEGREES;

  /**
   * List of possible units for acceleration
   */
  public enum AccelUnits {

    /**
     * Acceleration in milli-G
     */
    MILLIG(1.0),
    /**
     * Acceleration in G
     */
    G(0.001),
    /**
     * Acceleration in m/s^2
     */
    MS2(0.00981),
    /**
     * Acceleration in mm/s^2
     */
    MILLIMS2(9.81);
    private final double factor;

    AccelUnits(double factor) {
      this.factor = factor;
    }

    /**
     * Returns the multiplication factor to convert accel units from internal units (milli-g)
     * to output units
     * @return factor
     */
    public double factor() {
      return factor;
    }
  }

  /**
   * List of possible units for Tilt
   */
  public enum TiltUnits {

    /**
     * Tilt in degrees
     */
    DEGREES(57.2957795),
    /**
     * Tilt in radians
     */
    RADIANS(1.0),
    /**
     * Tilt in cosine
     */
    COSINE(0);
    private final double factor;

    TiltUnits(double factor) {
      this.factor = factor;
    }

    /**
     * Returns the multiplication factor to convert tilt units from internal units (cosine)
     * to output units
     * @return factor
     */
    public double factor() {
      return factor;
    }
  }

  /**
   * List of possible units for sensitivity (applies to firmware 3.20 or later)
   */
  public enum SensitivityUnits {

    /**
     * Range to 1.5 G
     */
    _1_5G((byte) 49),
    /**
     * Range to 2 G
     */
    _2G((byte) 50),
    /**
     * Range to 4 G
     */
    _4G((byte) 51),
    /**
     * Range to 6 G
     */
    _6G((byte) 52);
    private final byte code;

    SensitivityUnits(byte code) {
      this.code = code;
    }

    /**
     * Get to command code to set the sensitivity
     * @return byte code
     */
    public byte code() {
      return code;
    }
  }

  /**
   *
   * @param port
   */
  public AccelMindSensor3D(I2CPort port) {
    this(port, DEFAULT_I2C_ADDRESS);
  }

  /**
   *
   * @param port
   * @param address
   */
  public AccelMindSensor3D(I2CPort port, int address) {
    // TODO: Needs to be able to accept high-speed! Might be problem I was having.
    super(port, address, I2CPort.HIGH_SPEED, TYPE_LOWSPEED);
  }

  /**
   * Runs the top level interface to the sensor
   */
  public void runMenu() {
    int index = 0;
    String[] menuItems = {"View", "Settings", "Calibrate", "About"};
    TextMenu menu = new TextMenu(menuItems, 1, "Accelerometer menu");
    while (true) {
      LCD.clear();
      LCD.drawString("Escape to return", 1, 7);
      index = menu.select();
      switch (index) {
        case -1:
          return;
        case 0:
          displayOutput();
          break;
        case 1:
          runSettings();
          break;
        case 2:
          calibrate();
          break;
        case 3:
          displayAbout();
        default:
          break;
      }
    }
  }

  /**
   * Runs the settings interface to the sensor
   */
  private void runSettings() {
    int index = 0;
    String[] menuItems = {"Sens  :" + getSensitivity(), "Accel :" + getAccelUnit(), "Tilt  :" + getTiltUnit()};
    TextMenu menu = new TextMenu(menuItems, 1, "Settings menu");
    while (true) {
      LCD.clear();
      LCD.drawString("Escape to return", 1, 7);
      index = menu.select();
      switch (index) {
        case -1:
          return;
        case 0:
          selectSensitivity();
          menuItems[0] = "Sens  :" + getSensitivity();
          break;
        case 1:
          selectAccelUnit();
          menuItems[1] = "Accel :" + getAccelUnit();
          break;
        case 2:
          selectTiltUnit();
          menuItems[2] = "Tilt  :" + getTiltUnit();
          break;
        default:
          break;
      }
      menu.setItems(menuItems);
    }
  }

  /**
   * Runs the sensitivity interface to the sensor
   */
  public void selectSensitivity() {
    int index = 0;
    String[] menuItems;
    menuItems = new String[4];
    int i = 0;
    for (SensitivityUnits p : SensitivityUnits.values()) {
      menuItems[i++] = p.name();
    }

    TextMenu menu = new TextMenu(menuItems, 1, "Select Sensitivity");
    LCD.clear();
    LCD.drawString("Escape to return", 1, 7);
    SensitivityUnits s;
    s = getSensitivity();
    index = menu.select(s.ordinal());

    if (index != -1) {
      SensitivityUnits[] p;
      p = SensitivityUnits.values();
      setSensitivity(p[index]);
    }

  }

  /**
   * Runs the acceleration units interface to the sensor
   */
  public void selectAccelUnit() {
    int index = 0;
    String[] menuItems;
    menuItems = new String[4];
    int i = 0;
    for (AccelUnits p : AccelUnits.values()) {
      menuItems[i++] = p.name();
    }

    TextMenu menu = new TextMenu(menuItems, 1, "Select accel unit");
    LCD.clear();
    LCD.drawString("Escape to return", 1, 7);
    AccelUnits s;
    s = getAccelUnit();
    index = menu.select(s.ordinal());

    if (index != -1) {
      AccelUnits[] p;
      p = AccelUnits.values();
      setAccelUnit(p[index]);
    }
  }

  /**
   * Runs the tilt units interface to the sensor
   */
  public void selectTiltUnit() {
    int index = 0;
    String[] menuItems;
    menuItems = new String[3];
    int i = 0;
    for (TiltUnits p : TiltUnits.values()) {
      menuItems[i++] = p.name();
    }

    TextMenu menu = new TextMenu(menuItems, 1, "Select tilt unit");
    LCD.clear();
    LCD.drawString("Escape to return", 1, 7);
    TiltUnits s;
    s = getTiltUnit();
    index = menu.select(s.ordinal());

    if (index != -1) {
      TiltUnits[] p;
      p = TiltUnits.values();
      setTiltUnit(p[index]);
    }
  }

  /**
   * Runs the about interface to the sensor
   */
  private void displayAbout() {
    LCD.clear();
    LCD.drawString(this.getProductID(), 1, 1);
    LCD.drawString(this.getSensorType(), 1, 2);
    LCD.drawString(this.getVersion(), 1, 3);
    LCD.drawString("Escape to return", 1, 7);
    Button.ESCAPE.waitForPressAndRelease();
  }

  /**
   * Displays output from the sensor, acceleration and tilt, in the selected units
   */
  private void displayOutput() {
    double[] accel = null;
    double[] tilt = null;
    while (!Button.ESCAPE.isPressed()) {
      accel = this.getXYZAccel();
      tilt = this.getXYZTilt();
      LCD.clear();
      LCD.drawString("Accel   Tilt", 4, 0);
      LCD.drawString("" + getAccelUnit(), 4, 1);
      LCD.drawString("" + getTiltUnit(), 12, 1);
      LCD.drawString("X", 1, 2);
      LCD.drawString("Y", 1, 3);
      LCD.drawString("Z", 1, 4);
      LCD.drawString("Escape to return", 1, 7);
      for (int i = 0; i < 3; i++) {
        LCD.drawString(String.valueOf((accel[i])), 4, i + 2);
        LCD.drawString(String.valueOf((tilt[i])), 12, i + 2);
      }
      try {
        Thread.sleep(50);
      } catch (InterruptedException ex) {
      }

    }
    while (Button.ESCAPE.isPressed());

  }

  /**
   * Sets the unit for acceleration
   * @param accelUnit
   */
  public void setAccelUnit(AccelMindSensor3D.AccelUnits accelUnit) {
    this.accelUnit = accelUnit;
  }

  /**
   * Returns the current acceleration unit
   * @return accelUnit
   */
  public AccelMindSensor3D.AccelUnits getAccelUnit() {
    return accelUnit;
  }

  /**
   * Sets the unit for tilt
   * @param tiltUnit
   */
  public void setTiltUnit(AccelMindSensor3D.TiltUnits tiltUnit) {
    this.tiltUnit = tiltUnit;
  }

  /**
   * Returns the current unit for tilt
   * @return
   */
  public AccelMindSensor3D.TiltUnits getTiltUnit() {
    return tiltUnit;
  }

  /**
   * Sets the sensitivity unit
   * @param sensitivity
   */
  public void setSensitivity(AccelMindSensor3D.SensitivityUnits sensitivity) {
    sendData(SETSENS, sensitivity.code());
    try {
      Thread.sleep(150);
    } catch (InterruptedException ex) {
    }
  }

  /**
   * Returns the current sensitivity of the sensor
   * @return sensitivity
   */
  public AccelMindSensor3D.SensitivityUnits getSensitivity() {
    getData(GETSENS, buf, 1);
    for (SensitivityUnits p : SensitivityUnits.values()) {
      if (p.code() == buf[0]) {
        return p;
      }
    }
    return SensitivityUnits._1_5G;
  }

  /**
   * Resets the sensor to factory calibrated values
   */
  public void resetToFactoryCalibration() {
    sendData(RESET, buf, 0);
    // swapYCalibrationSettings();
    try {
      Thread.sleep(100);
    } catch (InterruptedException ex) {
    }
  }

 
  /**
   * Runs the calibration interface of the sensor
   */
  public void calibrate() {
    int index = 0;
    byte acquire = 0;
    byte end = 0;
    String[] menuItems = {"X-axis", "Y-axis", "Z-axis", "Show settings", "Factory default"};
    TextMenu menu = new TextMenu(menuItems, 1, "Calibrate");
    while (true) {
      index = menu.select();
      switch (index) {
        case -1:
          return;
        case 0:
          calibrateAxis('X');
          break;
        case 1:
          calibrateAxis('Y');
          break;
        case 2:
          calibrateAxis('Z');
          break;
        case 3:
          showCalibrationSettings();
          break;
        case 4:
          this.resetToFactoryCalibration();
        default:
          break;
      }
    }
  }

  public int[] getCalibrationSettings() {
    getData(CALSET, buf, 12);
    int[] ret = new int[6];
    ret[0]=  EndianTools.decodeShortLE(buf,0);
    ret[1]=  EndianTools.decodeShortLE(buf,4);
    ret[2]=  EndianTools.decodeShortLE(buf,8);
    ret[3]=  EndianTools.decodeShortLE(buf,2);
    ret[4]=  EndianTools.decodeShortLE(buf,6);
    ret[6]=  EndianTools.decodeShortLE(buf,10);
    return ret;
  }

  public void showCalibrationSettings() {
    int[] settings = null;
    settings = getCalibrationSettings();
    LCD.clear();
    LCD.drawString("offset  range", 4, 0);
    LCD.drawString("X", 1, 2);
    LCD.drawString("Y", 1, 3);
    LCD.drawString("Z", 1, 4);
    LCD.drawString("Escape to return", 1, 7);
    for (int i = 0; i < 3; i++) {
      LCD.drawInt(settings[i], 4, i + 2);
      LCD.drawInt(settings[i + 3], 12, i + 2);
    }
    Button.ESCAPE.waitForPressAndRelease();

  }

  /**
   * Calibrates one single axis for the current sensitivity
   * To run calibration the sensor must be connected without any gearing to a motor on port A
   * The selected axis must be perpendicular to earths gravity
   * @param axis
   */
  private void calibrateAxis(char axis) {
    byte acquire = 0;
    byte end = 0;
    switch (axis) {
      case 'X':
        acquire = 0x58;
        end = 0x78;
        break;
      case 'Y':
        acquire = 0x59;
        end = 0x79;
        break;
      case 'Z':
        acquire = 0x5a;
        end = 0x7a;
        break;
      default:
        break;
    }
    while (Button.ENTER.isPressed());
    LCD.clear();
    LCD.drawString("Calibraing..", 1, 1);
    Motor.A.resetTachoCount();
    Motor.A.setSpeed(6);
    Motor.A.forward();
    int count = 0;
    int last = 1;
    int i = 0;
    while (count < 360) {
      count = Motor.A.getTachoCount();
      if ((count % 5) == 0 && count > last) {
        last = count;
        if (count < 360) {
          sendData(acquire, buf, 0);
          LCD.drawInt(++i, 1, 2);
        } else {
          sendData(end, buf, 0);
          Sound.beep();
        }
      }
    }
    Motor.A.stop();
    Motor.A.setSpeed(60);
    Motor.A.backward();
    while (count > 0) {
      count = Motor.A.getTachoCount();
    }
    Motor.A.stop();
    LCD.clear();
    LCD.drawString("Calibration done", 1, 1);
    LCD.drawString("Press enter to", 1, 2);
    LCD.drawString("shut down", 2, 3);
    while (!Button.ENTER.isPressed());
    // NXT.shutDown();
  }


  /**
   * Acceleration along 3 axis.
   * @return Acceleration in selected units
   */
  public double[] getXYZAccel() {
    return getXYZAccel(accelUnit);
  }

  /**
   * Acceleration along 3 axis.
   * @param unit of acceleration
   * @return Acceleration in provided unit
   */
  public double[] getXYZAccel(AccelMindSensor3D.AccelUnits unit) {
    getData(X_ACCEL, buf, 6);
    double[] ret = new double[3];
    ret[0] = unit.factor() * EndianTools.decodeShortLE(buf,0);
    ret[1] = unit.factor() * EndianTools.decodeShortLE(buf,2);
    ret[2] = unit.factor() * EndianTools.decodeShortLE(buf,4);
    return ret;
  }

  /**
   * Tilt along 3 axis
   * @return Tilt in selected unit
   */
  public double[] getXYZTilt() {
    return getXYZTilt(tiltUnit);
  }

  /**
   * Tilt along 3 axis
   * @param unit of tilt
   * @return Tilt in provided unit
   */
  public double[] getXYZTilt(AccelMindSensor3D.TiltUnits unit) {
    double[] accel;
    double[] ret = new double[3];

    accel = this.getXYZAccel(AccelUnits.G);

    if (unit == TiltUnits.COSINE) {
      return accel;
    }
    for (int i = 0; i < 3; i++) {
      ret[i] = unit.factor() * Math.acos(accel[i]);
    }
    return ret;
  }

  /**
   * Acceleration in milli-g of x-axis
   * provided for backwards compatibility. use getXYZaccel() instead
   * @return
   */
  public int getXAccel() {
    getData(X_ACCEL, buf, 2);
    return EndianTools.decodeShortLE(buf,0);
  }

  /**
   * Acceleration in milli-g of z-axis
   * provided for backwards compatibility. use getXYZaccel() instead
   * @return
   */
  public int getZAccel() {
    getData(Z_ACCEL, buf, 2);
    return EndianTools.decodeShortLE(buf,0);
  }

  /**
   * Acceleration in milli-g of y-axis
   * provided for backwards compatibility. use getXYZaccel() instead
   * @return
   */
  public int getYAccel() {
    getData(Y_ACCEL, buf, 2);
    return EndianTools.decodeShortLE(buf,0);
  }

  /**
   * Tilt in internal units (0 to 255) of x-axis
   * provided for backwards compatibility. use getXYZTilt() instead
   * @return
   */
  public int getXTilt() {
    getData(X_TILT, buf, 1);
    return (0xFF & buf[0]);
  }

  /**
   * Tilt in internal units (0 to 255) of y-axis
   * provided for backwards compatibility. use getXYZTilt() instead
   * @return
   */
  public int getYTilt() {
    getData(Y_TILT, buf, 1);
    return (0xFF & buf[0]);
  }

  /**
   * Tilt in internal units 0 to 255 of z-axis
   * provided for backwards compatibility. use getXYZTilt() instead
   * @return
   */
  public int getZTilt() {
    getData(Z_TILT, buf, 1);
    return (0xFF & buf[0]);
  }
}




This is the interface, AccelerometerAdvanced
Code: Select all
/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package nl.totan.test;

import lejos.util.Matrix;

/**
 *
 * @author Aswin
 */
public interface AccelerometerAdvanced {
  void resetToFactoryCalibration();
  void calibrate();
  AccelMindSensor3D.SensitivityUnits getSensitivity();
  void setSensitivity(AccelMindSensor3D.SensitivityUnits sensitivity);
  double[] getXYZAccel();
  double[] getXYZAccel(AccelMindSensor3D.AccelUnits unit);
  double[] getXYZTilt();
  double[] getXYZTilt(AccelMindSensor3D.TiltUnits unit);
 
 
 
}

My NXT blog: http://nxttime.wordpress.com/
Aswin
leJOS Team Member
 
Posts: 197
Joined: Tue Apr 26, 2011 9:18 pm
Location: Netherlands

Return to NXJ Projects

Who is online

Users browsing this forum: No registered users and 0 guests

more stuff