Turning to a Heading with an MPU-9150

As the odour of fried electronics dissipates in the air, I’m unexpectedly afforded the opportunity to write this blog post a day or two earlier than planned. The on-board compass was exhibiting significant deviation, so I consulted my nephew Thorbjørn Christensen at DTU-Space. Thorbjørn makes a living designing magnetometers for space agencies around the world, and he suggested I use a ribbon cable to move the IMU away from the bot as a first step towards understanding the issue. He did warn me to be very careful when attaching it. I still don’t understand what I did wrong, but the evidence that I screwed up is pretty definitive: there are burn marks around every single connector and the IMU chip looks a bit soft in the middle. Most importantly, it no longer reports for duty on the Raspberry Pi’s I2C bus!

A fried IMU-9150

A fried IMU-9150 (click to enlarge)

The good news (apart from the fact that the Raspberry Pi, the Arduino and all other ‘bot components seem to have survived) is perhaps that, since I cannot play with the compass until I have a replacement, I found the time to write about where I am at the moment…and take some time out to work on my presentations for Dyalog ’14 (for those who managed to register before the user meeting in Eastbourne sold out, I still hope to do a ‘bot presentation on the evening of Tuesday September 23rd).

Introducing the MPU-9150

For the last few weeks, I have been using my “bot time” to play with the MPU-9150 breakout board that is attached to our ‘bot. The MPU-9150 is more or less identical to the 6050 that we were using earlier this summer to make gyro-controlled turns but also includes a magnetic compass, which will allow us to know the direction that we are heading in – making it a 9 degree of freedom inertial measurement unit, or “9-dof IMU” (9 = 3 gyro axes + 3 accelerometer axes + 3 compass axes).

RTIMULib

I was extremely happy to discover that Richard Barnett had shared his RTIMULib on GitHub. RTIMULib is a Linux library that produces “Kalman-filtered quaternion or Euler angle pose data” based on data from a 9-dof IMU. Jay Foad at Dyalog was quickly able to provide me with a couple of additional C functions designed to be easy to call from Dyalog APL. Jay’s fork of RTIMULib is also available on GitHub. Since we can assume that we are driving on a flat surface, I have just been using two items from the wealth of information provided by this library: the current rate of rotation and the current compass direction – in both cases around the vertical (or “z”) axis.

My First Compass-Controlled Rotation

Due to the fried IMU, I am unable to post a video recording; fortunately, at the moment where I toasted the chip, the Dyalog session output still contained output data from the last run of the function I am working on, which aims to provide smooth rotation to a selected compass heading. In the chart below, the red line tracks the speed of rotation and shows that the “smooth” part still needs work – the speed oscillates quite violently between 60 and 150 degrees per second. The blue line shows the number of degrees that the ‘bot still needs to turn to reach the desired heading. The dotted red line is slightly mislabeled: rather than acceleration, it actually shows the power applied to the motors via a digital-to-analog converter that accepts values between 0 and 255, producing between 0 and 5v of voltage for the DC motors:

Rotating to a compass heading

Commentary:

  • From 0 to 0.4 seconds, we slowly increase power until we detect that rotation has started. We note the power setting required to start rotation.
  • 0.4 to 0.7 (ish), we detect acceleration and hold the throttle steady (increasing it very slightly a couple of times when acceleration appears to stop).
  • 0.7 to 1.1 seconds, cruise mode: whenever speed is above the target of 100 degrees/sec, we idle. When speed is below 100, we re-apply power at a level that was sufficient to start the rotation.
  • 1.1 to 1.4 seconds: We are less than 30 degrees from target, and the target velocity and thus power is reduced proportional to the remaining distance (at 15 degrees, we are down to 50 deg/sec)
  • 1.4 to 1.6 seconds: We detect that we rotated too far, and slam on the brakes, coming to a stop about 10 degrees from target.
  • 1.6 to 2.3 seconds: since we are less than 10 degrees from target, we enter “step” mode, giving very brief bursts of power at “start rotation” level, idling, and watching the heading until we reach the goal.
  • 2.4 seconds: We count the bursts and, after 10, we double the power setting to avoid getting bogged down (you cannot see the bursts in the chart, it only records the power level used for each one). A little more patience would have been good, this probably means we overshot by a bit.

The “Heading” Function

Achieving anything resembling smooth movement is still some way away, mostly due to the fact that motor response to a given input voltage varies enormously from motor to motor and between different applications to the same motor. The strategy described above is implemented by a function named “Heading”, which can be found in the file mpu9150.dyalog in the APLBot folder on GitHub. An obvious improvement would be to have the robot self-calibrate itself somehow and add a model of how fast rotation speeds up and slows down (based on, and constantly adjusted with, observed data), in order to dampen the oscillations. I intend to return to this when I have a new IMU (and my other user meeting presentations are under control).

This has turned out to be a lot harder than I imagined: suggestions are very welcome – please leave comments if you have a good idea!

Isolated Mandelbrot Set Explorer

mandelbrot-equation

It amazes me that an equation so simple can produce images so beautiful and complex.  I was first introduced to the Mandelbrot set by the August 1985 issue of Scientific American. Wikipedia says “The Mandelbrot set is the set of complex numbers ‘c’ for which the sequence ( c , c² + c , (c²+c)² + c , ((c²+c)²+c)² + c , (((c²+c)²+c)²+c)² + c , …) does not approach infinity.” This means that in the images below, the areas in black are points in the Mandelbrot set and the other point colors are determined by how quickly that point “zooms” off towards infinity. The links above give a deeper discussion on the Mandelbrot set – for this post I’d like to focus on how I was able to speed my Mandelbrot set explorer up using isolates.

Mandelbrot Set
I find I learn things more easily through application than abstraction. In 2006 I wanted to learn more about the built-in GUI features of Dyalog for Windows and the application I chose was a Mandelbrot set explorer. While the basic algorithm for calculating a point in the Mandelbrot set is fairly simple, a 640×480 pixel image could involve over 75 million calculations. As new versions of Dyalog are released, I try to experiment with any applicable new features and possibly incorporate them into the explorer. For instance, when complex numbers were introduced in Dyalog, I modified the explorer to use them.

Dyalog v14.0 introduced a prototype of a parallel computing feature called isolates that may one day be integrated into the interpreter. Isolates behave much like namespaces except that they run in their own process with their own copy of the interpreter. Each process can run on a separate processor, greatly improving performance. You can even set up isolate servers on other machines.

Mandelbrot set calculations are ideal candidates for parallelization as:

  • they’re CPU intensive
  • each pixel can be calculated independently of others
  • there are no side effects and no state to maintain

The core calculation is shown below. It takes the real and imaginary parameters for the top left corner, the height and width of the image and the increment along each dimension and uses those parameters to build a set of co-ordinates, then iterates to see how quickly each co-ordinate “escapes” the Mandelbrot set.

 ∇ r←real buildset_core imaginary;height;i_incr;top;width;r_incr;left;set;coord;inds;cnt;escaped
  ⍝ Calculate new Mandelbrot set - remember using origin 0 (⎕IO←0)
  ⍝ real and imaginary are the parameters of the space to calculate
  ⍝       real            imaginary
  ⍝ [0]   width           height            of the image
  ⍝ [1]   increment size  increment size    along each axis
  ⍝ [2]   top             left              coordinates

   (height i_incr top)←imaginary            ⍝ split imaginary argument parameters
   (width r_incr left)←real                 ⍝ split real argument parameters
   set←coord←,(0J1×top+i_incr×⍳height)∘.+(left+r_incr×⍳width) ⍝ create all points in the image
   inds←⍳≢r←(≢coord)⍴0                      ⍝ initialize the result and create vector of indices
   :For cnt :In 1↓⍳256                      ⍝ loop up to 255 times (the size of our color palette)
       escaped←4<coord×+coord               ⍝ mark those that have escaped the Mandelbrot set
       r[escaped/inds]←cnt                  ⍝ set their index in the color palette
       (inds coord)←(~escaped)∘/¨inds coord ⍝ keep those that have not yet escaped
       :If 0∊⍴inds ⋄ :Leave ⋄ :EndIf        ⍝ anything left to do?
       coord←set[inds]+×⍨coord              ⍝ the core Mandelbrot computation... z←(z*2)+c
   :EndFor
   r←(height,width)⍴r                       ⍝ reshape to image size
 ∇

