//
you're reading...
Projects

Homemade IMU

2014-09-18 20.46.18

Here is an example of a more useful homemade EV3 UART sensor. It is an IMU that uses an MPU-9150 breakout board that can be bought cheaply on ebay. I am usinh a cheap Arduino Nano clone.

The sensor supports 4 modes: Temperature, Compass, Gyro and Acceleration. The last three are all 3D, and have X, Y and Z values. you can program the sensor to do interesting things like on-board fusion of the gyro and acceleration data.

The USB cable in the picture is used for programming the Arduino Nano, and for diagnostics. When used in anger, the device is powered from the EV3.

The EV3UARTEmulation library is used to implement the EV3 UART functionality and the code for driving the MPu-9150 is based on that on the Arduino playground page.

This is the Arduino code for the sensor:

// Some code copied from the MPU-9150 Arduino Playground page.
#include <EV3UARTEmulation.h>
#include <SoftwareSerial.h>
#include <Wire.h>

EV3UARTEmulation sensor(7, 8, 99, 38400);

#define MPU9150_TEMP_OUT_H         0x41
#define MPU9150_TEMP_OUT_L         0x42
#define MPU9150_PWR_MGMT_1         0x6B

#define MPU9150_CMPS_XOUT_L        0x4A   // R
#define MPU9150_CMPS_XOUT_H        0x4B   // R
#define MPU9150_CMPS_YOUT_L        0x4C   // R
#define MPU9150_CMPS_YOUT_H        0x4D   // R
#define MPU9150_CMPS_ZOUT_L        0x4E   // R
#define MPU9150_CMPS_ZOUT_H        0x4F   // R

#define MPU9150_GYRO_XOUT_H        0x43   // R  
#define MPU9150_GYRO_XOUT_L        0x44   // R  
#define MPU9150_GYRO_YOUT_H        0x45   // R  
#define MPU9150_GYRO_YOUT_L        0x46   // R  
#define MPU9150_GYRO_ZOUT_H        0x47   // R  
#define MPU9150_GYRO_ZOUT_L        0x48   // R

#define MPU9150_ACCEL_XOUT_H       0x3B   // R  
#define MPU9150_ACCEL_XOUT_L       0x3C   // R  
#define MPU9150_ACCEL_YOUT_H       0x3D   // R  
#define MPU9150_ACCEL_YOUT_L       0x3E   // R  
#define MPU9150_ACCEL_ZOUT_H       0x3F   // R  
#define MPU9150_ACCEL_ZOUT_L       0x40   // R 

void setup() {
  Serial.begin(9600);
  
  Wire.begin();
  
  // Clear the 'sleep' bit to start the sensor.
  MPU9150_writeSensor(MPU9150_PWR_MGMT_1, 0);
  
  MPU9150_setupCompass();
  
  sensor.create_mode("Temperature", true, DATA16, 1, 2, 0);
  sensor.create_mode("Compass", true, DATA16, 3, 5, 0);
  sensor.create_mode("Gyro", true, DATA16, 3, 5, 0);
  sensor.create_mode("Accel", true, DATA16, 3, 5, 0);
  
  sensor.reset();
}

unsigned long last_reading = 0;
short heading[4];
short gyro[4];
short accel[4];
short t;

void loop() {
  sensor.heart_beat();
  if (millis() - last_reading > 100) {
    switch (sensor.get_current_mode()) {
    case 0:
      t = ((int) MPU9150_readSensor(MPU9150_TEMP_OUT_L,MPU9150_TEMP_OUT_H) + 12412.0) / 340.0;
      sensor.send_data16(t);
      break;
    case 1:
      heading[0] = MPU9150_readSensor(MPU9150_CMPS_XOUT_L,MPU9150_CMPS_XOUT_H);
      heading[0] = MPU9150_readSensor(MPU9150_CMPS_YOUT_L,MPU9150_CMPS_YOUT_H);
      heading[1] = MPU9150_readSensor(MPU9150_CMPS_ZOUT_L,MPU9150_CMPS_ZOUT_H);
      sensor.send_data16(heading, 4);
      break;
    case 2:
      gyro[0] = MPU9150_readSensor(MPU9150_GYRO_XOUT_L,MPU9150_GYRO_XOUT_H);
      gyro[1] = MPU9150_readSensor(MPU9150_GYRO_YOUT_L,MPU9150_GYRO_YOUT_H);
      gyro[2] = MPU9150_readSensor(MPU9150_GYRO_ZOUT_L,MPU9150_GYRO_ZOUT_H);
      sensor.send_data16(gyro, 4);
      break;
    case 3:
      accel[0] = MPU9150_readSensor(MPU9150_ACCEL_XOUT_L,MPU9150_ACCEL_XOUT_H);
      accel[1] = MPU9150_readSensor(MPU9150_ACCEL_YOUT_L,MPU9150_ACCEL_YOUT_H);
      accel[2] = MPU9150_readSensor(MPU9150_ACCEL_ZOUT_L,MPU9150_ACCEL_ZOUT_H);
      sensor.send_data16(accel, 4);
      break;     
    }
    last_reading = millis();
  }
}

