MPU6050 sensorA(0x68);
MPU6050 sensorB(0x69);
On the hardware side, the MPU-6050 listens to its ADO pin to know which address it has been assigned. If the voltage is low, it associates with 0x68. If high, 0x69. We’ll need to connect an output pin on the Arduino to the ADO pin of the MPU-6050. Suppose we use pin 8 on the Arduino for the 0x68 sensor. In our sketch, we add:
#define ADO68 8
pinMode(ADO68, OUTPUT);
digitalWrite(ADO68, LOW);
Since each sensor will receive a different ADO signal, we need an output pin per sensor. Let’s add pin 9 for the 0x69 sensor.
#define ADO69 9
pinMode(ADO69, OUTPUT);
digitalWrite(ADO69, HIGH);
So, my connections looked like this, from Arduino to the MPU-6050s:
The first four connections are all shared. We can fan out the signal. Also note that there’s no connection to INT here. More on that later.
All this is great, but we had hoped to talk to more than two MPUs. Two addresses wasn’t going to be enough. When I searched the internet to figure out how to scale this up, I found myself in uncharted waters. People said it probably could be done, but I didn’t find anybody that lived to tell about it. What I did find suggested that I should be able to read from a whole lot of MPUs with the following algorithm, which cycles through all the sensors and has only the active sensor associated with 0x68:
for each mpu
set mpu to 0x68 through its ADO
for each other mpu
set other mpu to 0x69 through its ADO
read the 0x68 mpu
On the software side, all MPUs stay registered with 0x68. It’s only on the hardware side where things change—the inactive ones respond to 0x69 commands, of which there are none because the software is only talking to 0x68. In this way, at any given moment, only the active MPU will ever send data.
I made an Accelerometer class to manage the state of a single MPU-6050 and threw the activation cycling into loop. Here’s the complete sketch, which depends on the I2Cdev library:
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
// CONSTANTS ------------------------------------------------------------------
#define MPU_COUNT 2
const int ADO_PINS[MPU_COUNT] = {8, 9};
#define READ_ADDRESS 0x68 // could have been 0x69
#define MILLIS_TILL_RESET 1000
#define MILLIS_TILL_SHIP 100
// TYPES ----------------------------------------------------------------------
class Accelerometer : public MPU6050 {
public:
Accelerometer(int id) :
MPU6050(READ_ADDRESS),
id(id) {
}
bool initialize() {
MPU6050::initialize();
pinMode(ADO_PINS[id], OUTPUT);
last_serial_at = millis();
dev_status = dmpInitialize();
if (dev_status == 0) {
Serial.println(F("Enabling DMP..."));
setXAccelOffset(-1093);
setYAccelOffset(1662);
setZAccelOffset(1526);
setXGyroOffset(37);
setYGyroOffset(-40);
setZGyroOffset(57);
setDMPEnabled(true);
packet_size = dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(dev_status);
Serial.println(F(")"));
}
return dev_status == 0;
}
void deactivate() {
int drop_state = READ_ADDRESS == 0x68 ? HIGH : LOW;
digitalWrite(ADO_PINS[id], drop_state);
}
void activate() {
int read_state = READ_ADDRESS == 0x68 ? LOW : HIGH;
digitalWrite(ADO_PINS[id], read_state);
}
void read() {
if (dev_status != 0) {
Serial.print("reinitializing ");
Serial.println(id);
initialize();
}
if (dev_status != 0) {
return;
}
// Wait until we have enough data. Once we do, let's push out a
// quarternion onto the serial port! After abandoning interrupts,
// which didn't make sense with multiple MPU6050s issuing them,
// this code would occasionally hang. The workaround for the time
// being is to reset the MPU if it takes to long to acquire a
// full packet.
fifo_size = getFIFOCount();
if (fifo_size == 1024) {
resetFIFO();
Serial.println(F("FIFO overflow!"));
} else {
bool is_reset = false;
unsigned long polling_started_at = millis();
while (!is_reset && fifo_size < packet_size) {
fifo_size = getFIFOCount();
if (millis() - polling_started_at > MILLIS_TILL_RESET) {
Serial.print("resetting ");
Serial.println(id);
is_reset = true;
}
}
if (is_reset) {
initialize();
resetFIFO();
} else {
getFIFOBytes(fifo_buffer, packet_size);
fifo_size -= packet_size;
emit_quaternion();
}
}
}
void emit_quaternion() {
// The serial reading in Unity seems to get bogged down if the data
// comes in too quickly. We issue a serial write only once every
// MILLIS_TILL_SHIP milliseconds.
unsigned long presentTime = millis();
unsigned long elapsedTime = presentTime - last_serial_at;
if (elapsedTime > MILLIS_TILL_SHIP) {
dmpGetQuaternion(&q, fifo_buffer);
Serial.print("[");
Serial.print(id);
Serial.print("\t");
Serial.print(q.w, 5);
Serial.print("\t");
Serial.print(q.x, 5);
Serial.print("\t");
Serial.print(q.y, 5);
Serial.print("\t");
Serial.print(q.z, 5);
Serial.println("]");
last_serial_at = presentTime;
}
}
private:
int id;
uint16_t fifo_size;
uint8_t fifo_buffer[64];
uint8_t dev_status;
uint16_t packet_size;
unsigned long last_serial_at;
Quaternion q;
};
// GLOBALS --------------------------------------------------------------------
bool is_dmp_ready = false;
Accelerometer mpus[] = {
Accelerometer(0),
Accelerometer(1)
};
// ----------------------------------------------------------------------------
void setup() {
Wire.begin();
Wire.setClock(400000);
Serial.begin(115200);
Serial.println("Gimme a char, please, and then I'll start up the MPUs.");
while (Serial.available() && Serial.read());
while (!Serial.available());
while (Serial.available() && Serial.read());
Serial.println("Initializing DMP...");
is_dmp_ready = true;
for (int i = 0; i < MPU_COUNT; ++i) {
is_dmp_ready = is_dmp_ready && mpus[i].initialize();
}
}
// ----------------------------------------------------------------------------
void loop() {
if (!is_dmp_ready) return;
for (int i = 0; i < MPU_COUNT; ++i) {
// Associate this MPU with the read address.
mpus[i].activate();
// Divert all other MPUs to the ignored address.
for (int oi = 0; oi < MPU_COUNT; ++oi) {
if (i != oi) {
mpus[oi].deactivate();
}
}
mpus[i].read();
}
}
The code is largely a reorganization of the I2Cdev example that deals with a single MPU-6050, with activation cycling thrown in. Another significant difference is that I removed all the interrupt logic. When a sensor has data, it can signal the Arduino that it’s ready to be read from. But for this to work, each sensor would need to have its own interrupt pin, and I don’t have that many pins. I wanted to strip out the nonessentials.
Removing interrupts led to frequent hanging. I don’t really know what was wrong, but resetting the sensor if it took too long to fill its buffer seemed to set things right!
To test things out, I went to Unity and wired up one sensor to the left box and the other to the right box with this controller:
using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System;
using UnityEngine;
public class GameController : MonoBehaviour {
public Transform left;
public Transform right;
private SerialPort serial;
private bool isReading;
void Start() {
serial = new SerialPort("/dev/tty.usbmodem1411", 115200);
serial.ReadTimeout = 100;
serial.Open();
isReading = false;
}
void Update() {
if (Input.GetKeyDown (KeyCode.I)) {
serial.WriteLine ("y");
Debug.Log("start 'em");
isReading = true;
}
if (isReading) {
try {
string line = serial.ReadLine().TrimEnd();
if (line.StartsWith("[") && line.EndsWith("]")) {
string[] thingys = line.Substring(1, line.Length - 2).Split('\t');
if (thingys.Length == 5) {
int i;
int.TryParse(thingys[0], out i);
Quaternion q = new Quaternion();
float.TryParse(thingys[1], out q.w);
float.TryParse(thingys[2], out q.y);
float.TryParse(thingys[3], out q.z);
float.TryParse(thingys[4], out q.x);
q.x *= -1;
q.y *= -1;
if (i == 0) {
left.localRotation = q;
} else {
right.localRotation = q;
}
}
} else {
Debug.Log(line);
}
} catch (TimeoutException) {
}
}
}
}
And here are the two sensors in action:
I think my breadboard connections are a little loose. When the wires don’t bounce around, the rotation is pretty smooth.
Now, let’s get sixteen MPU-6050s working!
]]>Our first task was to figure out how to talk to an accelerometer. We settled on an MPU-6050, which meant nothing to us but came up in a lot of internet searches. One thing I didn’t quite understand about the electronics market is that one company will fabricate a barebones sensor, and a bunch of others will purchase (or license?) them and integrate them into breakout boards that are easier to tinker with and connect to ad hoc circuits on a breadboard. The core MPU-6050 is made by InvenSense. We bought these packaged on breakout boards from Longruner—because they had five stars on Amazon.
They arrived without headers attached. I needed to solder them in. Let’s not talk about that.
Once the headers were soldered, we were able to insert the sensors into a breadboard and start making connections. Tutorials told me to make these connections from an Arduino to the sensor, and I willingly obeyed:
I’ve only slowly learned what each of the sensor pins does. Here’s my current understanding:
From there we cloned Jeff Rowberg’s I2Cdev library. We copied out Arduino/MPU6050 and Arduino/I2Cdev to our Arduino project’s lib directory, and Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino to our project’s src directory. And the accelerometer readings flowed…
But the readings were unstable. There was considerable drift even when the accelerometer was absolutely idle. I stumbled across this script to help calibrate the sensitivities:
// Arduino sketch that returns calibration offsets for MPU6050 // Version 1.1 (31th January 2014)
// Done by Luis Rodenas <luisrodenaslorda@gmail.com>
// Based on the I2Cdev library and previous work by Jeff Rowberg <jeff@rowberg.net>
// Updates (of the library) should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
// These offsets were meant to calibrate MPU6050's internal DMP, but can be also useful for reading sensors.
// The effect of temperature has not been taken into account so I can't promise that it will work if you
// calibrate indoors and then use it outdoors. Best is to calibrate and use at the same room temperature.
/* ========== LICENSE ==================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2011 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
=========================================================
*/
// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
/////////////////////////////////// CONFIGURATION /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone=8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone=1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // <-- use for AD0 high
int16_t ax, ay, az,gx, gy, gz;
int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
/////////////////////////////////// SETUP ////////////////////////////////////
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
// initialize serial communication
Serial.begin(115200);
// initialize device
accelgyro.initialize();
// wait for ready
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()){
Serial.println(F("Send any character to start sketch.\n"));
delay(1500);
}
while (Serial.available() && Serial.read()); // empty buffer again
// start message
Serial.println("\nMPU6050 Calibration Sketch");
delay(2000);
Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
delay(3000);
// verify connection
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(1000);
// reset offsets
accelgyro.setXAccelOffset(0);
accelgyro.setYAccelOffset(0);
accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(0);
accelgyro.setYGyroOffset(0);
accelgyro.setZGyroOffset(0);
}
/////////////////////////////////// LOOP ////////////////////////////////////
void loop() {
if (state==0){
Serial.println("\nReading sensors for first time...");
meansensors();
state++;
delay(1000);
}
if (state==1) {
Serial.println("\nCalculating offsets...");
calibration();
state++;
delay(1000);
}
if (state==2) {
meansensors();
Serial.println("\nFINISHED!");
Serial.print("\nSensor readings with offsets:\t");
Serial.print(mean_ax);
Serial.print("\t");
Serial.print(mean_ay);
Serial.print("\t");
Serial.print(mean_az);
Serial.print("\t");
Serial.print(mean_gx);
Serial.print("\t");
Serial.print(mean_gy);
Serial.print("\t");
Serial.println(mean_gz);
Serial.print("Your offsets:\t");
Serial.print(ax_offset);
Serial.print("\t");
Serial.print(ay_offset);
Serial.print("\t");
Serial.print(az_offset);
Serial.print("\t");
Serial.print(gx_offset);
Serial.print("\t");
Serial.print(gy_offset);
Serial.print("\t");
Serial.println(gz_offset);
Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
while (1);
}
}
/////////////////////////////////// FUNCTIONS ////////////////////////////////////
void meansensors(){
long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;
while (i<(buffersize+101)){
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
buff_ax=buff_ax+ax;
buff_ay=buff_ay+ay;
buff_az=buff_az+az;
buff_gx=buff_gx+gx;
buff_gy=buff_gy+gy;
buff_gz=buff_gz+gz;
}
if (i==(buffersize+100)){
mean_ax=buff_ax/buffersize;
mean_ay=buff_ay/buffersize;
mean_az=buff_az/buffersize;
mean_gx=buff_gx/buffersize;
mean_gy=buff_gy/buffersize;
mean_gz=buff_gz/buffersize;
}
i++;
delay(2); //Needed so we don't get repeated measures
}
}
void calibration(){
ax_offset=-mean_ax/8;
ay_offset=-mean_ay/8;
az_offset=(16384-mean_az)/8;
gx_offset=-mean_gx/4;
gy_offset=-mean_gy/4;
gz_offset=-mean_gz/4;
while (1){
int ready=0;
accelgyro.setXAccelOffset(ax_offset);
accelgyro.setYAccelOffset(ay_offset);
accelgyro.setZAccelOffset(az_offset);
accelgyro.setXGyroOffset(gx_offset);
accelgyro.setYGyroOffset(gy_offset);
accelgyro.setZGyroOffset(gz_offset);
meansensors();
Serial.println("...");
if (abs(mean_ax)<=acel_deadzone) ready++;
else ax_offset=ax_offset-mean_ax/acel_deadzone;
if (abs(mean_ay)<=acel_deadzone) ready++;
else ay_offset=ay_offset-mean_ay/acel_deadzone;
if (abs(16384-mean_az)<=acel_deadzone) ready++;
else az_offset=az_offset+(16384-mean_az)/acel_deadzone;
if (abs(mean_gx)<=giro_deadzone) ready++;
else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);
if (abs(mean_gy)<=giro_deadzone) ready++;
else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);
if (abs(mean_gz)<=giro_deadzone) ready++;
else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);
if (ready==6) break;
}
}
The output is six numbers used to normalize the sensor readings. We plugged these numbers into the following lines of the MPU6050_DMP6.ino sketch:
mpu.setXAccelOffset(ax);
mpu.setYAccelOffset(ay);
mpu.setZAccelOffset(az);
mpu.setXGyroOffset(gx);
mpu.setYGyroOffset(gy);
mpu.setZGyroOffset(gz);
The results were much more stable.
With working hardware, it was time to do something interesting. Enter my nine-year-old son. He had a science and technology fair coming up, so I asked him if he wanted to make a little game for which he’d build his own controller. We agreed that it should be a flying game to test the full range of the accelerometer’s motion. He found a donut model and a fork model, and he put it all together in Unity. Here’s the end result:
At the beginning of each level, the system needs a few seconds to settle down. I’m not sure I understand why. Once it settles, we capture the current orientation and invert it. (I helped him with this part.) Each frame, we apply the inverse transformation to cancel out that initial wild aim and realign the fork it to its resting posture.
The fork tracks the accelerometer wonderfully well. I taught my son about low-pass filters, and he implemented one to smooth out some of the jerkiness.
Now it’s time to hook up more than one MPU-6050 in one circuit. Wish us luck!
]]>