So, what’s involved in using isolates to parallelize the explorer?  As it turns out, not much! First I )COPYed the isolate namespace from the isolate workspace distributed with Dyalog v14.0. I wanted to be able to select the number of processors to use, so I added a line to my init function to query how many processors are available:

 processors←1111⌶⍬ ⍝ query number of processors

Next I wrote a small function to initialize isolates for as many processors as were selected (2 or more). Notice that when creating the isolates, I copy buildset_core (from above) into each one:

 ∇ init_isolates processors
  ⍝ called upon initialization, or whenever user changes number of processors to use
   f.isomsg.Visible←1    ⍝ turn form's "Isolates initializing" message on
   {}isolate.Reset''     ⍝ reset the isolates
   {}isolate.Config'processors'processors ⍝ set the number of processors
   :If processors>1      ⍝ if using more than one processor
       ⍝ create the isolates, populating each one with buildset_core
       isolates←isolate.New¨processors⍴⎕NS'buildset_core'
   :EndIf
   f.isomsg.Visible←0    ⍝ turn the form message off
 ∇

Finally, I wrote a routine to share the work between the selected number of processors and assemble the results back together.

 ∇ r←size buildset rect;top;bottom;left;right;width;height;vert;horz;rows;incr;imaginary;real
  ⍝ Build new Mandelbrot Set using isolates if specified
  ⍝ rect is the top, bottom, left, and right
  ⍝ size is the height and width of the image
   (top bottom left right)←rect
   (height width)←size
   vert←bottom-top                             ⍝ vertical dimension
   horz←right-left                             ⍝ horizontal dimension
   rows←processors{¯2-/⌈⍵,⍨(⍳⍺)×⍵÷⍺}height     ⍝ divide image among processors (height=+/rows)
   incr←vert÷height                            ⍝ vertical increment
   imaginary←↓rows,incr,⍪top+incrׯ1↓0,+\rows  ⍝ imaginary arguments to buildset_core
   real←width,(horz÷width),left                ⍝ real arguments to buildset_core
   :If processors=1                            ⍝ if not using isolates
       r←real buildset_core⊃imaginary          ⍝ build the image here
   :Else
       r←⊃⍪/(⊂real)isolates.buildset_core imaginary ⍝ otherwise let the isolates to it
   :EndIf
 ∇

The images below depicts what happens with 4 processors. Each processor builds a slice of the image and the main task assembles them together.
Drawing1

So, how much faster is it?  Did my 8 processor Intel Core i7 speed things up by a factor of 8? No, but depending on where in the Mandelbrot set you explore, the speed is generally increased by at least a factor of 2 and sometimes as high as 5 or 6. I thought about why this might be and have a couple of ideas…

  • There’s overhead to receive and assemble the results from each isolate.
  • The overall time will be the maximum of the individual times; if one slice is in an area that requires more iterations, then the main task needs to wait for those results, even if the other slices have already finished. I could possibly show this by updating the individual slices as they become available. Maybe next time :)

What impressed me was that I could speed up my application significantly with about 20 lines of code and maybe an hour’s effort – most of which was spent tinkering to learn isolates. You can download the Mandelbrot set explorer from my GitHub repository.

Making Controlled Turns with the DyaBot

This blog originally started when I took delivery of the DyaBot, a Raspberry Pi and Arduino based variant of the C3Pi running Dyalog v13.2. The architecture of the ‘bot and instructions for building your own inexpensive robot can all be found in blog entries from April to July of last year.

The downside of only using inexpensive components is that some of them are not very precise. The worst problem we face is that the amount of wheel movement generated by the application of a particular power level varies from one motor to the next, and indeed from moment to moment. Trying to drive a specific distance in a straight line, or make an exact 90 degree turn regardless of the surface that the ‘bot is standing on, are impossible tasks with the original Dyabot. You can have a lot of fun, as I hope the early posts demonstrate, but we have higher ambitions!

Our next task is to add motion sensors to the DyaBot, with a goal of being able to measure actual motion with sufficient accuracy to maintain our heading while driving straight ahead – and to make exact turns to new headings, like the 90-degree turn made in this video (the ‘bot has been placed on a Persian carpet to provide a background containing right angles):

Introducing the MPU-6050

For some time we have had an MPU-6050, which has 3-axis rotation and acceleration sensors, attached to our I2C bus, but haven’t been able to use it. About ten days ago (sorry, I took a few days off last week), @RomillyC came to visit us in Bramley to help me read some Python code that was written for this device. The current acceleration or rotation on each axis is constantly available as a register and can be queried via I2C. Translated into APL, the code to read the current rate of rotation around the vertical axis is:

 ∇ r←z_gyro_reading ⍝ current z-axis rotation in degrees
   r←(read_word_2c 70)÷131 ⍝ Read register #70 and scale to deg/sec
 ∇

 ∇ r←read_word_2c n;zz
   r←I2 256⊥read_byte_2c¨n+0 1               ⍝ Read 2 registers and make signed I2
 ∇

 ∇ r←read_byte_2c n;z
   z←#.I2C.WriteArray i2c_address (1,n)0     ⍝ Write register address
   r←2 2⊃#.I2C.ReadArray i2c_address (1 0)1  ⍝ Read input
 ∇

 I2←{⍵>32767:1-65535-⍵ ⋄ ⍵} ⍝ 16-bit 2's complement to signed

Integrating to Track Attitude

Reading the register directly gives us the instantaneous rate of rotation. If we want to track the attitude, we need to integrate the rates over time. The next layer of functions allows us to reset the attitude and perform very primitive integration by multiplying the current rotation with the elapsed time since the last measurement:

∇ r←z_gyro_reset            ⍝ Reset Gyro Integration
  z_gyro_time←now           ⍝ current time in ms
  z_gyro_posn←0             ⍝ current rotation is 0
∇

∇ r←z_gyro;t;delta          ⍝ Integrated gyro rotation
  t←now                     ⍝ current time in ms
  delta←0.001×t-z_gyro_time ⍝ elapsed time in seconds
  r←z_gyro_posn←z_gyro_posn+delta×z_gyro_reading+z_gyro_offset
  z_gyro_time←t             ⍝ last time
∇

We can now write a function to rotate through a given number of degrees (assuming that the gyro has just been reset): we set the wheels in motion, monitor the integrated angle until we are nearly there, then shut off the engines. We continue logging the angles for a brief moment to monitor the deceleration. The function returns a two-column matrix containing the “log” of rotation angles and timestamps:

    ∇ log←rotate angle;bot;i;t
     
      log←0 2⍴0 ⍝ Angle, time
      bot.Speed←3 ¯3 ⍝ Slow counter-clockwise rotation
     
      :Trap 1000 ⍝ Catch interrupts
          :Repeat
              log⍪←(t←z_gyro),now
          :Until t>angle-7 ⍝ Stop rotating 7 degrees before we are done    
      :EndTrap

      bot.Speed←0 ⍝ cut power to the motors
      :For i :In ⍳25 ⍝ Capture data as we slow down
          log⍪←z_gyro,now
      :EndFor
    ∇

The logged data from the rotation captured in the video at the top is charted below:

Controlled 90-Degree RotationEven with the primitive integration and rotation strategies, the results already look quite promising. I’ll be taking most of this week off – part of it without access to the internet(!), but once I am back, expect the next blog entry to explore writing functions that accelerate and slow down in a more controlled fashion as well as stop at the right spot rather than relying on a specific amount of friction to rotate through the last 7 degrees (note the very slight reverse rotation at the end, probably caused by the Persian carpet being a bit “springy”). I will also clean up the code and post the complete solution on GitHub – and perhaps even look at some better integration techniques.

If you would like to make sure you don’t miss the next installment, follow the RSS feed or Dyalog on Twitter.