int MPU9150_I2C_ADDRESS = 0x68;

int MPU9150_readSensor(int addr){
  Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  Wire.write(addr);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU9150_I2C_ADDRESS, 1, true);
  return Wire.read();
}

int MPU9150_readSensor(int addrL, int addrH){
  Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  Wire.write(addrL);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU9150_I2C_ADDRESS, 1, true);
  byte L = Wire.read();

  Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  Wire.write(addrH);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU9150_I2C_ADDRESS, 1, true);
  byte H = Wire.read();

  return (int16_t)((H<<8)+L);
}

int MPU9150_writeSensor(int addr,int data){
  Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  Wire.write(addr);
  Wire.write(data);
  Wire.endTransmission(true);

  return 1;
}

void MPU9150_setupCompass(){
  MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass

  MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
  MPU9150_writeSensor(0x0A, 0x0F); //SelfTest
  MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode

  MPU9150_I2C_ADDRESS = 0x68;      //change Adress to MPU

  MPU9150_writeSensor(0x24, 0x40); //Wait for Data at Slave0
  MPU9150_writeSensor(0x25, 0x8C); //Set i2c address at slave0 at 0x0C
  MPU9150_writeSensor(0x26, 0x02); //Set where reading at slave 0 starts
  MPU9150_writeSensor(0x27, 0x88); //set offset at start reading and enable
  MPU9150_writeSensor(0x28, 0x0C); //set i2c address at slv1 at 0x0C
  MPU9150_writeSensor(0x29, 0x0A); //Set where reading at slave 1 starts
  MPU9150_writeSensor(0x2A, 0x81); //Enable at set length to 1
  MPU9150_writeSensor(0x64, 0x01); //overvride register
  MPU9150_writeSensor(0x67, 0x03); //set delay rate
  MPU9150_writeSensor(0x01, 0x80);

  MPU9150_writeSensor(0x34, 0x04); //set i2c slv4 delay
  MPU9150_writeSensor(0x64, 0x00); //override register
  MPU9150_writeSensor(0x6A, 0x00); //clear usr setting
  MPU9150_writeSensor(0x64, 0x01); //override register
  MPU9150_writeSensor(0x6A, 0x20); //enable master i2c mode
  MPU9150_writeSensor(0x34, 0x13); //disable slv4
}

