Uart Sensor using Sparkfun Pro Micro

2014-08-24 12.46.00

Here is an example of an EV3 UART sensor based a Sparkfun Pro Micro Arduino clone and a Dexter Industries breadboard adapter. It uses the EV3UARTEmulationHard library. This could be used with a variety of cheap sensors. It is shown with an MQ-2 gas sensor. There is also a Bluetooth serial adapter. This could be used to access the sensor wirelessly.

The EV3UARTEmulationHard library is the same as the EV3UARTEmulation library, except that it uses hardware serial rather than software serial. It is suitable for Arduino devices like the Leonardo or the Sparkfun Pro Micro or the Mega, that have a Serial1 hardware UART. For those devices, the USB Serial device can be used for diagnostics and Serial1 can be used for the EV3 communication. Hardware serial should be able to support high bit rates than software serial.

The Arduino code for this device, using EV3UARTEmulationHard is:

#include <EV3UARTEmulationHard.h>
#include <Serial.h>

EV3UARTEmulation sensor(&Serial1, 99, 38400);

void setup() {
  Serial.begin(9600);
  while(!Serial1);
  sensor.create_mode("TEST", true, DATA16, 1, 3, 0);
  sensor.reset();
}

unsigned long last_reading = 0;

void loop() {
  sensor.heart_beat();
  if (millis() - last_reading > 100) {
    int r = analogRead(0);
    sensor.send_data16(r);
    Serial.print("Reading: ");
    Serial.println(r);
    last_reading = millis();
  }
}

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();
	}
}

Getting heading from a gyro

The EV3 comes with a gyro sensor. This sensor measures rate of turn or angle over the vertical axis. Rate of turn is the speed at which the sensor rotates. This is also known as angular speed and is expressed (in leJOS) in degrees per second. Angle is the change in position of the sensor since the sensor was first used. Or since the last time it was reset. Angle is very much like a compass. The difference is that a compass measures the angle from an absolute heading, north. The gyro in angle mode measures from a relative heading, it’s starting position. Angle is not measured directly by the sensor. The sensor can only measure rate of turn directly. Angle is calculated from rate of turn by integration. This calculation is very sensitive to small errors in rate of turn. Lego therefore advices to reset the sensor before every turn. During a reset the angle is reset to zero, but more important, the offset error of the sensor is recalculated. The offset error is the difference between the actual rate of turn and the rate of turn the sensor measures. If this error is known one can correct for it. To determine this error the sensor must be motionless. The offset error is not constant, it therefor must be recalculated regularly.

There are several third party gyro sensors available for the NXT and EV3. Some are able to measure rate of turn over three axes but none is able to provide angle. This is a clear advantage of the Lego sensor. With leJOS however it is very easy to calculate angle from rate of turn. All you need to do is to apply two standard filters, the OffsetCorrectionFilter and the IntegrationFilter. The OffsetCorrectionFilter is applied first. The IntegrationFilter comes second. The OffsetCorrectionFilter uses advanced statistics to continuously update the offset error. There is no need to instruct it to recalculate it. You only have to make sure that the sensor is motionless during the first few reads ( >15 ) from the sensor. Also the IntegrationFilter needs little care. It performs best when the sensor is queried often. But it is not necessary to read it a precise regular intervals. You can reset the accumulated angle every so often. If you happen to have another reference like a compass you can use the angle it provides in the reset. This gives your gyro generated angle an absolute reference.

test setupIt is interesting to know how a software solution performs compared to the Lego gyro sensor. I ran some simple tests to compare three different setups. A Dexter Industries dIMU sensor using the OffsetCorrection and Integraton filters, a EV3 gyro sensor in angle mode and the same EV3 gyro sensor in rate mode using the two filters. I tested the drift over a period of 5 minutes while the sensors were motionless. I tested the outcome of a ninety degree turn and back at high (90 degrees/s) and at slow (22 degrees/s) speed. The three setups were tested simultaneously, thus canceling out the effect of variation in test conditions. I turned the test device by hand.
Below are the test results. The figures in blue is the error from the expected value  (root mean squared error). The bigger this number the bigger the error.

 

 

 

Test dIMU EV3 angle EV3 rate
5 min Drift 2 0 20
. 0 0 36
. 1,0 0,0 20,6
90 degree turn 87 91 88
. 89 91 89
. 88 91 89
. 87 90 88
. 2,4 0,9 1,6
Back 0 1 2
. 0 2 4
. 0 1 2
. -1 0 1
. 0,5 1,2 2,5
slow 90 degree turn 89 87 79
. 87 92 87
. 87 92 86
. 89 90 85
. 2,2 2,1 6,5
slow Back -1 -4 -6
. -3 5 7
. -1 4 4
. 0 1 3
. 1,7 3,8 5,2

The results show that the software solution does not work very well on the EV3 gyro sensor. This setup gave the biggest errors. I think this is because the rate that this sensor returns to the brick is rounded (or truncated) to degrees. The resolution of the dIMU sensor is much better 1/70 of a degree. It gives much better results. The EV3 gyro sensor in angle mode gives best absolute results, although not much better then the software solution. It is interesesting to see that the dIMU performs best at the turn and turn back test. This makes me believe that the dIMU has a scaling error. It would take another filter to compensate for that.

The EV3 in angle mode seems not to drift at all. In rate mode the drift is huge, all on account of rounding off errors. the dIMU has very little drift. A test with really really slow turns showed that the angle given by the EV3 in angle mode remains zero, even after a ninety degree turn. So something in the sensor makes it ignore very small changes.

Overall both the software solution and the EV3 sensor in angle mode perform satisfactory. There is no clear advantage to one one of them. If one goes for the software solution one must use a sensor that has an good resolution.

The tests were run using leJOS 0.8.2 beta. This is not released at the time of writing. The OffsetCorrectionFilter of earlier releases does not perform well.

 

Arduino EV3 UART Emulation Library

The previous article explained how to create LEGO Mindstorms EV3 UART sensors using an Arduino. There is now an Arduino library called EV3UARTEmulation that makes wrting the Arduino program a lot simpler. For the Nose sensor, the program is now:

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

EV3UARTEmulation sensor(10, 11, 99, 38400); // rx pin, tx pin, type, bit rate

void setup() {
  // Single short value with 3 decimal digits
  sensor.create_mode("NOSE", true, DATA16, 1, 3, 0); 
  sensor.reset();
}

unsigned long last_reading = 0;

void loop() {
  sensor.heart_beat();
  if (millis() - last_reading > 100) {
    sensor.send_data16(0, analogRead(0)); // Read MQ-2 every 100 milliseconds
    last_reading = millis();
  }
}

Arduino UART sensors

2014-06-03 17.48.12

Do you want to create your own UART sensors for the EV3?

Here is how to do do it using an Arduino.

We will implement a Nose sensor for our robot, using an MQ-2 gas sensor that you can buy cheaply from ebay.

The Arduino in the picture is a rather big Arduino Mega 2560. It would be better to use one of the many tiny format Arduinos, and a 3D printed case for it would be good. The sensor can be powrered by the EV3. You could use the Dexter Industries breadboard adapter instead of the cut up NXT/EV3 cable, to make things look neater.

You will need a NoseSensor class:

package mypackage;

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

public class NoseSensor extends UARTSensor implements SensorMode {

	public NoseSensor(Port port) {
		super(port);
	}

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

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

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

You will need a test program:

package mypackage;

import lejos.hardware.Button;
import lejos.hardware.port.SensorPort;

public class Nose {

	public static void main(String[] args) {
		NoseSensor nose = new NoseSensor(SensorPort.S1);
		float[] sample = new float[nose.sampleSize()];
		
		while(Button.ESCAPE.isUp()) {
			nose.fetchSample(sample,0);
			System.out.println("Smelliness: " + sample[0]);
		}	
		nose.close();
	}
}

And finally, you will need an Arduino sketch that implements the EV3 UART protocol, and reads the data from the MQ-2. This one is very messy and full of diagnostics. I plan to write an Arduino library to make this much easier. This code uses the Arduino USB serial port (Serial) for sending optional diagnostics to your PC, and a second hardware serial link (Serial1) for the UART link to the EV3. You could use Serial for the UART if you remove the diagnostics, or you can use SoftwareSerial.

#include <Serial.h>

#define MY_TYPE 99
#define MY_NAME "NOSE"
#define SPEED 38400

#define   BYTE_ACK                      0x04
#define   BYTE_NACK                     0x02
#define   CMD_TYPE                      0x40
#define   CMD_MODES                     0x49
#define   CMD_SPEED                     0x52
#define   CMD_MASK                      0xC0
#define   CMD_INFO                      0x80
#define   CMD_LLL_MASK                  0x38
#define   CMD_LLL_SHIFT                 3
#define   CMD_MMM_MASK                  0x07
#define   CMD_DATA                      0xc0

unsigned long last_nack = 0;

void setup() {
  Serial.begin(115200);
  reset();
}

unsigned long last_message;

void loop() {
  byte cmd;
  if (millis() - last_nack > 10000) reset();
  if (Serial1.available()) {
      cmd = Serial1.read();
      Serial.print("Received: ");
      Serial.println(cmd, HEX);
      if (cmd == BYTE_NACK) last_nack = millis();
  }
  if (millis() - last_message > 100) {
    byte bb[2];
    int r = analogRead(0);
    bb[0] = r & 0xff;
    bb[1] = r >> 8;
    
    send_cmd(CMD_DATA | (1 << CMD_LLL_SHIFT), bb, 2); // mode 0, length 2
    last_message = millis();
  }
}

void reset() {
  for(;;) {
    Serial.println("Resetting");
    byte bb[4];
    Serial1.begin(2400);
    delay(50);
    send_cmd(CMD_TYPE, MY_TYPE);
    bb[0] = 0; // modes = 1
    bb[1] = 0; // views = 1
    send_cmd(CMD_MODES, bb, 2);
    byte* speed = get_bytes(SPEED);
    send_cmd(CMD_SPEED, speed, 4);
    send_cmd(CMD_INFO | (2 << CMD_LLL_SHIFT), 0, (byte*) MY_NAME, 4);
    bb[0] = 1; // sets
    bb[1] = 1; // format Data16
    bb[2] = 4; // figures
    bb[3] = 0; // decimals
    send_cmd(CMD_INFO | (2 << CMD_LLL_SHIFT), 0x80, bb, 4);
    Serial1.write(BYTE_ACK);
    Serial.println("ACK Sent");
    unsigned long m = millis();
    while(!Serial1.available() && millis() - m < 1000);
    if (Serial1.available()) {
      byte cmd = Serial1.read();
      Serial.print("Received for Ack: ");
      Serial.println(cmd, HEX);
      if (cmd == BYTE_ACK) {
        Serial1.end();
        Serial1.begin(SPEED);
        delay(80);
        last_nack = millis();
        break;
      }
    } else Serial.println("ACK timeout");
  }
}

byte* get_bytes(unsigned long val) {
  byte bb[4];
  
  for(int i=0;i<4;i++) {
    bb[i] = (val >> (i *8)) & 0xff;
  }
  return bb;
}

void send_cmd(byte cmd, byte type, byte* data, byte len) {
  byte checksum = 0xff ^ cmd;
  Serial.print("Sent: ");
  Serial.print(cmd, HEX);
  Serial1.write(cmd);
  checksum ^= type;
  Serial1.write(type);
  Serial.print(type, HEX);
  for(int i=0;i<len;i++) {
    checksum ^= data[i];
    Serial1.write(data[i]);
    Serial.print(data[i], HEX);
  }
  Serial1.write(checksum);
  Serial.println(checksum, HEX);
}

void send_cmd(byte cmd, byte data) {
  byte bb[1];
  bb[0] = data;
  send_cmd(cmd,bb,1);
}

void send_cmd(byte cmd) {
  byte checksum = 0xff ^ cmd;
  Serial1.write(cmd);
  Serial.print("Sent: ");
  Serial.print(cmd, HEX);
  Serial1.write(checksum);
  Serial.println(checksum, HEX);
}

void send_cmd(byte cmd, byte* data, byte len) {
  byte checksum = 0xff ^ cmd;
  Serial1.write(cmd);
    Serial.print("Sent: ");
  Serial.print(cmd, HEX);
  for(int i=0;i<len;i++) {
    checksum ^= data[i];
    Serial1.write(data[i]);
    Serial.print(data[i], HEX);
  }
  Serial1.write(checksum);
  Serial.println(checksum, HEX);
}

Audio playback improvements

For some time now I’ve not been happy with the playback quality of audio in leJOS on the EV3. Although the EV3 speaker is small we really should have been able to do better. The audio playback mechanism used in 0.8.1-beta and earlier is based upon the standard Lego audio playback kernel modules. It works by using a combination of the EV3 PWM (pulse width modulation) hardware and a high resolution timer to provide playback using a fixed sample rate (8KHz) and a fixed sample size (8 bits). It works but the results are not great. Here is a recording of the EV3 playing an audio clip

Not too bad but a little shaky and distorted. To really hear what is going on, we need a simpler purer sample. Here is a recording of the EV3 repeatedly playing a simple 3 note piano tune

You can hear how distorted the playback is and that there are noticeable clicks at the start and end of playing the .wav file.

Doing something about this has been on my “todo” list for some time, but I’d not found a good way to address the issues. Then a few weeks ago the ev3dev team posted a video showing the audio capabilities of ev3dev. Although the code was not directly usable in the leJOS Linux kernel some of the ideas certainly were. After a little bit of work in the kernel and kernel modules we get


and

which are much better. As part of this work the supported formats have also been improved, you can now use rates from 8KHz to 48KHz with 8bit or 16bit samples.

So why is the new implementation better? The original code used the pwm hardware to feed a series of on/off pulses to the an audio amplifier. The duration of these pulses was changed at the sample rate (8KHz) and the pulse width was basically proportional to the sample value. Things were actually a little more complex as the pwm hardware actually generated 8 pulses per sample, which raised the sample frequency above that audible to the human ear (a technique called over sampling). Although in theory this is a good way to generate an audio waveform (given the limited resources available), the actual implementation had problems:

