Introduction: Wallace - DIY Autonomous Robot - Part 5 - Add IMU

About: Have enjoyed a mostly fun ride in an electronics and software career. Also enjoy latin dancing.

We are proceeding along with Wallace. The name Wallace came from a mix of "Wall-E", and from a previous project (voice-recognition), and in using the "espeak" utility, it sounded a bit british. And like a valet or butler. And that's the end goal: for this project to turn into something useful. Thus "Wallace".

Wallace can move around, he can avoid obstacles using IR distance sensors (recently, somehow they fried(?) (have to look into that when I get a chance), also has some acoustic distance sensors (three of those went bad at the same time, along with an MCP23017 expander) , and finally, can detect changes in motor-current to know when it's bumped into something.

In addition to the sensors, Wallace "remembers" the 100 moves, and has some rudimentary analysis using the movement history.

The goal so far for Wallace is to merely try to keep moving forward, and to know when it's stuck in some repeating pattern (such as in a corner) and not really moving forward.

I've gone through several iterations for movement and navigation, and the consistent headache has been during rotation.

Since Wallace is a tracked robot, and I wanted to keep things simpler in the software (for later), in order to turn I just have him pivot/rotate in place. Thus, apply equal but opposite power / duty cycle to the motors.

The problem encountered is due to the design of the Agent 390 robot platform. The track belts tend to rub against the sides. And worse, one side does so more than the other.

On flooring and going straight, it has not been a problem. It shows up on carpeting. I chose to keep Wallace off of the carpeting after it's tracks got grimy (they pick up grime extremely easy).

The real problem is when pivoting on flooring.

If I have the software apply a high level duty cycle, then it more or less consistently turns. However, during a low duty cycle, it may or may not actually turn. Or it may turn for a bit and then slow down. The pivoting action seems to be uncontrollable via the software, or at best very difficult.

The problem shows up during navigation and moving around or away from obstacles. It can either swing too wildly away, or it can get stuck attempting to make very minute shifts, without really even moving.

And so the above explanation motivated this Instructable.

Initially, I had wanted to dispense with, or delay introducing a motion-sensing unit (IMU), because they're A) complicated, B) noisy, C) errors can get introduced over time, and etc, etc. My thought had been that we could do very well by jumping ahead to time-of-flight IR laser sensors. And we could - using lasers we could know if the robot rotated or not, by tracking the changes in distance.

In fact, we could also (sort of) do that now, with the acoustic sensors.

However, all of that is a very indirect, complicated way to answer one simple question: "have we rotated or not?"

It seemed to me that jumping to use the ToF laser sensors would take me to the next level of software; namely, SLAM (Simultaneous Localization and Mapping). I was not ready to go there just yet.

It's a good thing to do a robot project in layers, with the first (lower) layers being simpler, and the latter (upper) layers being more abstract and tackling more difficult issues.

Layers can be thought of something like this:

  1. robot physical frame / mechanical structural basis
  2. rudimentary drive system (Raspberry, Roboclaw, motors, cabling, etc, basic software, keyboard-driven)
  3. essential circuitry to support sensors (bi-directional voltage shifter, port expander, E-Stop, power distribution, etc)
  4. obstacle-avoidance sensors (acoustic, IR)
  5. essential , basic positioning and movement - detection (accelerometer, gyro, magnetometer, motor encoders, wheel encoders)

You can come up with your own list. The points about this list are that you probably should do these more or less in that order, and also that if you spend some time at each layer to get each to a good working state, that should help you later as things get more complicated.

The above list could be more or less mapped to these conceptual layers in software.

  • SLAM (Simultaneous Localization and Mapping)
  • Control and Awareness of Movement, Rotation
  • Basic Obstacle Avoidance
  • Control and Detection Of Sensor Data
  • Essential Movement Forward, Backwards, Left and Right, Speed Up, Slow Down, Stop

As you can see, for this list, the first items would be the upper, more complicated layers that tackle more abstract issues and questions, such as "where am I" and "where am I going", while the latter items would be the lower software layers that handle the "how to talk/listen to sensor A" or "how to move this wheel".

Now, I'm not saying that when you start at a layer, you'll have completed it and then it's on the next layer, never to return to the previous one. A robot project can be a lot like modern, iterative software development methods (agile, SCRUM, etc).

I'm just saying to take time at each. You'll have to balance how much to do at each, and decide is what you're attempting at a certain layer worth the time and trouble.

There's a certain "conflict" or "tension" between two competing ideas or directions.

One is what I would call "plug-n-play" to solve problem A.

The other one is DIY (do it yourself). And that may not even be the best label for this other idea.

Here's an example of each, hopefully you'll see the tension or conflict between the two choices.

For this example, let's lump SLAM , obstacle-avoidance, and essential basic movement all as one problem to solve at the same time.

  1. If we decide to go the plug-n-play route, we immediately jump (depending on budget) to things like those top-mounted rotating lasers, or depth-of-field camera, or ToF lasers, and the IMU (topic of this Instructable).
  2. If we, on the other hand, want to go the second route, we may try to extract every possible bit of information out of some acoustic sensors or IR sensors, or no sensors at all - we just use motor-current monitoring (bump)

What can be said about #1 vs #2? One thing would be that we will have learned a lot more by doing #2. The limitations of having only acoustic sensors to work with, forces us to think about a lot more issues.

On the other hand, if we are too focused on doing things via #2, we may be wasting time, because we asking for more than we should from acoustic sensors.

One more concept or idea to think about: What mixture of hardware and software best answers the questions of "how to", and what mixture of software (and hardware?) answers the question of "what", "when", "where". Because "how to" is typically a lower-level question upon which "what", "when", and "where" depend in order to get an answer.

Anyway, all the above were just something to think about.

In my case, after a lot of effort and having the consistent annoying issue of track-friction and unable to get consistent control and movement, it is time to do something else.

Thus this Instructable - an IMU.

The goal is that if the IMU says that the robot is NOT pivoting, we increase the duty cycle. If we're pivoting too fast, we decrease the duty cycle.

Step 1: The IMU Sensor

And so our next sensor to add to Wallace is the IMU. After some research, I was settling on an MPU6050. But then at this time, the MPU9050 (and even more recently, the MPU9250) seemed like an even better idea.

My go-to source has been Amazon (in the U.S.). So I ordered two of them.

What I got in fact (there seems to be no control over this; that's what I don't like about Amazon) were two MPU92/65. I wonder a bit about the designation. Take a look at the images; that seems to be a "family" designation. In any case, that's what I'm stuck with.

Adding it is very simple -get a proto board with connecting tracks, solder the sensor to board, add a 10-pin screw terminal block (I got mine from Pololu).

In order to minimize any interference, I tried to place these sensors away from every thing else.

That also meant using some nylon bolts/nuts.

Am going to use the I2C protocol. Hopefully total wire length will not be too bad.

There is plenty of information elsewhere about the basic connections and voltage-levels, etc, so I won't repeat that here.

Step 2: Things Are Not Always Clean, Easy

At this writing, there doesn't seem to be a lot online for this particular MPU-92/65. What is available, just like with most sensors, seem to be examples using Arduino.

I try to make these Instructables a bit different by presenting a not-so-clean process, because things don't always work right away.

I suppose these Instructables are more similar to a blog than straight A-B-C, 1-2-3 "this is how you do it".

Step 3: Initial Test

From the images in the previous step, the red and black wires going to the sensors are of course VCC (5V) and GND. The green and yellow wires are the I2C connections.

If you have done other I2C projects, or have been following along with these series, then you already know about "i2cdetect", and that's the first step to know if the Raspberry can see the new sensor.

As you can see from the images in this step, our first attempt was unsuccessful. The IMU does not appear (should be device id 0x68).

However, the good news is that the I2C bus is operating. We do see one device 0x20 and it is the MCP23017 port expander (currently responsible for the HCSR04 acoustic sensors).

It's not easy to see in the image, but I did connect the same colored green and yellow wires from the IMU to the MCP23017 (see lower left in image)

We will have to do some troubleshooting.

Step 4: Troubleshooting

Using the continuity setting on a voltmeter (the one with the high-pitched tone) , I tested VCC(5V) , GND, SDA, and SCL connections. Those were good.

Next try was to disconnect the MCP23017 from the I2C bus, leaving only the MPU-92/65 on the bus. That proved fruitless - "i2cdetect" then showed no devices.

So, next, I unmounted the sensor from totem pole, and re-wired it straight to the 5V-to-3V bidirectional bus; i.e., straight to the Raspberry. (shorter wires?).

And voila. This time there is success. We see 0x68 show up using "i2cdetect".

But we don't yet know why it worked this time. Could it be the length of the wires? The previous location?

Note: It didn't make any difference whether ADO was grounded or not. It could be there are on-board pullup and pull-down resistors. The same might be true for FSYNC.

Next, I re-connected the MCP23017. So now we have two devices on the I2C bus. (see image). Succes, we now see both 0x20 and 0x68 with i2cdetect.

The videos go into a bit more of what happened during the troubleshooting.

Step 5: Reading the Sensor's Data

Various Approaches

I decided to take multiple approaches to getting useful information from the sensor. Here they are, not in any order:

  1. try some basic programming
  2. look through some online documentation on registers
  3. take a look at others' examples and / or code

Why these approaches? Why not just look for some existing library or code?

By experimenting and trying some ideas, we can better absorb some knowledge about not only this particular sensor, but also gain some technique, skill, and ways of thinking about tackling something new, and something that may not have a lot documentation; something that may have a lot of unknowns.

Also, once we've have played with and tried out some of our own ideas and gained some insight, we are in a better position to evaluate someone else's code or library.

For instance, after looking at some C++ code for the MPU9250 in github, I realized it was forcing me to use interrupts, which I don't yet wish to do.

Also, it comes with extra things like calibration; again, something I'm not yet interested in.

It may be that what I need to do to answer the simple question "is the robot rotating yes or no" could be answered very simply by just reading some registers.


At this writing, there doesn't seem to be much available on this sensor. In fact, if you take a look at the images that come with this Instructable, and take a close look at the inscriptions on the actual chips, it makes me wonder if this isn't a knock-off. I'm not relating what I see to anything from Invense. Regardless, I chose to look at the register-information for the models that i found: the MPU-6050, and the MPU-9250.

In both cases, the following is the same for both. And for starters, we assume it will also be the same for this MPU-92/65.

59 to 64  - accelerometer measurements
65 , 66    - temperature measurements
67 to 72  - gyroscope measurements
73 to 96  - external sensor data

An item of note: The MPU-6050 appears to NOT have a magnetometer, whereas the MPU-9250 (and we assume this one, too) does have one.

Some more interesting, hopefully useful information gleaned from the register-document:

Magnetometer Info:

magnetometer id : 0x48

registers 00 through 09:
00H WIA 0 1 0 0 1 0 0 0 
02H ST1 0 0 0 0 0 0 DOR DRDY 
03H HXL HX7 HX6 HX5 HX4 HX3 HX2 HX1 HX0 
04H HXH HX15 HX14 HX13 HX12 HX11 HX10 HX9 HX8 
05H HYL HY7 HY6 HY5 HY4 HY3 HY2 HY1 HY0 
06H HYH HY15 HY14 HY13 HY12 HY11 HY10 HY9 HY8 
07H HZL HZ7 HZ6 HZ5 HZ4 HZ3 HZ2 HZ1 HZ0 
08H HZH HZ15 HZ14 HZ13 HZ12 HZ11 HZ10 HZ9 HZ8 
09H ST2 0 0 0 BITM HOFL 0 0 0

a breakdown of what each register means:
HXL[7:0]: X-axis measurement data lower 8bit 
HXH[15:8]: X-axis measurement data higher 8bit 
HYL[7:0]: Y-axis measurement data lower 8bit 
HYH[15:8]: Y-axis measurement data higher 8bit 
HZL[7:0]: Z-axis measurement data lower 8bit 
HZH[15:8]: Z-axis measurement data higher 8bit


One other bit of information from the register docs is that there seemed to only be about 100 or so registers. So one tactic could be to write a simple program that accesses the device (0x68) and attempts to read a series of registers sequentially, without any regard to their meaning, just to see what data can be seen.

And then, do successive passes , using the same code, and compare the data from one pass vs the next.

The idea is that we could probably eliminate any registers that seem to have no data (zeros or FF ?) or that absolutely never change, and we could also focus in on the ones that do change.

Then, one we're looking at only the ones that change, add in an averaging function that averages the latest N reads of that register, to see if there is in fact a certain steady value for that register. This would assume we are keeping the sensor very still, and in the same location.

Finally, we could then gently try things with the sensor, such as nudging it (accelerometer, gyro), or blowing on it (temperature), or rotating it (the previous two plus magnetometer) and see what effect this has on the values.

I like to use the wiringPi library as much as possible. It has support for I2C.

First run:

 * to build:  gcc first.test.mpu9265.c -o first.test.mpu9265 -lwiringPi
 * to run:   sudo ./first.test.mpu9265
 * this program just output a range of (possible) registers from the MCP23017,
 * and then from the MPU9265 (or any other MPU at that 0x68 address)
 * I used it to validate if I could even read from the sensor, since I already
 * had confidence in the MCP23017.
 * *****************************************************************************/ 
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

int main(int argc, char** argv) {

	puts("Let's see what MCP23017 @ 0x20 has to say:");
	errno = 0;
	int deviceId1 = 0x20;
	int fd1 = wiringPiI2CSetup(deviceId1);
	if (-1 == fd1)
		fprintf (stderr, "Can't open  wiringPi I2C device : %s\n", strerror (errno)) ;
		return 1;

	for (int reg=0;reg<300;reg++) {

		fprintf(stderr,"%d ",	wiringPiI2CReadReg8(fd1,reg));fflush(stderr);


	puts("Let's see what MPU9265 @ 0x20 has to say:");
	errno = 0;
	int deviceId2 = 0x68;
	int fd2 = wiringPiI2CSetup(deviceId2);
	if (-1 == fd2)
		fprintf (stderr, "Can't open wiringPi I2C device: %s\n", strerror (errno)) ;
		return 1;

	for (int reg=0;reg<300;reg++) {

		fprintf(stderr,"%d ",	wiringPiI2CReadReg8(fd2,reg));fflush(stderr);


	return 0;

The second run:

 * to build:  gcc second.test.mpu9265.c -o second.test.mpu9265 -lwiringPi
 * to run:   sudo ./second.test.mpu9265 <device id either 0x20, 0x68, 0x69>
 * This program outputs the register number alongside the value read.
 * This makes it useful to pipe (redirect) the output to a file, and then
 * several runs can be done, to compare. It might give some insight into
 * what register are important, and how the data might behave.
 * *****************************************************************************/ 
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

int main(int argc, char** argv) {

	int deviceId = -1;

	if (0) {

	} else if (!strncmp(argv[1],"0x20",strlen("0x20"))) {

		deviceId = 0x20;

	} else if (!strncmp(argv[1],"0x68",strlen("0x68"))) {

		deviceId = 0x68;

	} else if (!strncmp(argv[1],"0x69",strlen("0x69"))) {

		deviceId = 0x69;


	puts("Let's see what MPU9265 @ 0x20 has to say:");
	errno = 0;
	int fd = wiringPiI2CSetup(deviceId);
	if (-1 == fd)
		fprintf (stderr, "Can't open wiringPi I2C device: %s\n", strerror (errno)) ;
		return 1;

	for (int reg=0;reg<300;reg++) {



	return 0;

The third run:

 * to build:  gcc third.test.mpu9265.c -o third.test.mpu9265 -lwiringPi
 * to run:   sudo ./third.test.mpu9265 <device id either 0x68, 0x69>
 * This program is a result of the second one.  It only reads from the
 * registers that indicated a difference between one run and the next.
 * *****************************************************************************/ 
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

int main(int argc, char** argv) {

	int deviceId = -1;

	if (0) {

	} else if (!strncmp(argv[1],"0x68",strlen("0x68"))) {

		deviceId = 0x68;

	} else if (!strncmp(argv[1],"0x69",strlen("0x69"))) {

		deviceId = 0x69;


	puts("Let's see what MPU9265 @ 0x20 has to say:");
	errno = 0;
	int fd = wiringPiI2CSetup(deviceId);
	if (-1 == fd)
		fprintf (stderr, "Can't open wiringPi I2C device: %s\n", strerror (errno)) ;
		return 1;

	for (int reg=61;reg<=73;reg++) {



	for (int reg=111;reg<=112;reg++) {



	for (int reg=189;reg<=201;reg++) {



	for (int reg=239;reg<=240;reg++) {



	return 0;

So what did we learn so far? The image of the table with colored highlighted areas indicates that the output seems to match the first sets of registers.

The results so far can generate new questions.

Question: why is there only one register result for the "external" group?

Question: what are all those unknown registers "??????"

Question: since the program is not interrupt-driven, did it request data too slow? too fast?

Question: can we affect the results by trying things with the sensor itself as it runs?

Step 6: Let's Dig More Into the Readings / Data

I think the next step before anything else is to enhance the program to:

  • be flexible in how much loop delay (ms)
  • be flexible in over how many readings to give a running average per register

(I had to attach the program as a file. Seemed to be an issue inserting it here. "fourth.test.mpu9265.c")

Here's a run using the last 10 readings for an average, at a 10ms loop:

sudo ./fourth.test.mpu9265 0x68 10 10

 61:255   0 255   0 255   0 255   0   0   0 :  102
 62:204 112 140 164 148 156 188 248  88 228 :  167
 63:189 188 189 187 189 188 188 188 188 189 :  188
 64: 60  40  16  96 208 132 116 252 172  36 :  112
 65:  7   7   7   7   7   7   7   7   7   7 :    7
 66:224 224 224 240 160 208 224 208 144  96 :  195
 67:  0   0   0   0   0   0   0   0   0   0 :    0
 68:215 228 226 228 203 221 239 208 214 187 :  216
 69:  0 255   0 255 255   0 255   0   0   0 :  102
 70:242  43 253 239 239  45 206  28 247 207 :  174
 71:  0 255 255   0 255 255 255 255 255 255 :  204
 72: 51 199  19 214  11 223  21 236 193   8 :  117
 73:  0   0   0   0   0   0   0   0   0   0 :    0
111: 46 149  91 199 215  46 142   2 233 199 :  132
112:  0   0   0   0   0   0   0   0   0   0 :    0
189:255   0 255   0 255   0   0 255   0 255 :  127
190: 76  36 240  36 100   0 164 164 152 244 :  121
191:188 188 188 188 187 188 187 189 187 189 :  187
192:  8  48  48 196  96 220 144   0  76  40 :   87
193:  7   7   7   7   7   8   7   7   7   7 :    7
194:208 224 144 240 176 240 224 208 240 224 :  212
195:  0   0   0   0   0   0   0   0   0   0 :    0
196:243 184 233 200 225 192 189 242 188 203 :  209
197:255   0   0   0 255   0 255   0   0 255 :  102
198:223  39 247  43 245  22 255 221   0   6 :  130
199:  0 255 255 255   0 255 255 255 255   0 :  178
200:231 225 251   1 252  20 211 216 218  16 :  164
201:  0   0   0   0   0   0   0   0   0   0 :    0
239: 21 138 196  87  26  89  16 245 187 144 :  114
240:  0   0   0   0   0   0   0   0   0   0 :    0

The first, left-most column is the register number. Then come the last 10 readings for that register. Finally, the last column is the average for each row.

It looks like registers 61, 69, 71, 189, 197, and 199 are either only binary, or ready / not ready, or they are the high byte of a 16-bit value (negative?).

Other interesting observations:

  • registers 65, 193 - very steady and same value
  • register 63, 191 - very steady and same value
  • registers 73, 112, 195, 201, 240 - all at zero

Let's relate these observations back to the multi-colored, highlighted table image from earlier.

Register 65 - temperature

Register 193 - ??????

Register 63 - accelerometer

Register 191 - ??????

Register 73 - external

Register 112 and on - ??????

Well, we still have unknowns , however, we have learned something useful.

Register 65 (temperature) and register 63 (accelerometer) were both very steady. This is something we would expect. I haven't touched the sensor; it's not moving, other than any incidental vibrations, as the robot is resting on the same table as my computer.

There's one interesting test we can for each of these temperature/accelerometer registers. For that test, we need yet another version of the program.

Step 7: We Are Able to Affect Temperature and Acceleration

In the previous steps we narrowed down at least one register for temperature, and one for acceleration.

With this next version of the program ("fifth.test.mpu9265.c"), we can actually see a change take place for both registers. Please watch the videos.

More Digging

If we go back and take a look at the register information, we see that there are:

  • three 16 bit outputs for gyroscope
  • three 16 bit outputs for accelerometer
  • three 16 bit outputs for magnetometer
  • one 16 bit output for temperature

However, the results obtained by our simple test programs were all single 8 bit outputs. (single registers).

So let's try more of the same approach, but this time reading 16 bits instead of 8.

We're probably going to have to do something like below. Let's use the temperature as an example, since it's only one 16 bit output.

//obtain file descriptor fd...

int tempRegHi = 65;
int tempRegLo = 66;

int hiByte = wiringPiI2CReadReg8(fd, tempRegHi);
int loByte = wiringPiI2CReadReg8(fd, tempRegLo);

int result = hiByte << 8;  // put the hi order 8 bits into the upper part of a 16 bit value

result |= loByte;  // now add in the lo order 8 bits, yielding a complete 16 bit number

// print that number or use the display horizontal graphing function from before

From our previous steps we've seen that register 65 is pretty rock steady, while register 66 is very noisy. Since 65 is the hi order byte, and 66 the low order byte, that makes sense.

For reading, we can take register 65's data as-is, but we could average out register 66's values.

Or we can just average the entire result.

Take a look at the last video for this part; it demonstrates reading the entire 16 bit temperature value. The code is "sixth.test.mpu9265.c"

Step 8: The Accelerometer and Gyroscope

The videos for this section show output from the accelerometer and the gyroscope, using a test program "seventh.test.mpu9265.c". That code can reads 1, 2, or 3 consecutive byte-pairs (hi and lo bytes) and converts the values to a single 16 bit value. Thus, we can read any single axis, or we can read two of them together (and it sums the changes), or we can read all three (and it sums the changes).

To reiterate, for this phase, for this Instructable, I'm just looking to answer a simple question: "did the robot rotate/pivot?". I'm not looking for any precise value, such as, did it rotate 90 degrees. That will come later when we get to doing SLAM, but it's not required for simple obstacle-avoidance and random movement.

Step 9: (work in Progress) the Magnetometer

when using the i2cdetect tool, the MPU9265 shows up as 0x68 in the table:

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 
70: -- -- -- -- -- -- -- -- 

There's extra steps required to read from the magnetometer portion of the IMU.

From the Invesense registers PDF doc: