Using the L3G4200D gyroscope with the Arduino

 The last piece of the IMU puzzle is the gyroscope (previous adventures looked at the accelerometer and the magnetometer). Once I have drivers for this, I can start to look into combination of the three sensors to provide an estimate of heading. Then coupling the range sensors to the odometer readings I can build a dead reckoning system that keeps track (as best as possible) of the robot location in the environment.

Gyroscopes measure an angular rate of motion about three axes. Typically, these angles are referred to as roll, pitch, yaw. When mounted in the same orientation as the magnetometer, they provide and additional bit of information at a higher rate. Gyrscopes are a relative measurement sensor and do not provide an absolute angle. Therefore the gyroscope is perfect for fusion with the magnetometer in the IMU. Several different MEMS gyroscope technologies exist, for a an overview, see the links below.

I went with the L3G4200D from SparkFun. This was a little on the pricey size compared to some of the other components, but it is an essential puzzle piece. Here are the parts lists for this experiment:

1.  L3G4200D: http://www.sparkfun.com/products/10612
2. Arduino UNO R3: http://www.sparkfun.com/products/11021
3. Jumper wires: http://www.sparkfun.com/products/9387

 On the software side, you will need to install a few things (see previous adventures):

1. ArdAdv code: https://github.com/mark-r-stevens/Ardadv
2. ROS serial: http://ros.org/wiki/rosserial/
3. Various macports: http://www.macports.org/

In figuring out how to use the chip, I found the following sites to be very useful:

1. http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Gyros/3-Axis/17116.pdf
2. http://bildr.org/2011/06/l3g4200d-arduino/
3. http://en.wikipedia.org/wiki/Gyroscope
4. http://www.sensorsmag.com/sensors/acceleration-vibration/an-overview-mems-inertial-sensing-technology-970
5. http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00265057.pdf
6. https://github.com/pololu/L3G4200D
7. http://forums.trossenrobotics.com/showthread.php?5431-L3G4200D-Gyro-Integration-on-Arduino

I wired up the L3G4200D to use the SPI interface. Not sure this is the best as it also supports an I2C interface (meaning less pins, plus the magnetometer also uses SPI). However, it is a place to start. Here is a wiring diagram of what I setup:

The first thing I did was turn on the gyroscope and let it sit stationary on the table. This estimates the zero settings. Here is an example of the output.

The mean of response is <52.5054, -17.3098, -12.8859>. This is used to correct the up front bias in the voltage settings: simply subtract this value as it is read to remove the bias. I figure eventually, I will add a calibration step that can be used to estimate the gain/offset parameters to get better calibration values. The next step was to make some cumulative angle measurements. I printed out a protractor image and then turned the gyroscope back and forth between 0° and 180°.

Then I integrated the z rotation angle to compute the absolute angle. The idea is that the gyroscope produces a delta angle (call it θ): 

θ = ∫₀^t θ^t dt

using this formulation, I generated the following plot:

Here is the test code used to call the gyroscope class:

#include "gyroscope.h" 
ardadv::sensors::gyroscope::Gyroscope gyroscope; 


void setup() 
{ 
  Serial.begin(9600);     
  Serial.flush(); 
  typedef ardadv::sensors::gyroscope::Gyroscope Gyroscope;
  gyroscope.setup(Gyroscope::INTA(7), Gyroscope::INTB(6), Gyroscope::CS(10)); 
} 
void loop()
{ 
  gyroscope.update();
  const unsigned long t = millis();
  ::Serial.print(t); 
  ::Serial.print(","); 
  ::Serial.print(gyroscope.x(), DEC); 
  ::Serial.print(","); 
  ::Serial.print(gyroscope.y(), DEC); 
  ::Serial.print(",");  
  ::Serial.println(gyroscope.z(), DEC); 
  ::Serial.flush(); 
  ::delay(100); 
}

This uses the code locate at the git hub site: https://github.com/mark-r-stevens/Ardadv/tree/master/device/sensors/gyroscope. The last test I did was to see how stable the gyroscope is and how much drift to expect over time. I hooked up a simple servo and had the servo move back and forth between 0 and 180. Here is a movie of it running.

I then repeated the process and performed the integration. This gave the following plot:

Really hard to say at this point if the difference is due to the error in how precise the servo can achieve the requested angle (the servo is fairly cheap) or due to the accumulated drift. However, the plot definitely looks like drift. Here is the matlab code I used to make this plot:

A = csvread('Capture.txt'); 
a = A(:,2); 
z = -A(:,3); 
t = 1:length(a); 
dt = conv(A(:,1),[1,-1], 'valid'); 
figure(1), plot(t, a, 'r-', t, cumsum(z/mean(dt)), 'g-'); 
legend('servo', 'gyro'); 
ylabel('angle'); 
xlabel('time'); 

Next I will try to either make measurements with the gyro on the robot or fuse in the magnetometer or both.....