  • The sample rate was too low for good quality.
  • The sample size was too small

Increasing the sample size is relatively easy, but needs care because the value defining the maximum pulse length varies as the pulse rate changes and basically gets smaller as the rate increases (which in turn reduces the available resolution).

Increasing the sample rate is not so simple. The hires timer is generating an interrupt 8000 times a second. This imposes a load on the system and even at this rate the interrupts are not always delivered exactly on time. This leads to jitter in the way the samples are played and the ear is very good at detecting these sorts of problems. Increasing the sample rate to 16KHz and above would simply impose too much load on the system and the jitter would get much worse.

The solution used by the ev3dev team (and now leJOS), is to make use of more of the EV3 hardware, the fiq and additional features of the pwm unit. The processor chip used in the EV3 supports something called a fiq (Fast Interrupt reQuest), which is basically a very fast interrupt mechanism, that completely bypasses the standard Linux interrupt system. Using this allows for very low overhead high frequency interrupts that are always delivered on time. In our new solution these interrupts are generated not by a timer (as in the original implementation), which is hard to synchronized with the PWM hardware, but instead by the PWM hardware itself. The AM1808 PWM unit has a feature that will generate an ineterrupt after 1, 2 or 3 complete PWM cycles. This is used to trigger the fiq which in turn loads the new pulse width into the PWM unit. A combination of these two hardware features allow 16 bit samples to be fed smoothly to the PWM unit at rates between 24KHz and 64KHz, with very little intervention by the Linux kernel.