and here is the leJOS sensor driver class:

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

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

	public IMU(Port port) {
		super(port);
		setModes(new SensorMode[] { new TemperatureMode(), new CompassMode(), new GyroMode(), new AccelerationMode()});
	}
	
	private class TemperatureMode implements SensorMode {
	    private static final int   MODE = 0;
		private int temp;
	
	    @Override
	    public int sampleSize() {
	    	return 1;
	    }
	
	    @Override
	    public void fetchSample(float[] sample, int offset) {
	    	switchMode(MODE, SWITCHDELAY);
	    	temp = port.getShort();
			sample[offset] = temp;
	    }
	
	    @Override
	    public String getName() {
	    	return "Temperature";
	    }
	}
	
	public SampleProvider getTemperatureMode() {
		return getMode(0);
	}
	
	private class CompassMode implements SensorMode {
	    private static final int   MODE = 1;
		private short[] heading = new short[sampleSize()];
	
	    @Override
	    public int sampleSize() {
	    	return 3;
	    }
	
	    @Override
	    public void fetchSample(float[] sample, int offset) {
	    	switchMode(MODE, SWITCHDELAY);
			port.getShorts(heading, 0 ,3);
			for(int i=0;i<sampleSize();i++) sample[offset+i] = heading[i];
	    }
	
	    @Override
	    public String getName() {
	    	return "Compass";
	    }
	}
	
	public SampleProvider getCompassMode() {
		return getMode(1);
	}
	
	private class GyroMode implements SensorMode {
	    private static final int   MODE = 2;
		private short[] gyro = new short[sampleSize()];
	
	    @Override
	    public int sampleSize() {
	    	return 3;
	    }
	
	    @Override
	    public void fetchSample(float[] sample, int offset) {
	    	switchMode(MODE, SWITCHDELAY);
			port.getShorts(gyro, 0 ,3);
			for(int i=0;i<sampleSize();i++) sample[offset+i] = gyro[i];
	    }
	
	    @Override
	    public String getName() {
	    	return "Gyro";
	    }
	}
	
	public SampleProvider getGyroMode() {
		return getMode(2);
	}
	
	private class AccelerationMode implements SensorMode {
	    private static final int   MODE = 3;
		private short[] accel = new short[sampleSize()];
	
	    @Override
	    public int sampleSize() {
	    	return 3;
	    }
	
	    @Override
	    public void fetchSample(float[] sample, int offset) {
	    	switchMode(MODE, SWITCHDELAY);
			port.getShorts(accel, 0 ,3);
			for(int i=0;i<sampleSize();i++) sample[offset+i] = accel[i];
	    }
	
	    @Override
	    public String getName() {
	    	return "Acceleration";
	    }
	}
	
	public SampleProvider getAccelerationMode() {
		return getMode(3);
	}
}

and 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 IMUTest {
	public static void main(String[] args) {
		IMU sensor = new IMU(SensorPort.S1);
		SampleProvider temp = sensor.getTemperatureMode();
		SampleProvider heading = sensor.getCompassMode();
		SampleProvider gyro = sensor.getGyroMode();
		SampleProvider accel = sensor.getAccelerationMode();
		float[] sample = new float[heading.sampleSize()];
		
		LCD.drawString("Temp:", 0, 1);
		LCD.drawString("H X       Y", 0, 2);
		LCD.drawString("  Z", 0, 3);
		LCD.drawString("G X       Y", 0, 4);
		LCD.drawString("  Z", 0, 5);
		LCD.drawString("A X       Y", 0, 6);
		LCD.drawString("  Z", 0, 7);
		
		while(Button.ESCAPE.isUp()) {
			temp.fetchSample(sample, 0);
			LCD.drawInt((int) sample[0], 4, 5, 1);
			heading.fetchSample(sample,0);
			for (int i=0;i<2;i++) LCD.drawInt((int)sample[i], 6, 3 + i*8, 2);
			LCD.drawInt((int)sample[2], 6, 3, 3);
			gyro.fetchSample(sample, 0);
			for (int i=0;i<2;i++) LCD.drawInt((int)sample[i], 6, 3 + i*8, 4);
			LCD.drawInt((int)sample[2], 6, 3, 5);
		    accel.fetchSample(sample, 0);
			for (int i=0;i<2;i++) LCD.drawInt((int)sample[i], 6, 3 + i*8, 6);
			LCD.drawInt((int)sample[2], 6, 3, 7);
		}	
		sensor.close();
	}
}
Advertisements

Discussion

5 thoughts on “Homemade IMU

  1. Reblogged this on 612photonics and commented:
    This is a cool project with Nano.

    Posted by 612photonics | 2014/10/23, 18:22
  2. Is there any significant drift in the Gyro-values?

    Posted by philipjuhl | 2014/11/03, 12:16
  3. Where can I see the wiring schematics?

    Posted by Corey | 2015/04/05, 05:37
    • I did not produce a wiring schematic, but it is very straightworward. The 5v, GND, RX and TX from the EV3 connector go to 5v, GND, TX and RX on the Arduino, and 5V, GND, SCL and SDA from the Arduino go to VCC, GND, SCL and SDA on the sensor.

      Posted by Lawrie Griffiths | 2015/04/08, 09:59
  4. Hi, I just discovered the EV3 Uart emulation code. I have got an Arduino uno connected to an EV3 and with the test code, the EV3 recognizes it as a sensor (it recognized modes etc), but I am unable to get a value to the EV3.

    Any ideas?

    Posted by Michael Homeijer | 2017/02/11, 12:07

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: