CompassSensor.java

package lejos.nxt.addon;

import lejos.nxt.I2CPort;
import lejos.nxt.I2CSensor;
import lejos.robotics.*;
/*
 * WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
 * DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
 */

/**
 * Abstraction for a HiTechnic or Mindsensors compass.
 * 
 */
public class CompassSensor extends I2CSensor implements DirectionFinder {
	byte[] buf = new byte[2];
	private static final String MINDSENSORS_ID = "mndsnsrs";
	private boolean isMindsensors; // For comparing HiTechnic vs. Mindsensors
	private float cartesianCalibrate = 0; // Used by both cartesian methods. 
	
	// Mindsensors.com constants:
	private final static byte COMMAND = 0x41;
	private final static byte BEGIN_CALIBRATION = 0x43;
	private final static byte END_CALIBRATION = 0x44;
	
	// HiTechnic constants:
	private final static byte MEASUREMENT_MODE = 0x00;

	public CompassSensor(I2CPort port)
	{
		super(port);		
		isMindsensors = (this.getProductID().equals(MINDSENSORS_ID));
	}
	
	/**
	 * Returns the directional heading in degrees. (0 to 359.9)
	 * 0 is due North (on Mindsensors circuit board a white arrow indicates
	 * the direction of compass). Reading increases clockwise.
	 * @return Heading in degrees. Resolution is within 0.1 degrees
	 */
	public float getDegrees() {		
		int ret = getData(0x42, buf, 2);
		if(ret != 0) return -1;
		
		if(isMindsensors) { // Check if this is mindsensors
			// NOTE: The following only works when Mindsensors compass in integer mode
			/*int iHeading = (0xFF & buf[0]) | ((0xFF & buf[1]) << 8);
			float dHeading = iHeading / 10.00F;*/
			// Byte mode (default - will use Integer mode later)
			int dHeading = (0xFF & buf[0]);
			dHeading = dHeading * 360;
			dHeading = dHeading / 255;
			return dHeading;
		} else {
			return ((buf[0] & 0xff)<< 1) + buf[1];
		}
	}
	/**
	 * Compass readings increase clockwise from 0 to 360, but Cartesian
	 * coordinate systems increase counter-clockwise. This method returns
	 * the Cartesian compass reading. Also, the resetCartesianZero() method
	 * can be used to designate any direction as zero, rather than relying
	 * on North as being zero.
	 * @return Cartesian direction.
	 */
	public float getDegreesCartesian() {
		float degrees = cartesianCalibrate - getDegrees() ;
		if(degrees>=360) degrees -= 360;
		if(degrees<0) degrees += 360;
		return degrees;
	}
	
	/**
	 * Changes the current direction the compass is facing into the zero 
	 * angle. 
	 *
	 */
	public void resetCartesianZero() {
		cartesianCalibrate = getDegrees();
	}
	
	/**
	 * Starts calibration for Mindsensors.com compass. Must rotate *very* 
	 * slowly ,taking at least 20 seconds per rotation.
	 * Mindsensors: At least 2 full rotations.
	 * HiTechnic: 1.5 to 2 full rotations.
	 * Must call stopCalibration() when done.
	 */
	public void startCalibration() {
		buf[0] = BEGIN_CALIBRATION; 
		// Same value for HiTechnic and Mindsensors
		super.sendData(COMMAND, buf, 1);
	}
	
	/**
	 * Ends calibration sequence.
	 *
	 */
	public void stopCalibration() {
		if(isMindsensors)
			buf[0] = END_CALIBRATION;
		else
			buf[0] = MEASUREMENT_MODE;
		super.sendData(COMMAND, buf, 1);
	}
}