//
you're reading...
Projects

UART Compass Sensor

uartcompass

This is an example of a homemade EV3 UART sensor using an HMC5883L compass module, which can be bought cheaply from ebay or amazon. I am using an Arduino Uno, but a smaller device like an Arduino Nano or a Sparkfun Arduino Pro Micro would be better.

The code below implements two modes: the raw mode and the calculated heading. The EV3 UART protocol only supports sending number of data items that are a power of 2, so 4 items rather than 3 are sent for the raw mode. The EV3UARTEmulation library could do the necessary padding of the data, but currently does not.

The heading is not adjusted for tilt of the sensor. See the HMC5883L library and example program for more details on this.

Here is the Arduino code:

#include <SoftwareSerial.h>
#include <EV3UARTEmulation.h>
#include <Serial.h>

EV3UARTEmulation sensor(10, 11, 99, 38400);

// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>

// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

void setup() {
 Serial.begin(9600);
 sensor.create_mode("RAW", true, DATA16, 3, 5, 0);
 sensor.create_mode("DEGREES", true, DATA16, 1, 3, 0);
 sensor.reset();

 Serial.println("Starting the I2C interface.");
 Wire.begin(); // Start the I2C interface.

 Serial.println("Constructing new HMC5883L");
 compass = HMC5883L(); // Construct a new HMC5883 compass.

 Serial.println("Setting measurement mode to continous.");
 error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
 if(error != 0) // If there is an error, print it out.
 Serial.println(compass.GetErrorText(error));

 Serial.println("Setting scale to +/- 1.3 Ga");
 error = compass.SetScale(1.3); // Set the scale of the compass.
 if(error != 0) // If there is an error, print it out.
 Serial.println(compass.GetErrorText(error));
}

short heading[4];
unsigned long last_reading = 0;

void loop() {
 sensor.heart_beat();
 if (millis() - last_reading > 100) {
 MagnetometerRaw raw;
 MagnetometerScaled scaled;
 switch (sensor.get_current_mode()) {
 case 0:
   // Retrieve the raw values from the compass (not scaled).
   raw = compass.ReadRawAxis();
   heading[0] = raw.XAxis;
   heading[1] = raw.YAxis;
   heading[2] = raw.ZAxis;
   Serial.print("Raw X: ");
   Serial.print(raw.XAxis);
   Serial.print(" Y: ");
   Serial.print(raw.YAxis);
   Serial.print(" Z: ");
   Serial.println(raw.ZAxis);
   sensor.send_data16(heading, 4);
   break;
 case 1:
   // Retrieve the scaled values from the compass (scaled to the configured scale).
   scaled = compass.ReadScaledAxis();

   // Calculate heading when the magnetometer is level, then correct for signs of axis.
   short degrees = atan2(scaled.YAxis, scaled.XAxis) * 180/M_PI;

   if (degrees < 0) degrees += 360;
   Serial.print("Heading: ");
   Serial.println(degrees);
   sensor.send_data16(degrees);
   break;
 }
 last_reading = millis();
 }
}

The corresponding Java code for the Compass sensor is:

import lejos.hardware.port.Port;
import lejos.hardware.sensor.SensorMode;
import lejos.hardware.sensor.UARTSensor;
import lejos.robotics.SampleProvider;

public class CompassSensor extends UARTSensor {
 private static final long SWITCHDELAY = 200;

 public CompassSensor(Port port) {
   super(port);
   setModes(new SensorMode[] { new RawMode(), new HeadingMode()});
 }

 private class RawMode implements SensorMode {
   private static final int MODE = 0;
   private short[] raw = new short[sampleSize()];

   @Override
   public int sampleSize() {
     return 3;
   }

   @Override
   public void fetchSample(float[] sample, int offset) {
     switchMode(MODE, SWITCHDELAY);
     port.getShorts(raw,0,sampleSize());
     for(int i=0;i<sampleSize();i++) sample[offset+i] = raw[i];
   }

   @Override
   public String getName() {
     return "Raw";
   }
 }

 public SampleProvider getRawMode() {
   return getMode(0);
 }

 private class HeadingMode implements SensorMode {
   private static final int MODE = 1;
   private int heading;

   @Override
   public int sampleSize() {
     return 1;
   }

   @Override
   public void fetchSample(float[] sample, int offset) {
     switchMode(MODE, SWITCHDELAY);
     heading = port.getShort();
     sample[offset] = heading;
   }

   @Override
   public String getName() {
     return "Heading";
   }
 }

 public SampleProvider getHeadingMode() {
   return getMode(1);
 }
}

And here is a test program:

import lejos.hardware.Button;
import lejos.hardware.lcd.LCD;
import lejos.hardware.port.SensorPort;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

public class CompassTest {
	public static void main(String[] args) {
		CompassSensor sensor = new CompassSensor(SensorPort.S1);
		SampleProvider raw = sensor.getRawMode();
		SampleProvider heading = sensor.getHeadingMode();
		float[] sample = new float[raw.sampleSize()];
		float[] degrees = new float[1];
		LCD.drawString("Raw:", 3, 0);
		LCD.drawString("X:",0, 3);
		LCD.drawString("Y:",0, 4);
		LCD.drawString("Z:",0, 5);
		LCD.drawString("Heading:", 0, 7);

		while(Button.ESCAPE.isUp()) {
			raw.fetchSample(sample,0);
			for (int i=0;i<3;i++) LCD.drawInt((int)sample[i],5,4,3+i);
			heading.fetchSample(degrees, 0);
			LCD.drawInt((int) degrees[0], 3, 10, 7);
			Delay.msDelay(1000);
		}
		sensor.close();
	}
}
Advertisements

Discussion

No comments yet.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

About leJOS News

leJOS News keeps you up-to-date with leJOS. It features latest news, explains cool features, shows advanced techniques and highlights amazing projects. Be sure to subscribe to leJOS News and never miss an article again. Best of all, subscription is free!
Follow leJOS News on WordPress.com
%d bloggers like this: