Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Accelerometer.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Accelerometer.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Accelerometer.cpp	(revision 232)
@@ -0,0 +1,87 @@
+#include <Wire.h>
+#include <LSM303.h>
+#include <RunningAverage.h>
+#include <Accelerometer.h>
+
+// class Accelerometer -- member function definitions
+
+// enable accelerometer only
+// to enable both accelerometer and magnetometer, call enableDefault() instead
+void Accelerometer::enable(void)
+{
+  // Enable Accelerometer
+  // 0x27 = 0b00100111
+  // Normal power mode, all axes enabled
+  writeAccReg(LSM303::CTRL_REG1_A, 0x27);
+
+  if (getDeviceType() == LSM303::device_DLHC)
+  writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
+}
+
+void Accelerometer::getLogHeader(void)
+{
+  Serial.print("millis    x      y     len     dir  | len_avg  dir_avg  |  avg_len");
+  Serial.println();
+}
+
+void Accelerometer::readAcceleration(unsigned long timestamp)
+{
+  readAcc();
+  if (a.x == last.x && a.y == last.y) return;
+  
+  last.timestamp = timestamp;
+  last.x = a.x;
+  last.y = a.y;
+  
+  ra_x.addValue(last.x);
+  ra_y.addValue(last.y);
+ 
+#ifdef LOG_SERIAL
+ Serial.print(last.timestamp);
+ Serial.print("  ");
+ Serial.print(last.x);
+ Serial.print("  ");
+ Serial.print(last.y);
+ Serial.print("  ");
+ Serial.print(len_xy());
+ Serial.print("  ");
+ Serial.print(dir_xy());
+ Serial.print("  |  ");
+ Serial.print(sqrt(static_cast<float>(ss_xy_avg())));
+ Serial.print("  ");
+ Serial.print(dir_xy_avg());
+ Serial.println();
+#endif
+}
+
+float Accelerometer::len_xy() const
+{
+  return sqrt(last.x*a.x + last.y*a.y);
+}
+
+float Accelerometer::dir_xy() const
+{
+  return atan2(last.x, last.y) * 180.0 / M_PI;
+}
+
+int Accelerometer::x_avg(void) const
+{
+  return ra_x.getAverage();
+}
+
+int Accelerometer::y_avg(void) const
+{
+  return ra_y.getAverage();
+}
+
+long Accelerometer::ss_xy_avg(void) const
+{
+  long x_avg_long = static_cast<long>(x_avg());
+  long y_avg_long = static_cast<long>(y_avg()); 
+  return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
+}
+
+float Accelerometer::dir_xy_avg(void) const
+{
+  return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Accelerometer.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Accelerometer.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Accelerometer.h	(revision 232)
@@ -0,0 +1,37 @@
+// Accelerometer Settings
+#define RA_SIZE 3  // number of readings to include in running average of accelerometer readings
+
+// Accelerometer Class -- extends the LSM303 Library to support reading and averaging the x-y acceleration 
+//   vectors from the onboard LSM303DLHC accelerometer/magnetometer
+class Accelerometer : public LSM303
+{
+  typedef struct acc_data_xy
+  {
+    unsigned long timestamp;
+    int x;
+    int y;
+    float dir;
+  } acc_data_xy;
+  
+  public: 
+	Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
+    ~Accelerometer() {};
+    void begin(void) {
+		Wire.begin();
+		init();
+		enable();
+	};
+    void enable(void);
+    void getLogHeader(void);
+    void readAcceleration(unsigned long timestamp);
+    float len_xy() const;
+    float dir_xy() const;
+    int x_avg(void) const;
+    int y_avg(void) const;
+    long ss_xy_avg(void) const;
+    float dir_xy_avg(void) const;
+  private:
+    acc_data_xy last;
+    RunningAverage<int> ra_x;
+    RunningAverage<int> ra_y;   
+};
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/L3G.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/L3G.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/L3G.cpp	(revision 232)
@@ -0,0 +1,238 @@
+#include <L3G.h>
+#include <Wire.h>
+#include <math.h>
+
+// Defines ////////////////////////////////////////////////////////////////
+
+// The Arduino two-wire interface uses a 7-bit number for the address,
+// and sets the last bit correctly based on reads and writes
+#define D20_SA0_HIGH_ADDRESS      0b1101011 // also applies to D20H
+#define D20_SA0_LOW_ADDRESS       0b1101010 // also applies to D20H
+#define L3G4200D_SA0_HIGH_ADDRESS 0b1101001
+#define L3G4200D_SA0_LOW_ADDRESS  0b1101000
+
+#define TEST_REG_ERROR -1
+
+#define D20H_WHO_ID     0xD7
+#define D20_WHO_ID      0xD4
+#define L3G4200D_WHO_ID 0xD3
+
+// Constructors ////////////////////////////////////////////////////////////////
+
+L3G::L3G(void)
+{
+  _device = device_auto;
+
+  io_timeout = 0;  // 0 = no timeout
+  did_timeout = false;
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+// Did a timeout occur in read() since the last call to timeoutOccurred()?
+bool L3G::timeoutOccurred()
+{
+  bool tmp = did_timeout;
+  did_timeout = false;
+  return tmp;
+}
+
+void L3G::setTimeout(unsigned int timeout)
+{
+  io_timeout = timeout;
+}
+
+unsigned int L3G::getTimeout()
+{
+  return io_timeout;
+}
+
+bool L3G::init(deviceType device, sa0State sa0)
+{
+  int id;
+  
+  // perform auto-detection unless device type and SA0 state were both specified
+  if (device == device_auto || sa0 == sa0_auto)
+  {
+    // check for L3GD20H, D20 if device is unidentified or was specified to be one of these types
+    if (device == device_auto || device == device_D20H || device == device_D20)
+    {
+      // check SA0 high address unless SA0 was specified to be low
+      if (sa0 != sa0_low && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR)
+      {
+        // device responds to address 1101011; it's a D20H or D20 with SA0 high     
+        sa0 = sa0_high;
+        if (device == device_auto)
+        {
+          // use ID from WHO_AM_I register to determine device type
+          device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
+        }
+      }
+      // check SA0 low address unless SA0 was specified to be high
+      else if (sa0 != sa0_high && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR)
+      {
+        // device responds to address 1101010; it's a D20H or D20 with SA0 low      
+        sa0 = sa0_low;
+        if (device == device_auto)
+        {
+          // use ID from WHO_AM_I register to determine device type
+          device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
+        }
+      }
+    }
+    
+    // check for L3G4200D if device is still unidentified or was specified to be this type
+    if (device == device_auto || device == device_4200D)
+    {
+      if (sa0 != sa0_low && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID)
+      {
+        // device responds to address 1101001; it's a 4200D with SA0 high
+        device = device_4200D;
+        sa0 = sa0_high;
+      }
+      else if (sa0 != sa0_high && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID)
+      {
+        // device responds to address 1101000; it's a 4200D with SA0 low
+        device = device_4200D;
+        sa0 = sa0_low;
+      }
+    }
+    
+    // make sure device and SA0 were successfully detected; otherwise, indicate failure
+    if (device == device_auto || sa0 == sa0_auto)
+    {
+      return false;
+    }
+  }
+  
+  _device = device;
+
+  // set device address
+  switch (device)
+  {
+    case device_D20H:
+    case device_D20:
+      address = (sa0 == sa0_high) ? D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS;
+      break;
+
+    case device_4200D:
+      address = (sa0 == sa0_high) ? L3G4200D_SA0_HIGH_ADDRESS : L3G4200D_SA0_LOW_ADDRESS;
+      break;
+  }
+  
+  return true;
+}
+
+/*
+Enables the L3G's gyro. Also:
+- Sets gyro full scale (gain) to default power-on value of +/- 250 dps
+  (specified as +/- 245 dps for L3GD20H).
+- Selects 200 Hz ODR (output data rate). (Exact rate is specified as 189.4 Hz
+  for L3GD20H and 190 Hz for L3GD20.)
+Note that this function will also reset other settings controlled by
+the registers it writes to.
+*/
+void L3G::enableDefault(void)
+{
+  if (_device == device_D20H)
+  {
+    // 0x00 = 0b00000000
+    // Low_ODR = 0 (low speed ODR disabled)
+    writeReg(LOW_ODR, 0x00);
+  }
+  
+  // 0x00 = 0b00000000
+  // FS = 00 (+/- 250 dps full scale)
+  writeReg(CTRL_REG4, 0x00);
+  
+  // 0x6F = 0b01101111
+  // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
+  writeReg(CTRL_REG1, 0x6F);
+}
+
+// Writes a gyro register
+void L3G::writeReg(byte reg, byte value)
+{
+  Wire.beginTransmission(address);
+  Wire.write(reg);
+  Wire.write(value);
+  last_status = Wire.endTransmission();
+}
+
+// Reads a gyro register
+byte L3G::readReg(byte reg)
+{
+  byte value;
+
+  Wire.beginTransmission(address);
+  Wire.write(reg);
+  last_status = Wire.endTransmission();
+  Wire.requestFrom(address, (byte)1);
+  value = Wire.read();
+  Wire.endTransmission();
+
+  return value;
+}
+
+// Reads the 3 gyro channels and stores them in vector g
+void L3G::read()
+{
+  Wire.beginTransmission(address);
+  // assert the MSB of the address to get the gyro
+  // to do slave-transmit subaddress updating.
+  Wire.write(OUT_X_L | (1 << 7));
+  Wire.endTransmission();
+  Wire.requestFrom(address, (byte)6);
+  
+  unsigned int millis_start = millis();
+  while (Wire.available() < 6)
+  {
+    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
+    {
+      did_timeout = true;
+      return;
+    }
+  }
+
+  uint8_t xlg = Wire.read();
+  uint8_t xhg = Wire.read();
+  uint8_t ylg = Wire.read();
+  uint8_t yhg = Wire.read();
+  uint8_t zlg = Wire.read();
+  uint8_t zhg = Wire.read();
+
+  // combine high and low bytes
+  g.x = (int16_t)(xhg << 8 | xlg);
+  g.y = (int16_t)(yhg << 8 | ylg);
+  g.z = (int16_t)(zhg << 8 | zlg);
+}
+
+void L3G::vector_normalize(vector<float> *a)
+{
+  float mag = sqrt(vector_dot(a,a));
+  a->x /= mag;
+  a->y /= mag;
+  a->z /= mag;
+}
+
+// Private Methods //////////////////////////////////////////////////////////////
+
+int L3G::testReg(byte address, regAddr reg)
+{
+  Wire.beginTransmission(address);
+  Wire.write((byte)reg);
+  if (Wire.endTransmission() != 0)
+  {
+    return TEST_REG_ERROR;
+  }
+
+  Wire.requestFrom(address, (byte)1);
+  if (Wire.available())
+  {
+    return Wire.read();
+  }
+  else
+  {
+    return TEST_REG_ERROR;
+  }
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/L3G.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/L3G.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/L3G.h	(revision 232)
@@ -0,0 +1,121 @@
+#ifndef L3G_h
+#define L3G_h
+
+#include <Arduino.h> // for byte data type
+
+class L3G
+{
+  public:
+    template <typename T> struct vector
+    {
+      T x, y, z;
+    };
+
+    enum deviceType { device_4200D, device_D20, device_D20H, device_auto };
+    enum sa0State { sa0_low, sa0_high, sa0_auto };
+
+    // register addresses
+    enum regAddr
+    {
+       WHO_AM_I       = 0x0F,
+
+       CTRL1          = 0x20, // D20H
+       CTRL_REG1      = 0x20, // D20, 4200D
+       CTRL2          = 0x21, // D20H
+       CTRL_REG2      = 0x21, // D20, 4200D
+       CTRL3          = 0x22, // D20H
+       CTRL_REG3      = 0x22, // D20, 4200D
+       CTRL4          = 0x23, // D20H
+       CTRL_REG4      = 0x23, // D20, 4200D
+       CTRL5          = 0x24, // D20H
+       CTRL_REG5      = 0x24, // D20, 4200D
+       REFERENCE      = 0x25,
+       OUT_TEMP       = 0x26,
+       STATUS         = 0x27, // D20H
+       STATUS_REG     = 0x27, // D20, 4200D
+
+       OUT_X_L        = 0x28,
+       OUT_X_H        = 0x29,
+       OUT_Y_L        = 0x2A,
+       OUT_Y_H        = 0x2B,
+       OUT_Z_L        = 0x2C,
+       OUT_Z_H        = 0x2D,
+
+       FIFO_CTRL      = 0x2E, // D20H
+       FIFO_CTRL_REG  = 0x2E, // D20, 4200D
+       FIFO_SRC       = 0x2F, // D20H
+       FIFO_SRC_REG   = 0x2F, // D20, 4200D
+
+       IG_CFG         = 0x30, // D20H
+       INT1_CFG       = 0x30, // D20, 4200D
+       IG_SRC         = 0x31, // D20H
+       INT1_SRC       = 0x31, // D20, 4200D
+       IG_THS_XH      = 0x32, // D20H
+       INT1_THS_XH    = 0x32, // D20, 4200D
+       IG_THS_XL      = 0x33, // D20H
+       INT1_THS_XL    = 0x33, // D20, 4200D
+       IG_THS_YH      = 0x34, // D20H
+       INT1_THS_YH    = 0x34, // D20, 4200D
+       IG_THS_YL      = 0x35, // D20H
+       INT1_THS_YL    = 0x35, // D20, 4200D
+       IG_THS_ZH      = 0x36, // D20H
+       INT1_THS_ZH    = 0x36, // D20, 4200D
+       IG_THS_ZL      = 0x37, // D20H
+       INT1_THS_ZL    = 0x37, // D20, 4200D
+       IG_DURATION    = 0x38, // D20H
+       INT1_DURATION  = 0x38, // D20, 4200D
+
+       LOW_ODR        = 0x39  // D20H
+    };
+
+    vector<int16_t> g; // gyro angular velocity readings
+
+    byte last_status; // status of last I2C transmission
+
+    L3G(void);
+
+    bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
+    deviceType getDeviceType(void) { return _device; }
+
+    void enableDefault(void);
+
+    void writeReg(byte reg, byte value);
+    byte readReg(byte reg);
+
+    void read(void);
+
+    void setTimeout(unsigned int timeout);
+    unsigned int getTimeout(void);
+    bool timeoutOccurred(void);
+
+    // vector functions
+    template <typename Ta, typename Tb, typename To> static void vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
+    template <typename Ta, typename Tb> static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
+    static void vector_normalize(vector<float> *a);
+
+  private:
+      deviceType _device; // chip type (D20H, D20, or 4200D)
+      byte address;
+
+      unsigned int io_timeout;
+      bool did_timeout;
+
+      int testReg(byte address, regAddr reg);
+};
+
+template <typename Ta, typename Tb, typename To> void L3G::vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
+{
+  out->x = (a->y * b->z) - (a->z * b->y);
+  out->y = (a->z * b->x) - (a->x * b->z);
+  out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+template <typename Ta, typename Tb> float L3G::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
+{
+  return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
+
+
+
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LICENSE.txt
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LICENSE.txt	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LICENSE.txt	(revision 232)
@@ -0,0 +1,25 @@
+Copyright (c) 2013 Pololu Corporation.  For more information, see
+
+http://www.pololu.com/
+http://forum.pololu.com/
+
+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.
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.cpp	(revision 232)
@@ -0,0 +1,492 @@
+#include <LSM303.h>
+#include <Wire.h>
+#include <math.h>
+
+// Defines ////////////////////////////////////////////////////////////////
+
+// The Arduino two-wire interface uses a 7-bit number for the address,
+// and sets the last bit correctly based on reads and writes
+#define D_SA0_HIGH_ADDRESS                0b0011101
+#define D_SA0_LOW_ADDRESS                 0b0011110
+#define DLHC_DLM_DLH_MAG_ADDRESS          0b0011110
+#define DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS 0b0011001
+#define DLM_DLH_ACC_SA0_LOW_ADDRESS       0b0011000
+
+#define TEST_REG_ERROR -1
+
+#define D_WHO_ID    0x49
+#define DLM_WHO_ID  0x3C
+
+// Constructors ////////////////////////////////////////////////////////////////
+
+LSM303::LSM303(void)
+{
+  /*
+  These values lead to an assumed magnetometer bias of 0.
+  Use the Calibrate example program to determine appropriate values
+  for your particular unit. The Heading example demonstrates how to
+  adjust these values in your own sketch.
+  */
+  m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
+  m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
+
+  _device = device_auto;
+
+  io_timeout = 0;  // 0 = no timeout
+  did_timeout = false;
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+// Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()?
+bool LSM303::timeoutOccurred()
+{
+  bool tmp = did_timeout;
+  did_timeout = false;
+  return tmp;
+}
+
+void LSM303::setTimeout(unsigned int timeout)
+{
+  io_timeout = timeout;
+}
+
+unsigned int LSM303::getTimeout()
+{
+  return io_timeout;
+}
+
+bool LSM303::init(deviceType device, sa0State sa0)
+{
+  // perform auto-detection unless device type and SA0 state were both specified
+  if (device == device_auto || sa0 == sa0_auto)
+  {
+    // check for LSM303D if device is unidentified or was specified to be this type
+    if (device == device_auto || device == device_D)
+    {
+      // check SA0 high address unless SA0 was specified to be low
+      if (sa0 != sa0_low && testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
+      {
+        // device responds to address 0011101 with D ID; it's a D with SA0 high
+        device = device_D;
+        sa0 = sa0_high;
+      }
+      // check SA0 low address unless SA0 was specified to be high
+      else if (sa0 != sa0_high && testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
+      {
+        // device responds to address 0011110 with D ID; it's a D with SA0 low
+        device = device_D;
+        sa0 = sa0_low;
+      }
+    }
+    
+    // check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types
+    if (device == device_auto || device == device_DLHC || device == device_DLM || device == device_DLH)
+    {
+      // check SA0 high address unless SA0 was specified to be low
+      if (sa0 != sa0_low && testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
+      {
+        // device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high
+        sa0 = sa0_high;
+        if (device == device_auto)
+        { 
+          // use magnetometer WHO_AM_I register to determine device type
+          //
+          // DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this
+          // register isn't documented in its datasheet. Since the DLHC accelerometer address is the
+          // same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll
+          // guess that a device whose accelerometer responds to the SA0 high address and whose
+          // magnetometer gives the DLM ID is actually a DLHC.
+          device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLHC : device_DLH;
+        }
+      }
+      // check SA0 low address unless SA0 was specified to be high
+      else if (sa0 != sa0_high && testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
+      {
+        // device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low
+        sa0 = sa0_low;
+        if (device == device_auto)
+        {
+          // use magnetometer WHO_AM_I register to determine device type
+          device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLM : device_DLH;
+        }
+      }
+    }
+    
+    // make sure device and SA0 were successfully detected; otherwise, indicate failure
+    if (device == device_auto || sa0 == sa0_auto)
+    {
+      return false;
+    }
+  }
+  
+  _device = device;
+  
+  // set device addresses and translated register addresses
+  switch (device)
+  {
+    case device_D:
+      acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
+      translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
+      translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
+      translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
+      translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
+      translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
+      translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
+      break;
+
+    case device_DLHC:
+      acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high
+      mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+      translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
+      translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
+      translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
+      translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
+      translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
+      translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
+      break;
+
+    case device_DLM:
+      acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
+      mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+      translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
+      translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
+      translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
+      translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
+      translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
+      translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
+      break;
+
+    case device_DLH:
+      acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
+      mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+      translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
+      translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
+      translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
+      translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
+      translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
+      translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
+      break;
+  }
+  
+  return true;
+}
+
+/*
+Enables the LSM303's accelerometer and magnetometer. Also:
+- Sets sensor full scales (gain) to default power-on values, which are
+  +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
+  (+/- 4 gauss on LSM303D).
+- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
+  ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
+  settings for which the electrical characteristics are specified in
+  the datasheets.)
+- Enables high resolution modes (if available).
+Note that this function will also reset other settings controlled by
+the registers it writes to.
+*/
+void LSM303::enableDefault(void)
+{
+
+  if (_device == device_D)
+  {
+    // Accelerometer
+
+    // 0x00 = 0b00000000
+    // AFS = 0 (+/- 2 g full scale)
+    writeReg(CTRL2, 0x00);
+
+    // 0x57 = 0b01010111
+    // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
+    writeReg(CTRL1, 0x57);
+
+    // Magnetometer
+
+    // 0x64 = 0b01100100
+    // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
+    writeReg(CTRL5, 0x64);
+
+    // 0x20 = 0b00100000
+    // MFS = 01 (+/- 4 gauss full scale)
+    writeReg(CTRL6, 0x20);
+
+    // 0x00 = 0b00000000
+    // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
+    writeReg(CTRL7, 0x00);
+  }
+  else
+  {
+    // Accelerometer
+    
+    if (_device == device_DLHC)
+    {
+      // 0x08 = 0b00001000
+      // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
+      writeAccReg(CTRL_REG4_A, 0x08);
+
+      // 0x47 = 0b01000111
+      // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
+      writeAccReg(CTRL_REG1_A, 0x47);
+    }
+    else // DLM, DLH
+    {
+      // 0x00 = 0b00000000
+      // FS = 00 (+/- 2 g full scale)
+      writeAccReg(CTRL_REG4_A, 0x00);
+
+      // 0x27 = 0b00100111
+      // PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)
+      writeAccReg(CTRL_REG1_A, 0x27);
+    }
+
+    // Magnetometer
+
+    // 0x0C = 0b00001100
+    // DO = 011 (7.5 Hz ODR)
+    writeMagReg(CRA_REG_M, 0x0C);
+
+    // 0x20 = 0b00100000
+    // GN = 001 (+/- 1.3 gauss full scale)
+    writeMagReg(CRB_REG_M, 0x20);
+
+    // 0x00 = 0b00000000
+    // MD = 00 (continuous-conversion mode)
+    writeMagReg(MR_REG_M, 0x00);
+  }
+}
+
+// Writes an accelerometer register
+void LSM303::writeAccReg(byte reg, byte value)
+{
+  Wire.beginTransmission(acc_address);
+  Wire.write(reg);
+  Wire.write(value);
+  last_status = Wire.endTransmission();
+}
+
+// Reads an accelerometer register
+byte LSM303::readAccReg(byte reg)
+{
+  byte value;
+
+  Wire.beginTransmission(acc_address);
+  Wire.write(reg);
+  last_status = Wire.endTransmission();
+  Wire.requestFrom(acc_address, (byte)1);
+  value = Wire.read();
+  Wire.endTransmission();
+
+  return value;
+}
+
+// Writes a magnetometer register
+void LSM303::writeMagReg(byte reg, byte value)
+{
+  Wire.beginTransmission(mag_address);
+  Wire.write(reg);
+  Wire.write(value);
+  last_status = Wire.endTransmission();
+}
+
+// Reads a magnetometer register
+byte LSM303::readMagReg(int reg)
+{
+  byte value;
+
+  // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type)
+  if (reg < 0)
+  {
+    reg = translated_regs[-reg];
+  }
+
+  Wire.beginTransmission(mag_address);
+  Wire.write(reg);
+  last_status = Wire.endTransmission();
+  Wire.requestFrom(mag_address, (byte)1);
+  value = Wire.read();
+  Wire.endTransmission();
+
+  return value;
+}
+
+void LSM303::writeReg(byte reg, byte value)
+{
+  // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
+  if (_device == device_D || reg < CTRL_REG1_A)
+  {
+    writeMagReg(reg, value);
+  }
+  else
+  {
+    writeAccReg(reg, value);
+  }
+}
+
+// Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
+// To read those two registers, use readMagReg() instead.
+byte LSM303::readReg(int reg)
+{
+  // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
+  // Use readMagReg so it can translate OUT_[XYZ]_[HL]_M
+  if (_device == device_D || reg < CTRL_REG1_A)
+  {
+    return readMagReg(reg);
+  }
+  else
+  {
+    return readAccReg(reg);
+  }
+}
+
+// Reads the 3 accelerometer channels and stores them in vector a
+void LSM303::readAcc(void)
+{
+  Wire.beginTransmission(acc_address);
+  // assert the MSB of the address to get the accelerometer
+  // to do slave-transmit subaddress updating.
+  Wire.write(OUT_X_L_A | (1 << 7));
+  last_status = Wire.endTransmission();
+  Wire.requestFrom(acc_address, (byte)6);
+
+  unsigned int millis_start = millis();
+  while (Wire.available() < 6) {
+    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
+    {
+      did_timeout = true;
+      return;
+    }
+  }
+
+  byte xla = Wire.read();
+  byte xha = Wire.read();
+  byte yla = Wire.read();
+  byte yha = Wire.read();
+  byte zla = Wire.read();
+  byte zha = Wire.read();
+
+  // combine high and low bytes
+  // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0
+  // (12-bit resolution, left-aligned). The D has 16-bit resolution
+  a.x = (int16_t)(xha << 8 | xla);
+  a.y = (int16_t)(yha << 8 | yla);
+  a.z = (int16_t)(zha << 8 | zla);
+}
+
+// Reads the 3 magnetometer channels and stores them in vector m
+void LSM303::readMag(void)
+{
+  Wire.beginTransmission(mag_address);
+  // If LSM303D, assert MSB to enable subaddress updating
+  // OUT_X_L_M comes first on D, OUT_X_H_M on others
+  Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]);
+  last_status = Wire.endTransmission();
+  Wire.requestFrom(mag_address, (byte)6);
+
+  unsigned int millis_start = millis();
+  while (Wire.available() < 6) {
+    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
+    {
+      did_timeout = true;
+      return;
+    }
+  }
+
+  byte xlm, xhm, ylm, yhm, zlm, zhm;
+
+  if (_device == device_D)
+  {
+    // D: X_L, X_H, Y_L, Y_H, Z_L, Z_H
+    xlm = Wire.read();
+    xhm = Wire.read();
+    ylm = Wire.read();
+    yhm = Wire.read();
+    zlm = Wire.read();
+    zhm = Wire.read();
+  }
+  else
+  {
+    // DLHC, DLM, DLH: X_H, X_L...
+    xhm = Wire.read();
+    xlm = Wire.read();
+
+    if (_device == device_DLH)
+    {
+      // DLH: ...Y_H, Y_L, Z_H, Z_L
+      yhm = Wire.read();
+      ylm = Wire.read();
+      zhm = Wire.read();
+      zlm = Wire.read();
+    }
+    else
+    {
+      // DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L
+      zhm = Wire.read();
+      zlm = Wire.read();
+      yhm = Wire.read();
+      ylm = Wire.read();
+    }
+  }
+
+  // combine high and low bytes
+  m.x = (int16_t)(xhm << 8 | xlm);
+  m.y = (int16_t)(yhm << 8 | ylm);
+  m.z = (int16_t)(zhm << 8 | zlm);
+}
+
+// Reads all 6 channels of the LSM303 and stores them in the object variables
+void LSM303::read(void)
+{
+  readAcc();
+  readMag();
+}
+
+/*
+Returns the angular difference in the horizontal plane between a
+default vector and north, in degrees.
+
+The default vector here is chosen to point along the surface of the
+PCB, in the direction of the top of the text on the silkscreen.
+This is the +X axis on the Pololu LSM303D carrier and the -Y axis on
+the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.
+*/
+float LSM303::heading(void)
+{
+  if (_device == device_D)
+  {
+    return heading((vector<int>){1, 0, 0});
+  }
+  else
+  {
+    return heading((vector<int>){0, -1, 0});
+  }
+}
+
+void LSM303::vector_normalize(vector<float> *a)
+{
+  float mag = sqrt(vector_dot(a, a));
+  a->x /= mag;
+  a->y /= mag;
+  a->z /= mag;
+}
+
+// Private Methods //////////////////////////////////////////////////////////////
+
+int LSM303::testReg(byte address, regAddr reg)
+{
+  Wire.beginTransmission(address);
+  Wire.write((byte)reg);
+  if (Wire.endTransmission() != 0)
+  {
+    return TEST_REG_ERROR;
+  }
+
+  Wire.requestFrom(address, (byte)1);
+  if (Wire.available())
+  {
+    return Wire.read();
+  }
+  else
+  {
+    return TEST_REG_ERROR;
+  }
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.h	(revision 232)
@@ -0,0 +1,266 @@
+#ifndef LSM303_h
+#define LSM303_h
+
+#include <Arduino.h> // for byte data type
+
+class LSM303
+{
+  public:
+    template <typename T> struct vector
+    {
+      T x, y, z;
+    };
+
+    enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };
+    enum sa0State { sa0_low, sa0_high, sa0_auto };
+
+    // register addresses
+    enum regAddr
+    {
+      TEMP_OUT_L        = 0x05, // D
+      TEMP_OUT_H        = 0x06, // D
+
+      STATUS_M          = 0x07, // D
+
+      INT_CTRL_M        = 0x12, // D
+      INT_SRC_M         = 0x13, // D
+      INT_THS_L_M       = 0x14, // D
+      INT_THS_H_M       = 0x15, // D
+
+      OFFSET_X_L_M      = 0x16, // D
+      OFFSET_X_H_M      = 0x17, // D
+      OFFSET_Y_L_M      = 0x18, // D
+      OFFSET_Y_H_M      = 0x19, // D
+      OFFSET_Z_L_M      = 0x1A, // D
+      OFFSET_Z_H_M      = 0x1B, // D
+      REFERENCE_X       = 0x1C, // D
+      REFERENCE_Y       = 0x1D, // D
+      REFERENCE_Z       = 0x1E, // D
+
+      CTRL0             = 0x1F, // D
+      CTRL1             = 0x20, // D
+      CTRL_REG1_A       = 0x20, // DLH, DLM, DLHC
+      CTRL2             = 0x21, // D
+      CTRL_REG2_A       = 0x21, // DLH, DLM, DLHC
+      CTRL3             = 0x22, // D
+      CTRL_REG3_A       = 0x22, // DLH, DLM, DLHC
+      CTRL4             = 0x23, // D
+      CTRL_REG4_A       = 0x23, // DLH, DLM, DLHC
+      CTRL5             = 0x24, // D
+      CTRL_REG5_A       = 0x24, // DLH, DLM, DLHC
+      CTRL6             = 0x25, // D
+      CTRL_REG6_A       = 0x25, // DLHC
+      HP_FILTER_RESET_A = 0x25, // DLH, DLM
+      CTRL7             = 0x26, // D
+      REFERENCE_A       = 0x26, // DLH, DLM, DLHC
+      STATUS_A          = 0x27, // D
+      STATUS_REG_A      = 0x27, // DLH, DLM, DLHC
+
+      OUT_X_L_A         = 0x28,
+      OUT_X_H_A         = 0x29,
+      OUT_Y_L_A         = 0x2A,
+      OUT_Y_H_A         = 0x2B,
+      OUT_Z_L_A         = 0x2C,
+      OUT_Z_H_A         = 0x2D,
+
+      FIFO_CTRL         = 0x2E, // D
+      FIFO_CTRL_REG_A   = 0x2E, // DLHC
+      FIFO_SRC          = 0x2F, // D
+      FIFO_SRC_REG_A    = 0x2F, // DLHC
+
+      IG_CFG1           = 0x30, // D
+      INT1_CFG_A        = 0x30, // DLH, DLM, DLHC
+      IG_SRC1           = 0x31, // D
+      INT1_SRC_A        = 0x31, // DLH, DLM, DLHC
+      IG_THS1           = 0x32, // D
+      INT1_THS_A        = 0x32, // DLH, DLM, DLHC
+      IG_DUR1           = 0x33, // D
+      INT1_DURATION_A   = 0x33, // DLH, DLM, DLHC
+      IG_CFG2           = 0x34, // D
+      INT2_CFG_A        = 0x34, // DLH, DLM, DLHC
+      IG_SRC2           = 0x35, // D
+      INT2_SRC_A        = 0x35, // DLH, DLM, DLHC
+      IG_THS2           = 0x36, // D
+      INT2_THS_A        = 0x36, // DLH, DLM, DLHC
+      IG_DUR2           = 0x37, // D
+      INT2_DURATION_A   = 0x37, // DLH, DLM, DLHC
+
+      CLICK_CFG         = 0x38, // D
+      CLICK_CFG_A       = 0x38, // DLHC
+      CLICK_SRC         = 0x39, // D
+      CLICK_SRC_A       = 0x39, // DLHC
+      CLICK_THS         = 0x3A, // D
+      CLICK_THS_A       = 0x3A, // DLHC
+      TIME_LIMIT        = 0x3B, // D
+      TIME_LIMIT_A      = 0x3B, // DLHC
+      TIME_LATENCY      = 0x3C, // D
+      TIME_LATENCY_A    = 0x3C, // DLHC
+      TIME_WINDOW       = 0x3D, // D
+      TIME_WINDOW_A     = 0x3D, // DLHC
+
+      Act_THS           = 0x3E, // D
+      Act_DUR           = 0x3F, // D
+
+      CRA_REG_M         = 0x00, // DLH, DLM, DLHC
+      CRB_REG_M         = 0x01, // DLH, DLM, DLHC
+      MR_REG_M          = 0x02, // DLH, DLM, DLHC
+
+      SR_REG_M          = 0x09, // DLH, DLM, DLHC
+      IRA_REG_M         = 0x0A, // DLH, DLM, DLHC
+      IRB_REG_M         = 0x0B, // DLH, DLM, DLHC
+      IRC_REG_M         = 0x0C, // DLH, DLM, DLHC
+
+      WHO_AM_I          = 0x0F, // D
+      WHO_AM_I_M        = 0x0F, // DLM
+
+      TEMP_OUT_H_M      = 0x31, // DLHC
+      TEMP_OUT_L_M      = 0x32, // DLHC
+
+
+      // dummy addresses for registers in different locations on different devices;
+      // the library translates these based on device type
+      // value with sign flipped is used as index into translated_regs array
+
+      OUT_X_H_M         = -1,
+      OUT_X_L_M         = -2,
+      OUT_Y_H_M         = -3,
+      OUT_Y_L_M         = -4,
+      OUT_Z_H_M         = -5,
+      OUT_Z_L_M         = -6,
+      // update dummy_reg_count if registers are added here!
+
+      // device-specific register addresses
+
+      DLH_OUT_X_H_M     = 0x03,
+      DLH_OUT_X_L_M     = 0x04,
+      DLH_OUT_Y_H_M     = 0x05,
+      DLH_OUT_Y_L_M     = 0x06,
+      DLH_OUT_Z_H_M     = 0x07,
+      DLH_OUT_Z_L_M     = 0x08,
+
+      DLM_OUT_X_H_M     = 0x03,
+      DLM_OUT_X_L_M     = 0x04,
+      DLM_OUT_Z_H_M     = 0x05,
+      DLM_OUT_Z_L_M     = 0x06,
+      DLM_OUT_Y_H_M     = 0x07,
+      DLM_OUT_Y_L_M     = 0x08,
+
+      DLHC_OUT_X_H_M    = 0x03,
+      DLHC_OUT_X_L_M    = 0x04,
+      DLHC_OUT_Z_H_M    = 0x05,
+      DLHC_OUT_Z_L_M    = 0x06,
+      DLHC_OUT_Y_H_M    = 0x07,
+      DLHC_OUT_Y_L_M    = 0x08,
+
+      D_OUT_X_L_M       = 0x08,
+      D_OUT_X_H_M       = 0x09,
+      D_OUT_Y_L_M       = 0x0A,
+      D_OUT_Y_H_M       = 0x0B,
+      D_OUT_Z_L_M       = 0x0C,
+      D_OUT_Z_H_M       = 0x0D
+    };
+
+    vector<int16_t> a; // accelerometer readings
+    vector<int16_t> m; // magnetometer readings
+    vector<int16_t> m_max; // maximum magnetometer values, used for calibration
+    vector<int16_t> m_min; // minimum magnetometer values, used for calibration
+
+    byte last_status; // status of last I2C transmission
+
+    LSM303(void);
+
+    bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
+    deviceType getDeviceType(void) { return _device; }
+
+    void enableDefault(void);
+
+    void writeAccReg(byte reg, byte value);
+    byte readAccReg(byte reg);
+    void writeMagReg(byte reg, byte value);
+    byte readMagReg(int reg);
+
+    void writeReg(byte reg, byte value);
+    byte readReg(int reg);
+
+    void readAcc(void);
+    void readMag(void);
+    void read(void);
+
+    void setTimeout(unsigned int timeout);
+    unsigned int getTimeout(void);
+    bool timeoutOccurred(void);
+
+    float heading(void);
+    template <typename T> float heading(vector<T> from);
+
+    // vector functions
+    template <typename Ta, typename Tb, typename To> static void vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
+    template <typename Ta, typename Tb> static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
+    static void vector_normalize(vector<float> *a);
+
+  private:
+    deviceType _device; // chip type (D, DLHC, DLM, or DLH)
+    byte acc_address;
+    byte mag_address;
+
+    static const int dummy_reg_count = 6;
+    regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
+
+    unsigned int io_timeout;
+    bool did_timeout;
+
+    int testReg(byte address, regAddr reg);
+};
+
+/*
+Returns the angular difference in the horizontal plane between the
+"from" vector and north, in degrees.
+
+Description of heading algorithm:
+Shift and scale the magnetic reading based on calibration data to find
+the North vector. Use the acceleration readings to determine the Up
+vector (gravity is measured as an upward acceleration). The cross
+product of North and Up vectors is East. The vectors East and North
+form a basis for the horizontal plane. The From vector is projected
+into the horizontal plane and the angle between the projected vector
+and horizontal north is returned.
+*/
+template <typename T> float LSM303::heading(vector<T> from)
+{
+    vector<int32_t> temp_m = {m.x, m.y, m.z};
+
+    // subtract offset (average of min and max) from magnetometer readings
+    temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
+    temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
+    temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
+
+    // compute E and N
+    vector<float> E;
+    vector<float> N;
+    vector_cross(&temp_m, &a, &E);
+    vector_normalize(&E);
+    vector_cross(&a, &E, &N);
+    vector_normalize(&N);
+
+    // compute heading
+    float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
+    if (heading < 0) heading += 360;
+    return heading;
+}
+
+template <typename Ta, typename Tb, typename To> void LSM303::vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
+{
+  out->x = (a->y * b->z) - (a->z * b->y);
+  out->y = (a->z * b->x) - (a->x * b->z);
+  out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+template <typename Ta, typename Tb> float LSM303::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
+{
+  return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
+
+
+
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Pushbutton.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Pushbutton.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Pushbutton.cpp	(revision 232)
@@ -0,0 +1,173 @@
+#include "Pushbutton.h"
+
+// constructor; takes arguments specifying whether to enable internal pull-up
+// and the default state of the pin that the button is connected to
+Pushbutton::Pushbutton(unsigned char pin, unsigned char pullUp, unsigned char defaultState)
+{
+  _pin = pin;
+  _pullUp = pullUp;
+  _defaultState = defaultState;
+  gsdpState = 0;
+  gsdrState = 0;
+  gsdpPrevTimeMillis = 0;
+  gsdrPrevTimeMillis = 0;
+  initialized = false;
+}
+
+// wait for button to be pressed
+void Pushbutton::waitForPress()
+{
+  init(); // initialize if necessary
+
+  do
+  {
+    while (!_isPressed()){delay(1);}; // wait for button to be pressed
+    delay(10);            // debounce the button press
+  }
+  while (!_isPressed());   // if button isn't still pressed, loop
+}
+
+// wait for button to be released
+void Pushbutton::waitForRelease()
+{
+  init(); // initialize if necessary
+
+  do
+  {
+    while (_isPressed()){delay(1);}; // wait for button to be released
+    delay(10);           // debounce the button release
+  }
+  while (_isPressed());   // if button isn't still released, loop
+}
+
+// wait for button to be pressed, then released
+void Pushbutton::waitForButton()
+{
+  waitForPress();
+  waitForRelease();
+}
+
+// indicates whether button is pressed
+boolean Pushbutton::isPressed()
+{
+  init(); // initialize if necessary
+
+  return _isPressed();
+}
+
+// Uses a finite state machine to detect a single button press and returns
+// true to indicate the press (false otherwise).  It requires the button to be
+// released for at least 15 ms and then pressed for at least 15 ms before
+// reporting the press.  This function handles all necessary debouncing and
+// should be called repeatedly in a loop.
+boolean Pushbutton::getSingleDebouncedPress()
+{
+  unsigned long timeMillis = millis();
+
+  init(); // initialize if necessary
+
+  switch (gsdpState)
+  {
+    case 0:
+      if (!_isPressed())                  // if button is released
+      {
+        gsdpPrevTimeMillis = timeMillis;
+        gsdpState = 1;                    // proceed to next state
+      }
+      break;
+
+    case 1:
+      if ((timeMillis - gsdpPrevTimeMillis >= 15) && !_isPressed()) // if 15 ms or longer has elapsed and button is still released
+        gsdpState = 2;                    // proceed to next state
+      else if (_isPressed())
+        gsdpState = 0;                    // button is pressed or bouncing, so go back to previous (initial) state
+      break;
+
+    case 2:
+      if (_isPressed())                   // if button is now pressed
+      {
+        gsdpPrevTimeMillis = timeMillis;
+        gsdpState = 3;                    // proceed to next state
+      }
+      break;
+
+    case 3:
+      if ((timeMillis - gsdpPrevTimeMillis >= 15) && _isPressed())  // if 15 ms or longer has elapsed and button is still pressed
+      {
+        gsdpState = 0;                    // next state becomes initial state
+        return true;                      // report button press
+      }
+      else if (!_isPressed())
+        gsdpState = 2;                    // button is released or bouncing, so go back to previous state
+      break;
+  }
+
+  return false;
+}
+
+// Uses a finite state machine to detect a single button release and returns
+// true to indicate the release (false otherwise).  It requires the button to be
+// pressed for at least 15 ms and then released for at least 15 ms before
+// reporting the release.  This function handles all necessary debouncing and
+// should be called repeatedly in a loop.
+boolean Pushbutton::getSingleDebouncedRelease()
+{
+  unsigned int timeMillis = millis();
+
+  init(); // initialize if necessary
+
+  switch (gsdrState)
+  {
+    case 0:
+      if (_isPressed())                   // if button is pressed
+      {
+        gsdrPrevTimeMillis = timeMillis;
+        gsdrState = 1;                    // proceed to next state
+      }
+      break;
+
+    case 1:
+      if ((timeMillis - gsdrPrevTimeMillis >= 15) && _isPressed())  // if 15 ms or longer has elapsed and button is still pressed
+        gsdrState = 2;                    // proceed to next state
+      else if (!_isPressed())
+        gsdrState = 0;                    // button is released or bouncing, so go back to previous (initial) state
+      break;
+
+    case 2:
+      if (!_isPressed())                  // if button is now released
+      {
+        gsdrPrevTimeMillis = timeMillis;
+        gsdrState = 3;                    // proceed to next state
+      }
+      break;
+
+    case 3:
+      if ((timeMillis - gsdrPrevTimeMillis >= 15) && !_isPressed()) // if 15 ms or longer has elapsed and button is still released
+      {
+        gsdrState = 0;                    // next state becomes initial state
+        return true;                      // report button release
+      }
+      else if (_isPressed())
+        gsdrState = 2;                    // button is pressed or bouncing, so go back to previous state
+      break;
+  }
+
+  return false;
+}
+
+// initializes I/O pin for use as button inputs
+void Pushbutton::init2()
+{
+  if (_pullUp == PULL_UP_ENABLED)
+    pinMode(_pin, INPUT_PULLUP);
+  else
+    pinMode(_pin, INPUT); // high impedance
+
+  delayMicroseconds(5); // give pull-up time to stabilize
+}
+
+// button is pressed if pin state differs from default state
+inline boolean Pushbutton::_isPressed()
+{
+  return (digitalRead(_pin) == LOW) ^ (_defaultState == DEFAULT_STATE_LOW);
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Pushbutton.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Pushbutton.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/Pushbutton.h	(revision 232)
@@ -0,0 +1,70 @@
+/*! \file Pushbutton.h
+ *
+ * See the Pushbutton class reference for more information about this library.
+ *
+ * \class Pushbutton Pushbutton.h
+ * \brief Read button presses and releases with debouncing
+ * 
+ */
+
+#ifndef Pushbutton_h
+#define Pushbutton_h
+
+#include <Arduino.h>
+
+#define ZUMO_BUTTON 12
+
+#define PULL_UP_DISABLED    0
+#define PULL_UP_ENABLED     1
+
+#define DEFAULT_STATE_LOW   0
+#define DEFAULT_STATE_HIGH  1
+
+class Pushbutton
+{
+  public:
+
+    // constructor; takes arguments specifying whether to enable internal pull-up
+    // and the default state of the pin that the button is connected to
+    Pushbutton(unsigned char pin, unsigned char pullUp = PULL_UP_ENABLED, unsigned char defaultState = DEFAULT_STATE_HIGH);
+
+    // wait for button to be pressed, released, or pressed and released
+    void waitForPress();
+    void waitForRelease();
+    void waitForButton();
+
+    // indicates whether button is currently pressed
+    boolean isPressed();
+
+    // more complex functions that return true once for each button transition
+    // from released to pressed or pressed to released
+    boolean getSingleDebouncedPress();
+    boolean getSingleDebouncedRelease();
+
+    void init()
+    {
+      if (!initialized)
+      {
+        initialized = true;
+        init2();
+      }
+    }
+    
+  private:
+
+    unsigned char _pin;
+    unsigned char _pullUp;
+    unsigned char _defaultState;
+    unsigned char gsdpState;
+    unsigned char gsdrState;
+    unsigned int gsdpPrevTimeMillis;
+    unsigned int gsdrPrevTimeMillis;
+    boolean initialized;
+
+    // initializes I/O pin for use as button input
+    void init2();
+
+    boolean _isPressed();
+};
+
+#endif
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.cpp	(revision 232)
@@ -0,0 +1,574 @@
+/*
+  QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
+    sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+    QTR-8RC.  The object used will determine the type of the sensor (either
+    QTR-xA or QTR-xRC).  Then simply specify in the constructor which
+    Arduino I/O pins are connected to a QTR sensor, and the read() method
+    will obtain reflectance measurements for those sensors.  Smaller sensor
+    values correspond to higher reflectance (e.g. white) while larger
+    sensor values correspond to lower reflectance (e.g. black or a void).
+
+    * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+    * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010.
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ *   http://www.pololu.com
+ *   http://forum.pololu.com
+ *   http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above).  Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ *   http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty.  It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ */
+
+#include <stdlib.h>
+#include "QTRSensors.h"
+#include <Arduino.h>
+
+
+
+// Base class data member initialization (called by derived class init())
+void QTRSensors::init(unsigned char *pins, unsigned char numSensors,
+  unsigned char emitterPin)
+{
+    calibratedMinimumOn=0;
+    calibratedMaximumOn=0;
+    calibratedMinimumOff=0;
+    calibratedMaximumOff=0;
+
+    if (numSensors > QTR_MAX_SENSORS)
+        _numSensors = QTR_MAX_SENSORS;
+    else
+        _numSensors = numSensors;
+
+    if (_pins == 0)
+    {
+        _pins = (unsigned char*)malloc(sizeof(unsigned char)*_numSensors);
+        if (_pins == 0)
+            return;
+    }
+
+    unsigned char i;
+    for (i = 0; i < _numSensors; i++)
+    {
+        _pins[i] = pins[i];
+    }
+
+    _emitterPin = emitterPin;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in abstract units,
+// with higher values corresponding to lower reflectance (e.g. a black
+// surface or a void).
+void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
+{
+    unsigned int off_values[QTR_MAX_SENSORS];
+    unsigned char i;
+
+    if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF)
+        emittersOn();
+    else
+        emittersOff();
+
+    readPrivate(sensor_values);
+    emittersOff();
+
+    if(readMode == QTR_EMITTERS_ON_AND_OFF)
+    {
+        readPrivate(off_values);
+
+        for(i=0;i<_numSensors;i++)
+        {
+            sensor_values[i] += _maxValue - off_values[i];
+        }
+    }
+}
+
+
+// Turn the IR LEDs off and on.  This is mainly for use by the
+// read method, and calling these functions before or
+// after the reading the sensors will have no effect on the
+// readings, but you may wish to use these for testing purposes.
+void QTRSensors::emittersOff()
+{
+    if (_emitterPin == QTR_NO_EMITTER_PIN)
+        return;
+    pinMode(_emitterPin, OUTPUT);
+    digitalWrite(_emitterPin, LOW);
+    delayMicroseconds(200);
+}
+
+void QTRSensors::emittersOn()
+{
+    if (_emitterPin == QTR_NO_EMITTER_PIN)
+        return;
+    pinMode(_emitterPin, OUTPUT);
+    digitalWrite(_emitterPin, HIGH);
+    delayMicroseconds(200);
+}
+
+// Resets the calibration.
+void QTRSensors::resetCalibration()
+{
+    unsigned char i;
+    for(i=0;i<_numSensors;i++)
+    {
+        if(calibratedMinimumOn)
+            calibratedMinimumOn[i] = _maxValue;
+        if(calibratedMinimumOff)
+            calibratedMinimumOff[i] = _maxValue;
+        if(calibratedMaximumOn)
+            calibratedMaximumOn[i] = 0;
+        if(calibratedMaximumOff)
+            calibratedMaximumOff[i] = 0;
+    }
+}
+
+// Reads the sensors 10 times and uses the results for
+// calibration.  The sensor values are not returned; instead, the
+// maximum and minimum values found over time are stored internally
+// and used for the readCalibrated() method.
+void QTRSensors::calibrate(unsigned char readMode)
+{
+    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+    {
+        calibrateOnOrOff(&calibratedMinimumOn,
+                         &calibratedMaximumOn,
+                         QTR_EMITTERS_ON);
+    }
+
+
+    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+    {
+        calibrateOnOrOff(&calibratedMinimumOff,
+                         &calibratedMaximumOff,
+                         QTR_EMITTERS_OFF);
+    }
+}
+
+void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
+                                        unsigned int **calibratedMaximum,
+                                        unsigned char readMode)
+{
+    int i;
+    unsigned int sensor_values[16];
+    unsigned int max_sensor_values[16];
+    unsigned int min_sensor_values[16];
+
+    // Allocate the arrays if necessary.
+    if(*calibratedMaximum == 0)
+    {
+        *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+        // If the malloc failed, don't continue.
+        if(*calibratedMaximum == 0)
+            return;
+
+        // Initialize the max and min calibrated values to values that
+        // will cause the first reading to update them.
+
+        for(i=0;i<_numSensors;i++)
+            (*calibratedMaximum)[i] = 0;
+    }
+    if(*calibratedMinimum == 0)
+    {
+        *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+        // If the malloc failed, don't continue.
+        if(*calibratedMinimum == 0)
+            return;
+
+        for(i=0;i<_numSensors;i++)
+            (*calibratedMinimum)[i] = _maxValue;
+    }
+
+    int j;
+    for(j=0;j<10;j++)
+    {
+        read(sensor_values,readMode);
+        for(i=0;i<_numSensors;i++)
+        {
+            // set the max we found THIS time
+            if(j == 0 || max_sensor_values[i] < sensor_values[i])
+                max_sensor_values[i] = sensor_values[i];
+
+            // set the min we found THIS time
+            if(j == 0 || min_sensor_values[i] > sensor_values[i])
+                min_sensor_values[i] = sensor_values[i];
+        }
+    }
+
+    // record the min and max calibration values
+    for(i=0;i<_numSensors;i++)
+    {
+        if(min_sensor_values[i] > (*calibratedMaximum)[i])
+            (*calibratedMaximum)[i] = min_sensor_values[i];
+        if(max_sensor_values[i] < (*calibratedMinimum)[i])
+            (*calibratedMinimum)[i] = max_sensor_values[i];
+    }
+}
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+// 0 corresponds to the minimum value read by calibrate() and 1000
+// corresponds to the maximum value.  Calibration values are
+// stored separately for each sensor, so that differences in the
+// sensors are accounted for automatically.
+void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
+{
+    int i;
+
+    // if not calibrated, do nothing
+    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+        if(!calibratedMinimumOff || !calibratedMaximumOff)
+            return;
+    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+        if(!calibratedMinimumOn || !calibratedMaximumOn)
+            return;
+
+    // read the needed values
+    read(sensor_values,readMode);
+
+    for(i=0;i<_numSensors;i++)
+    {
+        unsigned int calmin,calmax;
+        unsigned int denominator;
+
+        // find the correct calibration
+        if(readMode == QTR_EMITTERS_ON)
+        {
+            calmax = calibratedMaximumOn[i];
+            calmin = calibratedMinimumOn[i];
+        }
+        else if(readMode == QTR_EMITTERS_OFF)
+        {
+            calmax = calibratedMaximumOff[i];
+            calmin = calibratedMinimumOff[i];
+        }
+        else // QTR_EMITTERS_ON_AND_OFF
+        {
+
+            if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
+                calmin = _maxValue;
+            else
+                calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
+
+            if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
+                calmax = _maxValue;
+            else
+                calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
+        }
+
+        denominator = calmax - calmin;
+
+        signed int x = 0;
+        if(denominator != 0) {
+            if (sensor_values[i] < calmin) {
+                x = 0;
+            }
+            else if (sensor_values[i] > calmax) {
+                x = 1000;
+            }
+            else {
+                x = (((signed long)sensor_values[i]) - calmin)
+                  * 1000 / denominator;
+            }
+        }
+        if(x < 0)
+            x = 0;
+        else if(x > 1000)
+            x = 1000;
+        sensor_values[i] = x;
+    }
+
+}
+
+
+// Operates the same as read calibrated, but also returns an
+// estimated position of the robot with respect to a line. The
+// estimate is made using a weighted average of the sensor indices
+// multiplied by 1000, so that a return value of 0 indicates that
+// the line is directly below sensor 0, a return value of 1000
+// indicates that the line is directly below sensor 1, 2000
+// indicates that it's below sensor 2000, etc.  Intermediate
+// values indicate that the line is between two sensors.  The
+// formula is:
+//
+//    0*value0 + 1000*value1 + 2000*value2 + ...
+//   --------------------------------------------
+//         value0  +  value1  +  value2 + ...
+//
+// By default, this function assumes a dark line (high values)
+// surrounded by white (low values).  If your line is light on
+// black, set the optional second argument white_line to true.  In
+// this case, each sensor value will be replaced by (1000-value)
+// before the averaging.
+int QTRSensors::readLine(unsigned int *sensor_values,
+    unsigned char readMode, unsigned char white_line)
+{
+    unsigned char i, on_line = 0;
+    unsigned long avg; // this is for the weighted total, which is long
+                       // before division
+    unsigned int sum; // this is for the denominator which is <= 64000
+    static int last_value=0; // assume initially that the line is left.
+
+    readCalibrated(sensor_values, readMode);
+
+    avg = 0;
+    sum = 0;
+
+    for(i=0;i<_numSensors;i++) {
+        int value = sensor_values[i];
+        if(white_line)
+            value = 1000-value;
+
+        // keep track of whether we see the line at all
+        if(value > 200) {
+            on_line = 1;
+        }
+
+        // only average in values that are above a noise threshold
+        if(value > 50) {
+            avg += (long)(value) * (i * 1000);
+            sum += value;
+        }
+    }
+
+    if(!on_line)
+    {
+        // If it last read to the left of center, return 0.
+        if(last_value < (_numSensors-1)*1000/2)
+            return 0;
+
+        // If it last read to the right of center, return the max.
+        else
+            return (_numSensors-1)*1000;
+
+    }
+
+    last_value = avg/sum;
+
+    return last_value;
+}
+
+
+
+// Derived RC class constructors
+QTRSensorsRC::QTRSensorsRC()
+{
+    calibratedMinimumOn = 0;
+    calibratedMaximumOn = 0;
+    calibratedMinimumOff = 0;
+    calibratedMaximumOff = 0;
+    _pins = 0;
+}
+
+QTRSensorsRC::QTRSensorsRC(unsigned char* pins,
+  unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+    calibratedMinimumOn = 0;
+    calibratedMaximumOn = 0;
+    calibratedMinimumOff = 0;
+    calibratedMaximumOff = 0;
+    _pins = 0;
+
+    init(pins, numSensors, timeout, emitterPin);
+}
+
+
+// The array 'pins' contains the Arduino pin number for each sensor.
+
+// 'numSensors' specifies the length of the 'pins' array (i.e. the
+// number of QTR-RC sensors you are using).  numSensors must be
+// no greater than 16.
+
+// 'timeout' specifies the length of time in microseconds beyond
+// which you consider the sensor reading completely black.  That is to say,
+// if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+// and the reading for that pin will be considered full black.
+// It is recommended that you set timeout to be between 1000 and
+// 3000 us, depending on things like the height of your sensors and
+// ambient lighting.  Using timeout allows you to shorten the
+// duration of a sensor-reading cycle while still maintaining
+// useful analog measurements of reflectance
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules.  If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsRC::init(unsigned char* pins,
+    unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+    QTRSensors::init(pins, numSensors, emitterPin);
+
+    _maxValue = timeout;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// ...
+// The values returned are in microseconds and range from 0 to
+// timeout (as specified in the constructor).
+void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
+{
+    unsigned char i;
+
+    if (_pins == 0)
+        return;
+
+    for(i = 0; i < _numSensors; i++)
+    {
+        sensor_values[i] = _maxValue;
+        digitalWrite(_pins[i], HIGH);   // make sensor line an output
+        pinMode(_pins[i], OUTPUT);      // drive sensor line high
+    }
+
+    delayMicroseconds(10);              // charge lines for 10 us
+
+    for(i = 0; i < _numSensors; i++)
+    {
+        pinMode(_pins[i], INPUT);       // make sensor line an input
+#ifndef ARDUINO_ARCH_SAMD        
+        digitalWrite(_pins[i], LOW);        // important: disable internal pull-up!
+#endif  /* !ARDUINO_ARCH_SAMD */
+    }
+
+    unsigned long startTime = micros();
+    while (micros() - startTime < _maxValue)
+    {
+        unsigned int time = micros() - startTime;
+        for (i = 0; i < _numSensors; i++)
+        {
+            if (digitalRead(_pins[i]) == LOW && time < sensor_values[i])
+                sensor_values[i] = time;
+        }
+    }
+}
+
+
+
+// Derived Analog class constructors
+QTRSensorsAnalog::QTRSensorsAnalog()
+{
+    calibratedMinimumOn = 0;
+    calibratedMaximumOn = 0;
+    calibratedMinimumOff = 0;
+    calibratedMaximumOff = 0;
+    _pins = 0;
+}
+
+QTRSensorsAnalog::QTRSensorsAnalog(unsigned char* pins,
+  unsigned char numSensors, unsigned char numSamplesPerSensor,
+  unsigned char emitterPin)
+{
+    calibratedMinimumOn = 0;
+    calibratedMaximumOn = 0;
+    calibratedMinimumOff = 0;
+    calibratedMaximumOff = 0;
+    _pins = 0;
+
+    init(pins, numSensors, numSamplesPerSensor, emitterPin);
+}
+
+
+// the array 'pins' contains the Arduino analog pin assignment for each
+// sensor.  For example, if pins is {0, 1, 7}, sensor 1 is on
+// Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+// and sensor 3 is on Arduino analog input 7.
+
+// 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+// number of QTR-A sensors you are using).  numSensors must be
+// no greater than 16.
+
+// 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+// to average per channel (i.e. per sensor) for each reading.  The total
+// number of analog-to-digital conversions performed will be equal to
+// numSensors*numSamplesPerSensor.  Note that it takes about 100 us to
+// perform a single analog-to-digital conversion, so:
+// if numSamplesPerSensor is 4 and numSensors is 6, it will take
+// 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+// Increasing this parameter increases noise suppression at the cost of
+// sample rate.  The recommended value is 4.
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules.  If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsAnalog::init(unsigned char* pins,
+    unsigned char numSensors, unsigned char numSamplesPerSensor,
+    unsigned char emitterPin)
+{
+    QTRSensors::init(pins, numSensors, emitterPin);
+
+    _numSamplesPerSensor = numSamplesPerSensor;
+    _maxValue = 1023; // this is the maximum returned by the A/D conversion
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in terms of a
+// 10-bit ADC average with higher values corresponding to lower
+// reflectance (e.g. a black surface or a void).
+void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
+{
+    unsigned char i, j;
+
+    if (_pins == 0)
+        return;
+
+    // reset the values
+    for(i = 0; i < _numSensors; i++)
+        sensor_values[i] = 0;
+
+    for (j = 0; j < _numSamplesPerSensor; j++)
+    {
+        for (i = 0; i < _numSensors; i++)
+        {
+            sensor_values[i] += analogRead(_pins[i]);   // add the conversion result
+        }
+    }
+
+    // get the rounded average of the readings for each sensor
+    for (i = 0; i < _numSensors; i++)
+        sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
+            _numSamplesPerSensor;
+}
+
+// the destructor frees up allocated memory
+QTRSensors::~QTRSensors()
+{
+    if (_pins)
+        free(_pins);
+    if(calibratedMaximumOn)
+        free(calibratedMaximumOn);
+    if(calibratedMaximumOff)
+        free(calibratedMaximumOff);
+    if(calibratedMinimumOn)
+        free(calibratedMinimumOn);
+    if(calibratedMinimumOff)
+        free(calibratedMinimumOff);
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.dox
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.dox	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.dox	(revision 232)
@@ -0,0 +1,31 @@
+/*! \file QTRSensors.h
+ * 
+ * This library is included in the Zumo Shield libraries as a dependency for the
+ * ZumoReflectanceSensorArray library. If you are programming an Arduino on a
+ * Zumo, you should generally use the ZumoReflectanceSensorArray library instead
+ * of using the %QTRSensors library directly.
+ *
+ * For documentation specific to the %QTRSensors library, please see the [user's
+ * guide](http://www.pololu.com/docs/0J19) on Pololu's website.
+ * 
+ *
+ * \class QTRSensors QTRSensors.h
+ * \brief <em>(%ZumoReflectanceSensorArray dependency)</em>
+ *
+ * This class is part of the \ref QTRSensors.h "QTRSensors" library, which is
+ * included in the Zumo Shield libraries as a dependency for the
+ * ZumoReflectanceSensorArray library. If you are programming an Arduino on a
+ * Zumo, you should generally use the ZumoReflectanceSensorArray library instead
+ * of using the %QTRSensors library directly.
+ *
+ * For documentation specific to the %QTRSensors library, please see the [user's
+ * guide](http://www.pololu.com/docs/0J19) on Pololu's website.
+ *
+ *
+ * \class QTRSensorsRC QTRSensors.h
+ * \copydoc QTRSensors
+ * 
+ *
+ * \class QTRSensorsAnalog QTRSensors.h
+ * \copydoc QTRSensors
+ */
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/QTRSensors.h	(revision 232)
@@ -0,0 +1,268 @@
+/*
+  QTRSensors.h - Library for using Pololu QTR reflectance
+    sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+    QTR-8RC.  The object used will determine the type of the sensor (either
+    QTR-xA or QTR-xRC).  Then simply specify in the constructor which
+    Arduino I/O pins are connected to a QTR sensor, and the read() method
+    will obtain reflectance measurements for those sensors.  Smaller sensor
+    values correspond to higher reflectance (e.g. white) while larger
+    sensor values correspond to lower reflectance (e.g. black or a void).
+
+    * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+    * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ *   http://www.pololu.com
+ *   http://forum.pololu.com
+ *   http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above).  Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ *   http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty.  It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ */
+
+#ifndef QTRSensors_h
+#define QTRSensors_h
+
+#define QTR_EMITTERS_OFF 0
+#define QTR_EMITTERS_ON 1
+#define QTR_EMITTERS_ON_AND_OFF 2
+
+#define QTR_NO_EMITTER_PIN  255
+
+#define QTR_MAX_SENSORS 16
+
+// This class cannot be instantiated directly (it has no constructor).
+// Instead, you should instantiate one of its two derived classes (either the
+// QTR-A or QTR-RC version, depending on the type of your sensor).
+class QTRSensors
+{
+  public:
+
+    // Reads the sensor values into an array. There *MUST* be space
+    // for as many values as there were sensors specified in the constructor.
+    // Example usage:
+    // unsigned int sensor_values[8];
+    // sensors.read(sensor_values);
+    // The values returned are a measure of the reflectance in abstract units,
+    // with higher values corresponding to lower reflectance (e.g. a black
+    // surface or a void).
+    // If measureOffAndOn is true, measures the values with the
+    // emitters on AND off and returns on - (timeout - off).  If this
+    // value is less than zero, it returns zero.
+    // This method will call the appropriate derived class's readPrivate(),
+    // which is defined as a virtual function in the base class and
+    // overridden by each derived class's own implementation.
+    void read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+    // Turn the IR LEDs off and on.  This is mainly for use by the
+    // read method, and calling these functions before or
+    // after the reading the sensors will have no effect on the
+    // readings, but you may wish to use these for testing purposes.
+    void emittersOff();
+    void emittersOn();
+
+    // Reads the sensors for calibration.  The sensor values are
+    // not returned; instead, the maximum and minimum values found
+    // over time are stored internally and used for the
+    // readCalibrated() method.
+    void calibrate(unsigned char readMode = QTR_EMITTERS_ON);
+
+    // Resets all calibration that has been done.
+    void resetCalibration();
+
+    // Returns values calibrated to a value between 0 and 1000, where
+    // 0 corresponds to the minimum value read by calibrate() and 1000
+    // corresponds to the maximum value.  Calibration values are
+    // stored separately for each sensor, so that differences in the
+    // sensors are accounted for automatically.
+    void readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+    // Operates the same as read calibrated, but also returns an
+    // estimated position of the robot with respect to a line. The
+    // estimate is made using a weighted average of the sensor indices
+    // multiplied by 1000, so that a return value of 0 indicates that
+    // the line is directly below sensor 0, a return value of 1000
+    // indicates that the line is directly below sensor 1, 2000
+    // indicates that it's below sensor 2000, etc.  Intermediate
+    // values indicate that the line is between two sensors.  The
+    // formula is:
+    //
+    //    0*value0 + 1000*value1 + 2000*value2 + ...
+    //   --------------------------------------------
+    //         value0  +  value1  +  value2 + ...
+    //
+    // By default, this function assumes a dark line (high values)
+    // surrounded by white (low values).  If your line is light on
+    // black, set the optional second argument white_line to true.  In
+    // this case, each sensor value will be replaced by (1000-value)
+    // before the averaging.
+    int readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char white_line = 0);
+
+    // Calibrated minumum and maximum values. These start at 1000 and
+    // 0, respectively, so that the very first sensor reading will
+    // update both of them.
+    //
+    // The pointers are unallocated until calibrate() is called, and
+    // then allocated to exactly the size required.  Depending on the
+    // readMode argument to calibrate, only the On or Off values may
+    // be allocated, as required.
+    //
+    // These variables are made public so that you can use them for
+    // your own calculations and do things like saving the values to
+    // EEPROM, performing sanity checking, etc.
+    unsigned int *calibratedMinimumOn;
+    unsigned int *calibratedMaximumOn;
+    unsigned int *calibratedMinimumOff;
+    unsigned int *calibratedMaximumOff;
+
+    ~QTRSensors();
+
+  protected:
+
+    QTRSensors()
+    {
+
+    };
+
+    void init(unsigned char *pins, unsigned char numSensors, unsigned char emitterPin);
+
+    unsigned char *_pins;
+    unsigned char _numSensors;
+    unsigned char _emitterPin;
+    unsigned int _maxValue; // the maximum value returned by this function
+
+  private:
+
+    virtual void readPrivate(unsigned int *sensor_values) = 0;
+
+    // Handles the actual calibration. calibratedMinimum and
+    // calibratedMaximum are pointers to the requested calibration
+    // arrays, which will be allocated if necessary.
+    void calibrateOnOrOff(unsigned int **calibratedMinimum,
+                          unsigned int **calibratedMaximum,
+                          unsigned char readMode);
+};
+
+
+
+// Object to be used for QTR-1RC and QTR-8RC sensors
+class QTRSensorsRC : public QTRSensors
+{
+  public:
+
+    // if this constructor is used, the user must call init() before using
+    // the methods in this class
+    QTRSensorsRC();
+
+    // this constructor just calls init()
+    QTRSensorsRC(unsigned char* pins, unsigned char numSensors,
+          unsigned int timeout = 4000, unsigned char emitterPin = 255);
+
+    // The array 'pins' contains the Arduino pin number for each sensor.
+
+    // 'numSensors' specifies the length of the 'pins' array (i.e. the
+    // number of QTR-RC sensors you are using).  numSensors must be
+    // no greater than 16.
+
+    // 'timeout' specifies the length of time in microseconds beyond
+    // which you consider the sensor reading completely black.  That is to say,
+    // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+    // and the reading for that pin will be considered full black.
+    // It is recommended that you set timeout to be between 1000 and
+    // 3000 us, depending on things like the height of your sensors and
+    // ambient lighting.  Using timeout allows you to shorten the
+    // duration of a sensor-reading cycle while still maintaining
+    // useful analog measurements of reflectance
+
+    // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+    // modules.  If you are using a 1RC (i.e. if there is no emitter pin),
+    // or if you just want the emitters on all the time and don't want to
+    // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+    void init(unsigned char* pins, unsigned char numSensors,
+          unsigned int timeout = 2000, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+  private:
+
+    // Reads the sensor values into an array. There *MUST* be space
+    // for as many values as there were sensors specified in the constructor.
+    // Example usage:
+    // unsigned int sensor_values[8];
+    // sensors.read(sensor_values);
+    // The values returned are a measure of the reflectance in microseconds.
+    void readPrivate(unsigned int *sensor_values);
+};
+
+
+
+// Object to be used for QTR-1A and QTR-8A sensors
+class QTRSensorsAnalog : public QTRSensors
+{
+  public:
+
+    // if this constructor is used, the user must call init() before using
+    // the methods in this class
+    QTRSensorsAnalog();
+
+    // this constructor just calls init()
+    QTRSensorsAnalog(unsigned char* pins,
+        unsigned char numSensors, unsigned char numSamplesPerSensor = 4,
+        unsigned char emitterPin = 255);
+
+    // the array 'pins' contains the Arduino analog pin assignment for each
+    // sensor.  For example, if pins is {0, 1, 7}, sensor 1 is on
+    // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+    // and sensor 3 is on Arduino analog input 7.
+
+    // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+    // number of QTR-A sensors you are using).  numSensors must be
+    // no greater than 16.
+
+    // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+    // to average per channel (i.e. per sensor) for each reading.  The total
+    // number of analog-to-digital conversions performed will be equal to
+    // numSensors*numSamplesPerSensor.  Note that it takes about 100 us to
+    // perform a single analog-to-digital conversion, so:
+    // if numSamplesPerSensor is 4 and numSensors is 6, it will take
+    // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+    // Increasing this parameter increases noise suppression at the cost of
+    // sample rate.  The recommended value is 4.
+
+    // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+    // modules.  If you are using a 1RC (i.e. if there is no emitter pin),
+    // or if you just want the emitters on all the time and don't want to
+    // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+    void init(unsigned char* analogPins, unsigned char numSensors,
+        unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+  private:
+
+    // Reads the sensor values into an array. There *MUST* be space
+    // for as many values as there were sensors specified in the constructor.
+    // Example usage:
+    // unsigned int sensor_values[8];
+    // sensors.read(sensor_values);
+    // The values returned are a measure of the reflectance in terms of a
+    // 10-bit ADC average with higher values corresponding to lower
+    // reflectance (e.g. a black surface or a void).
+    void readPrivate(unsigned int *sensor_values);
+
+    unsigned char _numSamplesPerSensor;
+};
+
+
+#endif
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/README.textile
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/README.textile	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/README.textile	(revision 232)
@@ -0,0 +1,76 @@
+h1. Pololu Zumo Shield Libraries(Modified)
+
+This Libraries is based on "Pololu Zumo Shield Libraries":https://github.com/pololu/zumo-shield. 
+
+Version: 1.2.3
+Release Date: 2013-11-27
+"www.pololu.com":http://www.pololu.com/
+
+h2. Summary
+
+This is a collection of libraries for the "Arduino":http://pololu.com/catalog/product/2191 that interfaces with the Pololu "Zumo Shield":http://www.pololu.com/catalog/product/2508, "Zumo Reflectance Sensor Array":http://www.pololu.com/catalog/product/1419, "Zumo robot kit":http://www.pololu.com/catalog/product/2509, and "Zumo robot for Arduino":http://www.pololu.com/catalog/product/2510. These libraries provide functions to help you program an Arduino-controlled Zumo robot, and the included example projects demonstrate how to use them for more complex tasks.
+
+For more information about the libraries and examples, please see the "Zumo Shield user's guide":http://www.pololu.com/docs/0J57.
+
+h2. Getting Started
+
+h3. Software
+
+Download the archive from "GitHub":https://github.com/pololu/zumo-shield-arduino, decompress it, and move each library folder (Pushbutton, QTRSensors, ZumoBuzzer, ZumoExamples, ZumoMotors, and ZumoReflectanceSensorArray) into the "libraries" subdirectory inside your Arduino sketchbook directory. You can view your sketchbook location by selecting File->Preferences in the Arduino environment; if there is not already a "libraries" folder in that location, you should create it yourself. After installing the library, restart the Arduino environment so it can find the Zumo Shield libraries and their examples.
+
+Some of the examples also require our "LSM303 library":https://github.com/pololu/lsm303-arduino to be installed.
+
+h3. Hardware
+
+The Zumo Shield for Arduino can be purchased on Pololu's website:
+* *"by itself":http://www.pololu.com/catalog/product/2508*;
+* as part of a *"Zumo robot kit for Arduino":http://www.pololu.com/catalog/product/2509* that also includes a "Zumo chassis":http://www.pololu.com/catalog/product/1418 and a stainless steel "Zumo blade":http://www.pololu.com/catalog/product/1410; or
+* as a fully-assembled *"Zumo robot for Arduino":http://www.pololu.com/catalog/product/2510* with "75:1 HP motors":http://www.pololu.com/catalog/product/2361 and a "reflectance sensor array":http://www.pololu.com/catalog/product/1419 installed.
+
+See the "Zumo Shield user's guide":http://www.pololu.com/docs/0J57 for more details about purchasing and assembling the hardware.
+
+h2. Using the Zumo Shield Libraries
+
+h3. ZumoMotors
+
+The ZumoMotors library provides functions for PWM-based speed (and direction) control of the two motors on the Zumo with the onboard DRV8835 dual motor driver. On Arduinos with ATmega328P, ATmega168, and ATmega32U4 microcontrollers (which include the Leonardo, Uno, and most older Arduinos), the motor control functions use hardware PWM outputs from Timer1 to generate pulse width modulation at a 20 kHz frequency.
+
+If you accidentally soldered a motor to the Zumo Shield backwards (opposite the orientation indicated in the assembly instructions), you can simply call @flipLeftMotor(true)@ and/or @flipRightMotor(true)@ to make the motors behave consistently with the directions in your code.
+
+This library comes with an example sketch demonstrating its use.
+
+h3. ZumoBuzzer
+
+The ZumoBuzzer library provides functions that allow various sounds to be played on the buzzer of the Zumo Shield, from simple beeps to complex tunes.  This library is fully compatible with the OrangutanBuzzer functions in the "Pololu AVR C/C++ Library":http://www.pololu.com/docs/0J18, so any melodies written for OrangutanBuzzer functions will also work with ZumoBuzzer functions.  This library comes with some example sketches demonstrating its use.
+
+h3. Pushbutton
+
+The Pushbutton library, which can also be found in the "pushbutton-arduino repository":https://github.com/pololu/pushbutton-arduino, provides a set of functions that are useful for detecting and debouncing pushbutton presses. While the most obvious application of this library is to work with the Zumo Shield's user pushbutton on digital pin 12, this library can be used as a general-purpose library for interfacing many types of buttons and switches to an Arduino, even without a Zumo Shield.  This library comes with an example sketch demonstrating its use.
+
+h3. ZumoReflectanceSensorArray
+
+This library provides a set of functions for reading reflectance values from a "Zumo Reflectance Sensor Array":http://www.pololu.com/catalog/product/1419.
+
+This library depends on the QTRSensors library.  The ZumoReflectanceSensorArray class is a subclass of QTRSensorsRC.  The functions provided by QTRSensorsRC can also be used on the ZumoReflectanceSensorArray class, and are documented in the "Arduino Library for the Pololu QTR Reflectance Sensors":http://www.pololu.com/docs/0J19 document on Pololu's website.
+
+This library comes with an example sketch demonstrating its use.
+
+h3. QTRSensors
+
+This library, which can also be found in the "qtr-sensors-arduino repository":https://github.com/pololu/qtr-sensors-arduino, is a general library for interfacing with "Pololu QTR reflectance sensors":http://www.pololu.com/catalog/category/123.  Since the "Zumo Reflectance Sensor Array":http://www.pololu.com/catalog/product/1419 has the same interface as the QTR RC reflectance sensors, the ZumoReflectanceSensorArray library uses QTRSensors to read the sensor array.
+
+h2. Example Projects
+
+Some additional example sketches can be found under Files->Examples->ZumoExamples in the Arduino environment. These examples demonstrate how you can program a Zumo to perform more complex and interesting tasks by combining the functionality of multiple libraries. The Example Projects section of the "Zumo Shield user's guide":http://www.pololu.com/docs/0J57 describes these examples in more detail.
+
+h2. Version History
+
+* 1.2.3 (2013-11-27): Updated examples to work with LSM303 library version 2.0.0.
+* 1.2.2 (2013-10-08): Added SumoCollisionDetect example.
+* 1.2.1 (2013-07-19): Added LineFollower example.
+* 1.2.0 (2013-06-03): Added ZumoExamples dummy library containing example projects.
+* 1.1.3 (2013-05-14): Added CompassExample.
+* 1.1.2 (2013-04-18): Pulled in QTRSensors version 2.1.0.
+* 1.1.1 (2013-04-17): Added ZumoReflectanceSensorArray constructors with parameters to call init() with and pulled fix from qtr-sensors-arduino 2.0.2.
+* 1.1.0 (2013-01-07): Added ZumoReflectanceSensorArray and QTRSensors libraries.
+* 1.0.0 (2012-11-09): Original release.
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/RunningAverage.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/RunningAverage.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/RunningAverage.h	(revision 232)
@@ -0,0 +1,93 @@
+#ifndef RunningAverage_h
+#define RunningAverage_h
+
+// RunningAverage class 
+// based on RunningAverage library for Arduino
+// source:  http://playground.arduino.cc/Main/RunningAverage
+template <typename T> 
+class RunningAverage
+{
+  public:
+    RunningAverage(void);
+    RunningAverage(int);
+    ~RunningAverage();
+    void clear();
+    void addValue(T);
+    T getAverage() const;
+    void fillValue(T, int);
+  protected:
+    int _size;
+    int _cnt;
+    int _idx;
+    T _sum;
+    T * _ar;
+    static T zero;
+};
+
+// RunningAverage class 
+// based on RunningAverage library for Arduino
+// source:  http://playground.arduino.cc/Main/RunningAverage
+// author:  Rob.Tillart@gmail.com
+// Released to the public domain
+
+template <typename T>
+T RunningAverage<T>::zero = static_cast<T>(0);
+
+template <typename T>
+RunningAverage<T>::RunningAverage(int n)
+{
+  _size = n;
+  _ar = (T*) malloc(_size * sizeof(T));
+  clear();
+}
+
+template <typename T>
+RunningAverage<T>::~RunningAverage()
+{
+  free(_ar);
+}
+
+// resets all counters
+template <typename T>
+void RunningAverage<T>::clear() 
+{ 
+  _cnt = 0;
+  _idx = 0;
+  _sum = zero;
+  for (int i = 0; i< _size; i++) _ar[i] = zero;  // needed to keep addValue simple
+}
+
+// adds a new value to the data-set
+template <typename T>
+void RunningAverage<T>::addValue(T f)
+{
+  _sum -= _ar[_idx];
+  _ar[_idx] = f;
+  _sum += _ar[_idx];
+  _idx++;
+  if (_idx == _size) _idx = 0;  // faster than %
+  if (_cnt < _size) _cnt++;
+}
+
+// returns the average of the data-set added so far
+template <typename T>
+T RunningAverage<T>::getAverage() const
+{
+  if (_cnt == 0) return zero; // NaN ?  math.h
+  return _sum / _cnt;
+}
+
+// fill the average with a value
+// the param number determines how often value is added (weight)
+// number should preferably be between 1 and size
+template <typename T>
+void RunningAverage<T>::fillValue(T value, int number)
+{
+  clear();
+  for (int i = 0; i < number; i++) 
+  {
+    addValue(value);
+  }
+}
+
+#endif
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoBuzzer.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoBuzzer.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoBuzzer.cpp	(revision 232)
@@ -0,0 +1,370 @@
+#include "ZumoBuzzer.h"
+#include <Arduino.h>
+
+#define BUZZER_PIN 6
+
+const char *buzzerSequence = 0;
+
+// declaring these globals as static means they won't conflict
+// with globals in other .cpp files that share the same name
+static volatile unsigned int buzzerTimeout = 0;    // tracks buzzer time limit
+static char play_mode_setting = PLAY_AUTOMATIC;
+
+extern volatile unsigned char buzzerFinished;  // flag: 0 while playing
+extern const char *buzzerSequence;
+
+
+static unsigned char use_program_space; // boolean: true if we should
+                    // use program space
+
+// music settings and defaults
+static unsigned char octave = 4;        // the current octave
+static unsigned int whole_note_duration = 2000;  // the duration of a whole note
+static unsigned int note_type = 4;              // 4 for quarter, etc
+static unsigned int duration = 500;        // the duration of a note in ms
+static unsigned int volume = 15;        // the note volume
+static unsigned char staccato = 0;       // true if playing staccato
+
+// staccato handling
+static unsigned char staccato_rest_duration;  // duration of a staccato
+                        //  rest, or zero if it is time
+                        //  to play a note
+
+static void nextNote();
+
+ZumoBuzzer::ZumoBuzzer()
+{
+}
+
+inline void ZumoBuzzer::init()
+{
+}
+
+void ZumoBuzzer::init2()
+{
+}
+
+void ZumoBuzzer::playFrequency(unsigned int freq, unsigned int dur, 
+                     unsigned char volume)
+{
+    tone(BUZZER_PIN, freq, dur);    
+}
+
+void ZumoBuzzer::playNote(unsigned char note, unsigned int dur,
+                 unsigned char volume)
+{
+      unsigned int freq = 0;
+  unsigned char offset_note = note - 16;
+
+  if (note == SILENT_NOTE || volume == 0)
+  {
+    freq = 1000;  // silent notes => use 1kHz freq (for cycle counter)
+    playFrequency(freq, dur, 0);
+    return;
+  }
+
+  if (note <= 16)
+    offset_note = 0;
+  else if (offset_note > 95)
+    offset_note = 95;
+
+  unsigned char exponent = offset_note / 12;
+
+  // frequency table for the lowest 12 allowed notes
+  //   frequencies are specified in tenths of a Hertz for added resolution
+  switch (offset_note - exponent * 12)  // equivalent to (offset_note % 12)
+  {
+    case 0:        // note E1 = 41.2 Hz
+      freq = 412;
+      break;
+    case 1:        // note F1 = 43.7 Hz
+      freq = 437;
+      break;
+    case 2:        // note F#1 = 46.3 Hz
+      freq = 463;
+      break;
+    case 3:        // note G1 = 49.0 Hz
+      freq = 490;
+      break;
+    case 4:        // note G#1 = 51.9 Hz
+      freq = 519;
+      break;
+    case 5:        // note A1 = 55.0 Hz
+      freq = 550;
+      break;
+    case 6:        // note A#1 = 58.3 Hz
+      freq = 583;
+      break;
+    case 7:        // note B1 = 61.7 Hz
+      freq = 617;
+      break;
+    case 8:        // note C2 = 65.4 Hz
+      freq = 654;
+      break;
+    case 9:        // note C#2 = 69.3 Hz
+      freq = 693;
+      break;
+    case 10:      // note D2 = 73.4 Hz
+      freq = 734;
+      break;
+    case 11:      // note D#2 = 77.8 Hz
+      freq = 778;
+      break;
+  }
+
+  if (exponent < 7)
+  {
+    freq = freq << exponent;  // frequency *= 2 ^ exponent
+    if (exponent > 1)      // if the frequency is greater than 160 Hz
+      freq = (freq + 5) / 10;  //   we don't need the extra resolution
+    else
+      freq += DIV_BY_10;    // else keep the added digit of resolution
+  }
+  else
+    freq = (freq * 64 + 2) / 5;  // == freq * 2^7 / 10 without int overflow
+
+  if (volume > 15)
+    volume = 15;
+  playFrequency(freq, dur, volume);  // set buzzer this freq/duration
+}
+
+// Returns 1 if the buzzer is currently playing, otherwise it returns 0
+unsigned char ZumoBuzzer::isPlaying()
+{
+    if(buzzerSequence) {
+        return 1;
+    }
+    else {
+        return 0;
+    }
+}
+
+#include "ZumoShield_cfg.h"
+#include "rca.h"
+
+void
+zumobuzzer_task(intptr_t exinf) {
+    while(buzzerSequence)  {
+        nextNote();
+    }
+}
+
+void ZumoBuzzer::play(const char *notes)
+{
+    buzzerSequence = notes;
+    use_program_space = 0;
+    staccato_rest_duration = 0;
+    act_tsk(ZUMOBUZZER_TASK);
+}
+
+void ZumoBuzzer::playFromProgramSpace(const char *notes_p)
+{
+    buzzerSequence = notes_p;
+    use_program_space = 1;
+    staccato_rest_duration = 0;
+    act_tsk(ZUMOBUZZER_TASK);
+}
+
+void ZumoBuzzer::stopPlaying()
+{
+}
+
+// Gets the current character, converting to lower-case and skipping
+// spaces.  For any spaces, this automatically increments sequence!
+static char currentCharacter()
+{
+  char c = 0;
+  do
+  {
+    if(use_program_space)
+      c = pgm_read_byte(buzzerSequence);
+    else
+      c = *buzzerSequence;
+
+    if(c >= 'A' && c <= 'Z')
+      c += 'a'-'A';
+  } while(c == ' ' && (buzzerSequence ++));
+
+  return c;
+}
+
+// Returns the numerical argument specified at buzzerSequence[0] and
+// increments sequence to point to the character immediately after the
+// argument.
+static unsigned int getNumber()
+{
+  unsigned int arg = 0;
+
+  // read all digits, one at a time
+  char c = currentCharacter();
+  while(c >= '0' && c <= '9')
+  {
+    arg *= 10;
+    arg += c-'0';
+    buzzerSequence ++;
+    c = currentCharacter();
+  }
+
+  return arg;
+}
+
+static void nextNote()
+{
+  unsigned char note = 0;
+  unsigned char rest = 0;
+  unsigned char tmp_octave = octave; // the octave for this note
+  unsigned int tmp_duration; // the duration of this note
+  unsigned int dot_add;
+
+  char c; // temporary variable
+
+  // if we are playing staccato, after every note we play a rest
+  if(staccato && staccato_rest_duration)
+  {
+    ZumoBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0);
+    staccato_rest_duration = 0;
+    return;
+  }
+
+ parse_character:
+
+  // Get current character
+  c = currentCharacter();
+  buzzerSequence ++;
+
+  // Interpret the character.
+  switch(c)
+  {
+  case '>':
+    // shift the octave temporarily up
+    tmp_octave ++;
+    goto parse_character;
+  case '<':
+    // shift the octave temporarily down
+    tmp_octave --;
+    goto parse_character;
+  case 'a':
+    note = NOTE_A(0);
+    break;
+  case 'b':
+    note = NOTE_B(0);
+    break;
+  case 'c':
+    note = NOTE_C(0);
+    break;
+  case 'd':
+    note = NOTE_D(0);
+    break;
+  case 'e':
+    note = NOTE_E(0);
+    break;
+  case 'f':
+    note = NOTE_F(0);
+    break;
+  case 'g':
+    note = NOTE_G(0);
+    break;
+  case 'l':
+    // set the default note duration
+    note_type = getNumber();
+    duration = whole_note_duration/note_type;
+    goto parse_character;
+  case 'm':
+    // set music staccato or legato
+    if(currentCharacter() == 'l')
+      staccato = false;
+    else
+    {
+      staccato = true;
+      staccato_rest_duration = 0;
+    }
+    buzzerSequence ++;
+    goto parse_character;
+  case 'o':
+    // set the octave permanently
+    octave = getNumber();
+    tmp_octave = octave;
+    goto parse_character;
+  case 'r':
+    // Rest - the note value doesn't matter.
+    rest = 1;
+    break;
+  case 't':
+    // set the tempo
+    whole_note_duration = 60*400/getNumber()*10;
+    duration = whole_note_duration/note_type;
+    goto parse_character;
+  case 'v':
+    // set the volume
+    volume = getNumber();
+    goto parse_character;
+  case '!':
+    // reset to defaults
+    octave = 4;
+    whole_note_duration = 2000;
+    note_type = 4;
+    duration = 500;
+    volume = 15;
+    staccato = 0;
+    // reset temp variables that depend on the defaults
+    tmp_octave = octave;
+    tmp_duration = duration;
+    goto parse_character;
+  default:
+    buzzerSequence = 0;
+    return;
+  }
+
+  note += tmp_octave*12;
+
+  // handle sharps and flats
+  c = currentCharacter();
+  while(c == '+' || c == '#')
+  {
+    buzzerSequence ++;
+    note ++;
+    c = currentCharacter();
+  }
+  while(c == '-')
+  {
+    buzzerSequence ++;
+    note --;
+    c = currentCharacter();
+  }
+
+  // set the duration of just this note
+  tmp_duration = duration;
+
+  // If the input is 'c16', make it a 16th note, etc.
+  if(c > '0' && c < '9')
+    tmp_duration = whole_note_duration/getNumber();
+
+  // Handle dotted notes - the first dot adds 50%, and each
+  // additional dot adds 50% of the previous dot.
+  dot_add = tmp_duration/2;
+  while(currentCharacter() == '.')
+  {
+    buzzerSequence ++;
+    tmp_duration += dot_add;
+    dot_add /= 2;
+  }
+
+  if(staccato)
+  {
+    staccato_rest_duration = tmp_duration / 2;
+    tmp_duration -= staccato_rest_duration;
+  }
+  
+  // this will re-enable the timer1 overflow interrupt
+  ZumoBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume);
+  delay(tmp_duration*1.5);
+}
+
+void ZumoBuzzer::playMode(unsigned char mode)
+{
+}
+
+unsigned char ZumoBuzzer::playCheck()
+{
+    return 0;    
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoBuzzer.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoBuzzer.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoBuzzer.h	(revision 232)
@@ -0,0 +1,345 @@
+/*! \file ZumoBuzzer.h
+ *
+ * See the ZumoBuzzer class reference for more information about this library.
+ *
+ * \class ZumoBuzzer ZumoBuzzer.h
+ * \brief Play beeps and music with buzzer
+ *
+ * The ZumoBuzzer library allows various sounds to be played through the buzzer
+ * on the Zumo Shield, from simple beeps to complex tunes. The buzzer is
+ * controlled using a PWM output of Timer 2 (on the Arduino Uno and other
+ * ATmega328/168 boards) or Timer 4 (on the Arduino Leonardo and other
+ * ATmega32U4 boards), so it will conflict with any other uses of that timer.
+ *
+ * Note durations are timed using a timer overflow interrupt
+ * (`TIMER2_OVF`/`TIMER4_OVF`), which will briefly interrupt execution of your
+ * main program at the frequency of the sound being played. In most cases, the
+ * interrupt-handling routine is very short (several microseconds). However,
+ * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode)
+ * with the `play()` command, this interrupt takes much longer than normal
+ * (perhaps several hundred microseconds) every time it starts a new note. It is
+ * important to take this into account when writing timing-critical code.
+ *
+ * The ZumoBuzzer library is fully compatible with the OrangutanBuzzer functions
+ * in the [Pololu AVR C/C++ Library](http://www.pololu.com/docs/0J18), so any
+ * sequences and melodies written for OrangutanBuzzer functions will also work
+ * with the equivalent ZumoBuzzer functions.
+ */
+
+#ifndef ZumoBuzzer_h
+#define ZumoBuzzer_h
+
+#define PLAY_AUTOMATIC 0
+#define PLAY_CHECK     1
+
+//                                             n
+// Equal Tempered Scale is given by f  = f  * a
+//                                   n    o
+//
+//  where f  is chosen as A above middle C (A4) at f  = 440 Hz
+//         o                                        o
+//  and a is given by the twelfth root of 2 (~1.059463094359)
+
+/*! \anchor note_macros
+ *
+ * \name Note Macros
+ * \a x specifies the octave of the note
+ * @{
+ */
+#define NOTE_C(x)       ( 0 + (x)*12)
+#define NOTE_C_SHARP(x) ( 1 + (x)*12)
+#define NOTE_D_FLAT(x)  ( 1 + (x)*12)
+#define NOTE_D(x)       ( 2 + (x)*12)
+#define NOTE_D_SHARP(x) ( 3 + (x)*12)
+#define NOTE_E_FLAT(x)  ( 3 + (x)*12)
+#define NOTE_E(x)       ( 4 + (x)*12)
+#define NOTE_F(x)       ( 5 + (x)*12)
+#define NOTE_F_SHARP(x) ( 6 + (x)*12)
+#define NOTE_G_FLAT(x)  ( 6 + (x)*12)
+#define NOTE_G(x)       ( 7 + (x)*12)
+#define NOTE_G_SHARP(x) ( 8 + (x)*12)
+#define NOTE_A_FLAT(x)  ( 8 + (x)*12)
+#define NOTE_A(x)       ( 9 + (x)*12)
+#define NOTE_A_SHARP(x) (10 + (x)*12)
+#define NOTE_B_FLAT(x)  (10 + (x)*12)
+#define NOTE_B(x)       (11 + (x)*12)
+
+/*! \brief silences buzzer for the note duration */
+#define SILENT_NOTE   0xFF
+
+/*! \brief frequency bit that indicates Hz/10<br>
+ * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz
+ */
+#define DIV_BY_10     (1 << 15)
+/*! @} */
+
+class ZumoBuzzer
+{
+  public:
+
+    // constructor
+  ZumoBuzzer();
+
+  /*! \brief Plays the specified frequency for the specified duration.
+   *
+   * \param freq     Frequency to play in Hz (or 0.1 Hz if the `DIV_BY_10` bit
+   *                 is set).
+   * \param duration Duration of the note in milliseconds.
+   * \param volume   Volume of the note (0--15).
+   *
+   * The \a frequency argument must be between 40 Hz and 10 kHz. If the most
+   * significant bit of \a frequency is set, the frequency played is the value
+   * of the lower 15 bits of \a frequency in units of 0.1 Hz. Therefore, you can
+   * play a frequency of 44.5 Hz by using a \a frequency of `(DIV_BY_10 | 445)`.
+   * If the most significant bit of \a frequency is not set, the units for
+   * frequency are Hz. The \a volume argument controls the buzzer volume, with
+   * 15 being the loudest and 0 being the quietest. A \a volume of 15 supplies 
+   * the buzzer with a 50% duty cycle PWM at the specified \a frequency.
+   * Lowering \a volume by one halves the duty cycle (so 14 gives a 25% duty
+   * cycle, 13 gives a 12.5% duty cycle, etc). The volume control is somewhat 
+   * crude (especially on the ATmega328/168) and should be thought of as a bonus
+   * feature.
+   *
+   * This function plays the note in the background while your program continues
+   * to execute. If you call another buzzer function while the note is playing,
+   * the new function call will overwrite the previous and take control of the
+   * buzzer. If you want to string notes together, you should either use the
+   * `play()` function or put an appropriate delay after you start a note
+   * playing. You can use the `is_playing()` function to figure out when the
+   * buzzer is through playing its note or melody.
+   *
+   * ### Example ###
+   *
+   * ~~~{.ino}
+   * ZumoBuzzer buzzer;
+   *
+   * ...
+   *
+   * // play a 6 kHz note for 250 ms at a lower volume
+   * buzzer.playFrequency(6000, 250, 12);
+   *
+   * // wait for buzzer to finish playing the note
+   * while (buzzer.isPlaying());
+   *
+   * // play a 44.5 Hz note for 1 s at full volume
+   * buzzer.playFrequency(DIV_BY_10 | 445, 1000, 15);
+   * ~~~
+   *
+   * \warning \a frequency &times; \a duration / 1000 must be no greater than
+     0xFFFF (65535). This means you can't use a max duration of 65535 ms for
+     frequencies greater than 1 kHz. For example, the maximum duration you can
+     use for a frequency of 10 kHz is 6553 ms. If you use a duration longer than
+     this, you will produce an integer overflow that can result in unexpected
+     behavior.
+   */
+  static void playFrequency(unsigned int freq, unsigned int duration,
+                unsigned char volume);
+
+  /*! \brief Plays the specified note for the specified duration.
+   *
+   *  \param note     Note to play (see \ref note_macros "Note Macros").
+   *  \param duration Duration of the note in milliseconds.
+   *  \param volume   Volume of the note (0--15).
+   *
+   * The \a note argument is an enumeration for the notes of the equal tempered
+   * scale (ETS). See \ref note_macros "Note Macros" for more information. The
+   * \a volume argument controls the buzzer volume, with 15 being the loudest 
+   * and 0 being the quietest. A \a volume of 15 supplies the buzzer with a 50% 
+   * duty cycle PWM at the specified \a frequency. Lowering \a volume by one 
+   * halves the duty cycle (so 14 gives a 25% duty cycle, 13 gives a 12.5% duty 
+   * cycle, etc). The volume control is somewhat crude (especially on the
+   * ATmega328/168) and should be thought of as a bonus feature.
+   *
+   * This function plays the note in the background while your program continues
+   * to execute. If you call another buzzer function while the note is playing,
+   * the new function call will overwrite the previous and take control of the
+   * buzzer. If you want to string notes together, you should either use the
+   * `play()` function or put an appropriate delay after you start a note
+   * playing. You can use the `is_playing()` function to figure out when the
+   * buzzer is through playing its note or melody.
+   */
+  static void playNote(unsigned char note, unsigned int duration,
+          unsigned char volume);
+
+  /*! \brief Plays the specified sequence of notes.
+   *
+   *  \param sequence Char array containing a sequence of notes to play.
+   *
+   * If the play mode is `PLAY_AUTOMATIC` (default), the sequence of notes will
+   * play with no further action required by the user. If the play mode is
+   * `PLAY_CHECK`, the user will need to call `playCheck()` in the main loop to
+   * initiate the playing of each new note in the sequence. The play mode can be
+   * changed while the sequence is playing. The sequence syntax is modeled after
+   * the PLAY commands in GW-BASIC, with just a few differences.
+   *
+   * The notes are specified by the characters **C**, **D**, **E**, **F**,
+   * **G**, **A**, and **B**, and they are played by default as "quarter notes"
+   * with a length of 500 ms. This corresponds to a tempo of 120 beats/min.
+   * Other durations can be specified by putting a number immediately after the
+   * note. For example, C8 specifies C played as an eighth note, with half the
+   * duration of a quarter note. The special note **R** plays a rest (no sound).
+   * The sequence parser is case-insensitive and ignores spaces, which may be
+   * used to format your music nicely.
+   *
+   * Various control characters alter the sound:
+   * <table>
+   * <tr><th>Control character(s)</th><th>Effect</th></tr>
+   * <tr><td><strong>A--G</strong></td>
+   *     <td>Specifies a note that will be played.</td></tr>
+   * <tr><td><strong>R</strong></td>
+   *     <td>Specifies a rest (no sound for the duration of the note).</td></tr>
+   * <tr><td><strong>+</strong></strong> or <strong>#</strong> after a note</td>
+   *     <td>Raises the preceding note one half-step.</td></tr>
+   * <tr><td><strong>-</strong> after a note</td>
+   *     <td>Lowers the preceding note one half-step.</td></tr>
+   * <tr><td><strong>1--2000</strong> after a note</td>
+   *     <td>Determines the duration of the preceding note. For example, C16
+   *         specifies C played as a sixteenth note (1/16th the length of a
+   *         whole note).</td></tr>
+   * <tr><td><strong>.</strong> after a note</td>
+   *     <td>"Dots" the preceding note, increasing the length by 50%. Each
+   *         additional dot adds half as much as the previous dot, so that "A.."
+   *         is 1.75 times the length of "A".</td></tr>
+   * <tr><td><strong>></strong> before a note</td>
+   *     <td>Plays the following note one octave higher.</td></tr>
+   * <tr><td><strong><</strong> before a note</td>
+   *     <td>Plays the following note one octave lower.</td></tr>
+   * <tr><td><strong>O</strong> followed by a number</td>
+   *     <td>Sets the octave. (default: **O4**)</td></tr>
+   * <tr><td><strong>T</strong> followed by a number</td>
+   *     <td>Sets the tempo in beats per minute (BPM). (default: **T120**)</td></tr>
+   * <tr><td><strong>L</strong> followed by a number</td>
+   *     <td>Sets the default note duration to the type specified by the number:
+   *         4 for quarter notes, 8 for eighth notes, 16 for sixteenth notes,
+   *         etc. (default: **L4**)</td></tr>
+   * <tr><td><strong>V</strong> followed by a number</td>
+   *     <td>Sets the music volume (0--15). (default: **V15**)</td></tr>
+   * <tr><td><strong>MS</strong></td>
+   *     <td>Sets all subsequent notes to play play staccato -- each note is
+   *     played for 1/2 of its allotted time, followed by an equal period of
+   *     silence.</td></tr>
+   * <tr><td><strong>ML</strong></td>
+   *     <td>Sets all subsequent notes to play legato -- each note is played for
+   *     full length. This is the default setting.</td></tr>
+   * <tr><td><strong>!</strong></td>
+   *     <td>Resets the octave, tempo, duration, volume, and staccato setting to
+   *     their default values. These settings persist from one `play()` to the
+   *     next, which allows you to more conveniently break up your music into
+   *     reusable sections.</td></tr>
+   * </table>
+   *
+   * This function plays the string of notes in the background while your
+   * program continues to execute. If you call another buzzer function while the
+   * melody is playing, the new function call will overwrite the previous and
+   * take control of the buzzer. If you want to string melodies together, you
+   * should put an appropriate delay after you start a melody playing. You can
+   * use the `is_playing()` function to figure out when the buzzer is through
+   * playing the melody.
+   *
+   * ### Example ###
+   *
+   * ~~~{.ino}
+   * ZumoBuzzer buzzer;
+   *
+   * ...
+   *
+   * // play a C major scale up and back down:
+   * buzzer.play("!L16 V8 cdefgab>cbagfedc");
+   * while (buzzer.isPlaying());
+   *
+   * // the first few measures of Bach's fugue in D-minor
+   * buzzer.play("!T240 L8 agafaea dac+adaea fa<aa<bac#a dac#adaea f4");
+   * ~~~
+   */
+  static void play(const char *sequence);
+
+  /*! \brief Plays the specified sequence of notes from program space.
+   *
+   * \param sequence Char array in program space containing a sequence of notes
+   *                 to play.
+   *
+   * A version of `play()` that takes a pointer to program space instead of RAM.
+   * This is desirable since RAM is limited and the string must be in program
+   * space anyway.
+   *
+   * ### Example ###
+   *
+   * ~~~{.ino}
+   * #include <avr/pgmspace.h>
+   *
+   * ZumoBuzzer buzzer;
+   * const char melody[] PROGMEM = "!L16 V8 cdefgab>cbagfedc";
+   *
+   * ...
+   *
+   * buzzer.playFromProgramSpace(melody);
+   * ~~~
+   */
+  static void playFromProgramSpace(const char *sequence_p);
+
+  /*! \brief Controls whether `play()` sequence is played automatically or
+   *         must be driven with `playCheck()`.
+   *
+   * \param mode Play mode (either `PLAY_AUTOMATIC` or `PLAY_CHECK`).
+   *
+   * This method lets you determine whether the notes of the `play()` sequence
+   * are played automatically in the background or are driven by the
+   * `play_check()` method. If \a mode is `PLAY_AUTOMATIC`, the sequence will
+   * play automatically in the background, driven by the timer overflow
+   * interrupt. The interrupt will take a considerable amount of time to execute
+   * when it starts the next note in the sequence playing, so it is recommended
+   * that you do not use automatic-play if you cannot tolerate being interrupted
+   * for more than a few microseconds. If \a mode is `PLAY_CHECK`, you can
+   * control when the next note in the sequence is played by calling the
+   * `play_check()` method at acceptable points in your main loop. If your main
+   * loop has substantial delays, it is recommended that you use automatic-play
+   * mode rather than play-check mode. Note that the play mode can be changed
+   * while the sequence is being played. The mode is set to `PLAY_AUTOMATIC` by
+   * default.
+   */
+  static void playMode(unsigned char mode);
+
+  /*! \brief Starts the next note in a sequence, if necessary, in `PLAY_CHECK`
+   *         mode.
+   *
+   *  \return 0 if sequence is complete, 1 otherwise.
+   *
+   * This method only needs to be called if you are in `PLAY_CHECK` mode. It
+   * checks to see whether it is time to start another note in the sequence
+   * initiated by `play()`, and starts it if so. If it is not yet time to start
+   * the next note, this method returns without doing anything. Call this as
+   * often as possible in your main loop to avoid delays between notes in the
+   * sequence. This method returns 0 (false) if the melody to be played is
+   * complete, otherwise it returns 1 (true).
+   */
+  static unsigned char playCheck();
+
+  /*! \brief Checks whether a note, frequency, or sequence is being played.
+   *
+   * \return 1 if the buzzer is current playing a note, frequency, or sequence;
+   *         0 otherwise.
+   *
+   * This method returns 1 (true) if the buzzer is currently playing a
+   * note/frequency or if it is still playing a sequence started by `play()`.
+   * Otherwise, it returns 0 (false). You can poll this method to determine when
+   * it's time to play the next note in a sequence, or you can use it as the
+   * argument to a delay loop to wait while the buzzer is busy.
+   */
+  static unsigned char isPlaying();
+
+  /*! \brief Stops any note, frequency, or melody being played.
+   *
+   * This method will immediately silence the buzzer and terminate any
+   * note/frequency/melody that is currently playing.
+   */
+  static void stopPlaying();
+
+
+  private:
+
+  // initializes timer for buzzer control
+  static void init2();
+  static void init();
+};
+
+#endif
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoMotors.cpp
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoMotors.cpp	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoMotors.cpp	(revision 232)
@@ -0,0 +1,114 @@
+#include "ZumoMotors.h"
+
+#define PWM_L 10
+#define PWM_R 9
+#define DIR_L 8
+#define DIR_R 7
+
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined (__AVR_ATmega32U4__)
+  #define USE_20KHZ_PWM
+#endif
+
+static boolean flipLeft = false;
+static boolean flipRight = false;
+
+// constructor (doesn't do anything)
+ZumoMotors::ZumoMotors()
+{
+}
+
+// initialize timer1 to generate the proper PWM outputs to the motor drivers
+void ZumoMotors::init2()
+{
+  pinMode(PWM_L,  OUTPUT);
+  pinMode(PWM_R,  OUTPUT);
+  pinMode(DIR_L, OUTPUT);
+  pinMode(DIR_R, OUTPUT);
+
+#ifdef USE_20KHZ_PWM
+  // Timer 1 configuration
+  // prescaler: clockI/O / 1
+  // outputs enabled
+  // phase-correct PWM
+  // top of 400
+  //
+  // PWM frequency calculation
+  // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
+  TCCR1A = 0b10100000;
+  TCCR1B = 0b00010001;
+  ICR1 = 400;
+#endif
+}
+
+// enable/disable flipping of left motor
+void ZumoMotors::flipLeftMotor(boolean flip)
+{
+  flipLeft = flip;
+}
+
+// enable/disable flipping of right motor
+void ZumoMotors::flipRightMotor(boolean flip)
+{
+  flipRight = flip;
+}
+
+// set speed for left motor; speed is a number between -400 and 400
+void ZumoMotors::setLeftSpeed(int speed)
+{
+  init(); // initialize if necessary
+    
+  boolean reverse = 0;
+  
+  if (speed < 0)
+  {
+    speed = -speed; // make speed a positive quantity
+    reverse = 1;    // preserve the direction
+  }
+  if (speed > 400)  // Max 
+    speed = 400;
+    
+#ifdef USE_20KHZ_PWM
+  OCR1B = speed;
+#else
+  analogWrite(PWM_L, speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
+#endif 
+
+  if (reverse ^ flipLeft) // flip if speed was negative or flipLeft setting is active, but not both
+    digitalWrite(DIR_L, HIGH);
+  else
+    digitalWrite(DIR_L, LOW);
+}
+
+// set speed for right motor; speed is a number between -400 and 400
+void ZumoMotors::setRightSpeed(int speed)
+{
+  init(); // initialize if necessary
+    
+  boolean reverse = 0;
+  
+  if (speed < 0)
+  {
+    speed = -speed;  // Make speed a positive quantity
+    reverse = 1;  // Preserve the direction
+  }
+  if (speed > 400)  // Max PWM dutycycle
+    speed = 400;
+    
+#ifdef USE_20KHZ_PWM
+  OCR1A = speed;
+#else
+  analogWrite(PWM_R, speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
+#endif
+
+  if (reverse ^ flipRight) // flip if speed was negative or flipRight setting is active, but not both
+    digitalWrite(DIR_R, HIGH);
+  else
+    digitalWrite(DIR_R, LOW);
+}
+
+// set speed for both motors
+void ZumoMotors::setSpeeds(int leftSpeed, int rightSpeed)
+{
+  setLeftSpeed(leftSpeed);
+  setRightSpeed(rightSpeed);
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoMotors.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoMotors.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoMotors.h	(revision 232)
@@ -0,0 +1,48 @@
+/*! \file ZumoMotors.h
+ *
+ * See the ZumoMotors class reference for more information about this library.
+ *
+ * \class ZumoMotors ZumoMotors.h
+ * \brief Control motor speed and direction
+ * 
+ */
+
+#ifndef ZumoMotors_h
+#define ZumoMotors_h
+
+#include <Arduino.h>
+
+class ZumoMotors
+{
+  public:  
+  
+    // constructor (doesn't do anything)
+    ZumoMotors();
+    
+    // enable/disable flipping of motors
+    static void flipLeftMotor(boolean flip);
+    static void flipRightMotor(boolean flip);
+    
+    // set speed for left, right, or both motors
+    static void setLeftSpeed(int speed);
+    static void setRightSpeed(int speed);
+    static void setSpeeds(int leftSpeed, int rightSpeed);
+    
+  private:
+
+    static inline void init()
+    {
+      static boolean initialized = false;
+
+      if (!initialized)
+      {
+        initialized = true;
+        init2();
+      }
+    }
+    
+    // initializes timer1 for proper PWM generation
+    static void init2();
+};
+
+#endif
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoReflectanceSensorArray.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoReflectanceSensorArray.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoReflectanceSensorArray.h	(revision 232)
@@ -0,0 +1,408 @@
+/*! \file ZumoReflectanceSensorArray.h
+ *
+ * See the ZumoReflectanceSensorArray class reference for more information about
+ * this library.
+ *
+ * \class ZumoReflectanceSensorArray ZumoReflectanceSensorArray.h
+ * \brief Read from reflectance sensor array
+ *
+ * The ZumoReflectanceSensorArray library provides an interface for using a
+ * [Zumo Reflectance Sensor Array](http://www.pololu.com/product/1419) connected
+ * to a Zumo robot. The library provides access to the raw sensor values as well
+ * as to high level functions including calibration and line-tracking.
+ *
+ * For calibration, memory is allocated using the `malloc()` function. This
+ * conserves RAM: if all six sensors are calibrated with the emitters both on
+ * and off, a total of 48 bytes is dedicated to storing calibration values.
+ * However, for an application where only two sensors are used, and the
+ * emitters are always on during reads, only 4 bytes are required.
+ *
+ * Internally, this library uses all standard Arduino functions such as
+ * `micros()` for timing and `digitalRead()` for getting the sensor values, so
+ * it should work on all Arduinos without conflicting with other libraries.
+ *
+ * ### Calibration ###
+ *
+ * This library allows you to use the `calibrate()` method to easily calibrate
+ * your sensors for the particular conditions it will encounter. Calibrating
+ * your sensors can lead to substantially more reliable sensor readings, which
+ * in turn can help simplify your code. As such, we recommend you build a
+ * calibration phase into your Zumo's initialization routine. This can be as
+ * simple as a fixed duration over which you repeatedly call the `calibrate()`
+ * method.
+ *
+ * During this calibration phase, you will need to expose each of your
+ * reflectance sensors to the lightest and darkest readings they will encounter.
+ * For example, if your Zumo is programmed to be a line follower, you will want
+ * to slide it across the line during the calibration phase so the each sensor
+ * can get a reading of how dark the line is and how light the ground is (or you
+ * can program it to automatically turn back and forth to pass all of the
+ * sensors over the line). The **SensorCalibration** example included with this
+ * library demonstrates a calibration routine.
+ *
+ * ### Reading the sensors
+ *
+ *
+ * This library gives you a number of different ways to read the sensors.
+ *
+ * - You can request raw sensor values using the `read()` method.
+ *
+ * - You can request calibrated sensor values using the `readCalibrated()`
+ *   method. Calibrated sensor values will always range from 0 to 1000, with the
+ *   extreme values corresponding to the most and least reflective surfaces
+ *   encountered during calibration.
+ *
+ * - For line-detection applications, you can request the line location using
+ *   the `readLine()` method. This function provides calibrated values
+ *   for each sensor and returns an integer that tells you where it thinks the
+ *   line is.
+ *
+ * ### Class Inheritance ###
+ *
+ * The ZumoReflectanceSensorArray class is derived from the QTRSensorsRC class,
+ * which is in turn derived from the QTRSensors base class. The QTRSensorsRC and
+ * QTRSensors classes are part of the \ref QTRSensors.h "QTRSensors" library,
+ * which provides more general functionality for working with reflectance
+ * sensors and is included in the Zumo Shield libraries as a dependency for this
+ * library.
+ *
+ * We recommend using the ZumoReflectanceSensorArray library instead of
+ * the \ref QTRSensors.h "QTRSensors" library when programming an Arduino on a
+ * Zumo. For documentation specific to the %QTRSensors library, please see its
+ * [user's guide](http://www.pololu.com/docs/0J19) on Pololu's website.
+ */
+
+#ifndef ZumoReflectanceSensorArray_h
+#define ZumoReflectanceSensorArray_h
+
+#include <QTRSensors.h>
+#include <Arduino.h>
+
+#if defined(__AVR_ATmega32U4__)
+  // Arduino Leonardo
+  #define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN  A4
+#else
+  // Arduino UNO and other ATmega328P/168 Arduinos
+  #define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN  2
+#endif
+
+class ZumoReflectanceSensorArray : public QTRSensorsRC
+{
+  public:
+
+  /*! \brief Minimal constructor.
+   *
+   * This version of the constructor performs no initialization. If it is used,
+   * the user must call init() before using the methods in this class.
+   */
+  ZumoReflectanceSensorArray() {}
+
+  /*! \brief Constructor; initializes with given emitter pin and defaults for
+   *         other settings.
+   *
+   * \param emitterPin Pin that turns IR emitters on or off.
+   *
+   * This constructor calls `init(unsigned char emitterPin)` with the specified
+   * emitter pin and default values for other settings.
+   */
+  ZumoReflectanceSensorArray(unsigned char emitterPin)
+  {
+    init(emitterPin);
+  }
+
+  /*! \brief Constructor; initializes with all settings as given.
+   *
+   * \param pins       Array of pin numbers for sensors.
+   * \param numSensors Number of sensors.
+   * \param timeout    Maximum duration of reflectance reading in microseconds.
+   * \param emitterPin Pin that turns IR emitters on or off.
+   *
+   * This constructor calls `init(unsigned char * pins, unsigned char
+   * numSensors, unsigned int timeout, unsigned char emitterPin)` with all
+   * settings as given.
+   */
+  ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+    unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+  {
+    QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+  }
+
+  /*! \brief Initializes with given emitter pin and and defaults for other
+   *         settings.
+   *
+   * \param emitterPin Pin that turns IR emitters on or off.
+   *
+   * This function initializes the ZumoReflectanceSensorArray object with the
+   * specified emitter pin. The other settings are set to default values: all
+   * six sensors on the array are active, and a timeout of 2000 microseconds is
+   * used.
+   *
+   * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+   * are on or off. This pin is optional; if a valid pin is specified, the
+   * emitters will only be turned on during a reading. If \a emitterPin is not
+   * specified, the emitters will be controlled with pin 2 on the Uno (and other
+   * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+   * boards). (The "LED ON" jumper on the Zumo Reflectance Sensor Array must be
+   * configured correctly for this to work.) If the value `QTR_NO_EMITTER_PIN`
+   * (255) is used, you can leave the emitter pin disconnected and the IR
+   * emitters will always be on.
+   */
+  void init(unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+  {
+    unsigned char sensorPins[] = { 4, A3, 11, A0, A2, 5 };
+    QTRSensorsRC::init(sensorPins, sizeof(sensorPins), 2000, emitterPin);
+  }
+
+  /*! \brief Initializes with all settings as given.
+   *
+   * \param pins       Array of pin numbers for sensors.
+   * \param numSensors Number of sensors.
+   * \param timeout    Maximum duration of reflectance reading in microseconds.
+   * \param emitterPin Pin that turns IR emitters on or off.
+   *
+   * This function initializes the ZumoReflectanceSensorArray object with all
+   * settings as given.
+   *
+   * The array \a pins should contain the Arduino digital pin numbers for each
+   * sensor.
+   *
+   * \a numSensors specifies the length of the \a pins array (the number of
+   * reflectance sensors you are using).
+   *
+   * \a timeout specifies the length of time in microseconds beyond which you
+   * consider the sensor reading completely black. That is to say, if the pulse
+   * length for a pin exceeds \a timeout, pulse timing will stop and the reading
+   * for that pin will be considered full black. It is recommended that you set
+   * \a timeout to be between 1000 and 3000 us, depending on factors like the
+   * height of your sensors and ambient lighting. This allows you to shorten the
+   * duration of a sensor-reading cycle while maintaining useful measurements of
+   * reflectance. If \a timeout is not specified, it defaults to 2000 us. (See
+   * the [product page](http://www.pololu.com/product/1419) for the Zumo
+   * Reflectance Sensor Array on Pololu's website for an overview of the
+   * sensors' principle of operation.)
+   *
+   * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+   * are on or off. This pin is optional; if a valid pin is specified, the
+   * emitters will only be turned on during a reading. If \a emitterPin is not
+   * specified, the emitters will be controlled with pin 2 on the Uno (and other
+   * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+   * boards). (The corresponding connection should be made with the "LED ON"
+   * jumper on the Zumo Reflectance Sensor Array.) If the value
+   * `QTR_NO_EMITTER_PIN` (255) is used, you can leave the emitter pin
+   * disconnected and the IR emitters will always be on.
+   *
+   * This version of `%init()` can be useful if you only want to use a subset
+   * of the six reflectance sensors on the array. For example, using the
+   * outermost two sensors (on pins 4 and 5 by default) is usually enough for
+   * detecting the ring border in sumo competitions:
+   *
+   * ~~~{.ino}
+   * ZumoReflectanceSensorArray reflectanceSensors;
+   *
+   * ...
+   *
+   * reflectanceSensors.init((unsigned char[]) {4, 5}, 2);
+   * ~~~
+   *
+   * Alternatively, you can use \ref ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+   * "a different constructor" to declare and initialize the object at the same
+   * time:
+   *
+   * ~~~{.ino}
+   *
+   * ZumoReflectanceSensorArray reflectanceSensors((unsigned char[]) {4, 5}, 2);
+   * ~~~
+   *
+   */
+  void init(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+    unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+  {
+    QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+  }
+};
+
+// documentation for inherited functions
+
+/*! \fn void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+\memberof ZumoReflectanceSensorArray
+ * \brief Reads the raw sensor values into an array.
+ *
+ * \param sensorValues Array to populate with sensor readings.
+ * \param readMode     Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ *                     `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * There **must** be space in the \a sensorValues array for as many values as
+ * there were sensors specified in the constructor. The values returned are
+ * measures of the reflectance in units of microseconds. They will be raw
+ * readings between 0 and the \a timeout argument (in units of microseconds)
+ * provided in the constructor (which defaults to 2000).
+ *
+ * The \a readMode argument specifies the kind of read that will be performed.
+ * Several options are defined:
+ *
+ * - `QTR_EMITTERS_OFF` specifies that the reading should be made without
+ *   turning on the infrared (IR) emitters, in which case the reading represents
+ *   ambient light levels near the sensor.
+ * - `QTR_EMITTERS_ON` specifies that the emitters should be turned on for the
+ *   reading, which results in a measure of reflectance.
+ * - `QTR_EMITTERS_ON_AND_OFF` specifies that a reading should be made in both
+ *   the on and off states. The values returned when this option is used are
+ *   given by the formula **on + max &minus; off**, where **on** is the reading
+ *   with the emitters on, **off** is the reading with the emitters off, and
+ *   **max** is the maximum sensor reading. This option can reduce the amount of
+ *   interference from uneven ambient lighting.
+ *
+ * Note that emitter control will only work if you specify a valid emitter pin
+ * in the constructor and make the corresponding connection (with the "LED ON"
+ * jumper or otherwise).
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOff()
+ * \brief Turns the IR LEDs off.
+ *
+ * This is mainly for use by the `read()` method, and calling this function
+ * before or after reading the sensors will have no effect on the readings, but
+ * you might wish to use it for testing purposes. This method will only do
+ * something if the emitter pin specified in the constructor is valid (i.e. not
+ * `QTR_NO_EMITTER_PIN`) and the corresponding connection is made.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOn()
+ * \brief Turns the IR LEDs on.
+ * \copydetails emittersOff
+ */
+
+/*! \fn void QTRSensors::calibrate(unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Reads the sensors for calibration.
+ *
+ * \param readMode     Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ *                     `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * The sensor values read by this function are not returned; instead, the
+ * maximum and minimum values found over time are stored internally and used for
+ * the `readCalibrated()` method. You can access the calibration (i.e raw max
+ * and min sensor readings) through the public member pointers
+ * `calibratedMinimumOn`, `calibratedMaximumOn`, `calibratedMinimumOff`, and
+ * `calibratedMaximumOff`. Note that these pointers will point to arrays of
+ * length \a numSensors, as specified in the constructor, and they will only be
+ * allocated **after** `calibrate()` has been called. If you only calibrate with
+ * the emitters on, the calibration arrays that hold the off values will not be
+ * allocated.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::resetCalibration()
+ * \brief Resets all calibration that has been done.
+ *
+ * This function discards the calibration values that have been previously
+ * recorded, resetting the min and max values.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Returns sensor readings normalized to values between 0 and 1000.
+ *
+ * \param sensorValues Array to populate with sensor readings.
+ * \param readMode     Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ *                     `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * 0 corresponds to a reading that is less than or equal to the minimum value
+ * read by `calibrate()` and 1000 corresponds to a reading that is greater than
+ * or equal to the maximum value. Calibration values are stored separately for
+ * each sensor, so that differences in the sensors are accounted for
+ * automatically.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn int QTRSensors::readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char whiteLine = 0)
+ * \brief Returns an estimated position of a line under the sensor array.
+ *
+ * \param sensorValues Array to populate with sensor readings.
+ * \param readMode     Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ *                     `QTR_EMITTERS_ON_AND_OFF`).
+ * \param whiteLine   0 to detect a dark line on a light surface; 1 to detect
+ *                     a light line on a dark surface.
+ * \return An estimated line position.
+ *
+ * This function operates the same as `readCalibrated()`, but with a feature
+ * designed for line following: it returns an estimated position of the line.
+ * The estimate is made using a weighted average of the sensor indices
+ * multiplied by 1000, so that a return value of 0 indicates that the line is
+ * directly below sensor 0 (or was last seen by sensor 0 before being lost), a
+ * return value of 1000 indicates that the line is directly below sensor 1, 2000
+ * indicates that it's below sensor 2, etc. Intermediate values indicate that
+ * the line is between two sensors. The formula is:
+ *
+ * \f[
+ *   \newcommand{sv}[1]{\mathtt{sensorValues[#1]}}
+ *   \text{return value} =
+ *     \frac{(0 \times \sv{0}) + (1000 \times \sv{1}) + (2000 \times \sv{2}) + \ldots}
+ *          {\sv{0} + \sv{1} + \sv{2} + \ldots}
+ * \f]
+ *
+ * As long as your sensors aren't spaced too far apart relative to the line,
+ * this returned value is designed to be monotonic, which makes it great for use
+ * in closed-loop PID control. Additionally, this method remembers where it last
+ * saw the line, so if you ever lose the line to the left or the right, its line
+ * position will continue to indicate the direction you need to go to reacquire
+ * the line. For example, if sensor 5 is your rightmost sensor and you end up
+ * completely off the line to the left, this function will continue to return
+ * 5000.
+ *
+ * By default, this function assumes a dark line (high values) on a light
+ * background (low values). If your line is light on dark, set the optional
+ * second argument \a whiteLine to true. In this case, each sensor value will be
+ * replaced by the maximum possible value minus its actual value before the
+ * averaging.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+ 
+// documentation for inherited member variables
+
+/*!
+ * \property unsigned int * QTRSensors::calibratedMinimumOn
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ *        on.
+ *
+ * This pointer is unallocated and set to 0 until `calibrate()` is called, and
+ * then allocated to exactly the size required. Depending on the \a readMode
+ * argument to `calibrate()`, only the On or Off values might be allocated, as
+ * required. This variable is made public so that you can use the calibration
+ * values for your own calculations and do things like saving them to EEPROM,
+ * performing sanity checking, etc.
+ *
+ * The ZumoReflectanceSensorArray class inherits this variable from the
+ * QTRSensors class.
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOn
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ *        on.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMinimumOff
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ *        off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOff
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ *        off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ */
+
+#endif
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield.cfg
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield.cfg	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield.cfg	(revision 232)
@@ -0,0 +1,3 @@
+#include "ZumoShield_cfg.h"
+
+CRE_TSK(ZUMOBUZZER_TASK, { TA_NULL, 0, zumobuzzer_task, 15, 512, NULL });
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield.h	(revision 232)
@@ -0,0 +1,237 @@
+#include <ZumoBuzzer.h>
+#include <ZumoMotors.h>
+#include <Pushbutton.h>
+#include <QTRSensors.h>
+#include <ZumoReflectanceSensorArray.h>
+#include <avr/pgmspace.h>
+#include <LSM303.h>
+#include <L3G.h>
+#include <RunningAverage.h>
+#include <Accelerometer.h>
+
+template <typename T> float heading(LSM303::vector<T> v, LSM303 *compass)
+{
+  float x_scaled =  2.0*(float)(v.x - compass->m_min.x) / (compass->m_max.x - compass->m_min.x) - 1.0;
+  float y_scaled =  2.0*(float)(v.y - compass->m_min.y) / (compass->m_max.y - compass->m_min.y) - 1.0;
+
+  float angle = atan2(y_scaled, x_scaled)*180 / M_PI;
+  if (angle < 0)
+    angle += 360;
+  return angle;
+}
+
+#define CRB_REG_M_2_5GAUSS 0x60 // CRB_REG_M value for magnetometer +/-2.5 gauss full scale
+#define CRA_REG_M_220HZ    0x1C // CRA_REG_M value for magnetometer 220 Hz update rate
+
+class ZumoCompass : public LSM303
+{
+  public:
+	ZumoCompass() {};
+	void begin() {
+		Wire.begin();
+		// Initiate LSM303
+		init();
+		// Enables accelerometer and magnetometer
+		enableDefault();
+		writeReg(LSM303::CRB_REG_M, CRB_REG_M_2_5GAUSS); // +/- 2.5 gauss sensitivity to hopefully avoid overflow problems
+		writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ);    // 220 Hz compass update rate  
+		// Set calibrated values to compass.m_max and compass.m_min
+		m_max.x = -4418;
+		m_max.y = 1180;
+		m_min.x = -6016;
+		m_min.y = -772;
+	};
+	
+	float averageHeading() {
+		LSM303::vector<int32_t> avg = {0, 0, 0};
+
+		for(int i = 0; i < 10; i ++)
+		  {
+			  read();
+			  avg.x += m.x;
+			  avg.y += m.y;
+		  }
+		avg.x /= 10.0;
+		avg.y /= 10.0;
+		
+		// avg is the average measure of the magnetic vector.
+		return ::heading(avg, (LSM303 *)this);
+	}
+};
+
+class ZumoGyro : public L3G
+{
+  public:
+	ZumoGyro(void){};
+
+/* turnAngle is a 32-bit unsigned integer representing the amount
+the robot has turned since the last time turnSensorReset was
+called. This is computed solely using the Z axis of the gyro, so
+it could be inaccurate if the robot is rotated about the X or Y
+axes.
+
+Our convention is that a value of 0x20000000 represents a 45
+degree counter-clockwise rotation.  This means that a uint32_t
+can represent any angle between 0 degrees and 360 degrees.  If
+you cast it to a signed 32-bit integer by writing
+(int32_t)turnAngle, that integer can represent any angle between
+-180 degrees and 180 degrees. */
+	uint32_t turnAngle = 0;
+
+	int32_t turnAngleDegree = 0;
+
+// turnRate is the current angular rate of the gyro, in units of
+// 0.07 degrees per second.
+	int16_t turnRate;
+
+// This is the average reading obtained from the gyro's Z axis
+// during calibration.
+	int16_t gyroOffset;
+
+// This variable helps us keep track of how much time has passed
+// between readings of the gyro.
+	uint16_t gyroLastUpdate = 0;
+
+	// This constant represents a turn of 45 degrees.
+	const int32_t turnAngle45 = 0x20000000;
+
+	// This constant represents a turn of 90 degrees.
+	const int32_t turnAngle90 = turnAngle45 * 2;
+
+	// This constant represents a turn of approximately 1 degree.
+	const int32_t turnAngle1 = (turnAngle45 + 22) / 45;
+
+	// This should be called to set the starting point for measuring
+	// a turn.  After calling this, turnAngle will be 0.
+	void turnSensorReset() {
+		gyroLastUpdate = micros();
+		turnAngle = 0;
+		turnAngleDegree = 0;
+	  }
+
+	// Read the gyro and update the angle.  This should be called as
+	// frequently as possible while using the gyro to do turns.
+	void turnSensorUpdate() {
+		// Read the measurements from the gyro.
+		read();
+		turnRate = g.z - gyroOffset;
+		
+		// Figure out how much time has passed since the last update (dt)
+		uint16_t m = micros();
+		uint16_t dt = m - gyroLastUpdate;
+		gyroLastUpdate = m;
+		
+		// Multiply dt by turnRate in order to get an estimation of how
+		// much the robot has turned since the last update.
+		// (angular change = angular velocity * time)
+		int32_t d = (int32_t)turnRate * dt;
+		  
+		// The units of d are gyro digits times microseconds.  We need
+		// to convert those to the units of turnAngle, where 2^29 units
+		// represents 45 degrees.  The conversion from gyro digits to
+		// degrees per second (dps) is determined by the sensitivity of
+		// the gyro: 0.07 degrees per second per digit.
+		//
+		// (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
+		// = 14680064/17578125 unit/(digit*us)
+		turnAngle += (int64_t)d * 14680064 / 17578125;
+		turnAngleDegree = ((int32_t)turnAngle / turnAngle1);
+	}
+
+	void turnSensorSetup() {
+		Wire.begin();
+		init();
+
+		// 800 Hz output data rate,
+		// low-pass filter cutoff 100 Hz
+		writeReg(L3G::CTRL1, 0b11111111);
+
+		// 2000 dps full scale
+		writeReg(L3G::CTRL4, 0b00100000);
+
+		// High-pass filter disabled
+		writeReg(L3G::CTRL5, 0b00000000);
+
+		// Delay to give the user time to remove their finger.
+		delay(500);
+
+		// Calibrate the gyro.
+		int32_t total = 0;
+		for (uint16_t i = 0; i < 1024; i++)
+		  {
+			  // Wait for new data to be available, then read it.
+			  while(!readReg(L3G::STATUS_REG) & 0x08);
+			  read();
+			  // Add the Z axis reading to the total.
+			  total += g.z;
+		  }
+		gyroOffset = total / 1024;
+
+		// Display the angle (in degrees from -180 to 180) until the
+		// user presses A.
+		turnSensorReset();   
+	};
+};
+
+class ZumoBuzzer2 : public ZumoBuzzer
+{
+  public:
+	ZumoBuzzer2(){};
+	void playOn(void) {play(">g32>>c32");noTone(6);};
+	void playStart(void) {playNote(NOTE_G(4), 500, 15);}
+	void playNum(int cnt) {
+		for (int i = 0; i < cnt; i++){
+			delay(1000);
+			playNote(NOTE_G(3), 50, 12);
+		}  
+	};
+};
+
+class ZumoLED
+{
+  public:
+	ZumoLED(){};
+	void init(){pinMode(13, OUTPUT);digitalWrite(13, LOW);};
+	void on(){digitalWrite(13, HIGH);};
+	void off(){digitalWrite(13, LOW);};
+	void set(int i){
+		digitalWrite(13, (i == 1)? HIGH : LOW);
+	};
+};
+
+class ZumoReflectanceSensorArray2 : public ZumoReflectanceSensorArray {
+  public:
+	unsigned int values[6];
+#ifndef ARDUINO_ARCH_SAMD
+	ZumoReflectanceSensorArray2() : ZumoReflectanceSensorArray(QTR_NO_EMITTER_PIN){
+		
+	};
+#else /* ARDUINO_ARCH_SAMD */
+	ZumoReflectanceSensorArray2() : ZumoReflectanceSensorArray(2){
+		
+	};    
+#endif /* !ARDUINO_ARCH_SAMD */   
+	void update(void){
+		read(values);
+	}
+	unsigned int value(int i){
+		if((i <= 6) && (i > 0)) {
+			return values[i-1];
+		}
+		return 0;
+	}
+};
+
+ZumoReflectanceSensorArray2 reflectances;
+Pushbutton  button(ZUMO_BUTTON);
+ZumoLED     led;
+ZumoBuzzer2 buzzer;
+ZumoMotors  motors;
+ZumoCompass compass;
+ZumoGyro    gyro;
+
+void
+ZumoInit(void) {
+    led.init();
+    button.init();
+}
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield_cfg.h
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield_cfg.h	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/ZumoShield_cfg.h	(revision 232)
@@ -0,0 +1,5 @@
+#ifdef __cplusplus
+extern "C" void zumobuzzer_task(intptr_t exinf);
+#else
+extern void zumobuzzer_task(intptr_t exinf);
+#endif /* __cplusplus */
Index: /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/keywords.txt
===================================================================
--- /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/keywords.txt	(revision 232)
+++ /rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/keywords.txt	(revision 232)
@@ -0,0 +1,118 @@
+#######################################
+# Syntax Coloring Map ZumoBuzzer
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+ZumoBuzzer	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+playFrequency	KEYWORD2
+playNote	KEYWORD2
+play	KEYWORD2
+playFromProgramSpace	KEYWORD2
+isPlaying	KEYWORD2
+stopPlaying	KEYWORD2
+playMode	KEYWORD2
+playCheck	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
+PLAY_AUTOMATIC	LITERAL1
+PLAY_CHECK	LITERAL1
+NOTE_C	LITERAL1
+NOTE_C_SHARP	LITERAL1
+NOTE_D_FLAT	LITERAL1
+NOTE_D	LITERAL1
+NOTE_D_SHARP	LITERAL1
+NOTE_E_FLAT	LITERAL1
+NOTE_E	LITERAL1
+NOTE_F	LITERAL1
+NOTE_F_SHARP	LITERAL1
+NOTE_G_FLAT	LITERAL1
+NOTE_G	LITERAL1
+NOTE_G_SHARP	LITERAL1
+NOTE_A_FLAT	LITERAL1
+NOTE_A	LITERAL1
+NOTE_A_SHARP	LITERAL1
+NOTE_B_FLAT	LITERAL1
+NOTE_B	LITERAL1
+SILENT_NOTE	LITERAL1
+DIV_BY_10	LITERAL1
+
+
+ZumoMotors	KEYWORD1
+
+flipLeftMotor	KEYWORD2
+flipRightMotor	KEYWORD2
+setLeftSpeed	KEYWORD2
+setRightSpeed	KEYWORD2
+setSpeeds	KEYWORD2
+
+# Syntax Coloring Map
+
+# Datatypes (KEYWORD1)
+ZumoReflectanceSensorArray	KEYWORD1
+
+# Methods and Functions (KEYWORD2)
+
+# Constants (LITERAL1)
+
+#######################################
+# Syntax Coloring Map QTRSensors
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+QTRSensorsAnalog	KEYWORD1
+QTRSensorsRC	KEYWORD1
+QTRSensors	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+read	KEYWORD2
+emittersOff	KEYWORD2
+emittersOn	KEYWORD2	
+calibrate	KEYWORD2
+readCalibrated	KEYWORD2
+readLine	KEYWORD2
+calibratedMinimumOn	KEYWORD2
+calibratedMaximumOn	KEYWORD2
+calibratedMinimumOff	KEYWORD2
+calibratedMaximumOff	KEYWORD2
+init	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
+QTR_EMITTERS_OFF	LITERAL1
+QTR_EMITTERS_ON	LITERAL1
+QTR_EMITTERS_ON_AND_OFF	LITERAL1
+QTR_NO_EMITTER_PIN	LITERAL1
+
+Pushbutton	KEYWORD1
+
+waitForPress	KEYWORD2
+waitForRelease	KEYWORD2
+waitForButton	KEYWORD2
+isPressed	KEYWORD2
+getSingleDebouncedPress	KEYWORD2
+getSingleDebouncedRelease	KEYWORD2
+
+ZUMO_BUTTON	LITERAL1
+PULL_UP_DISABLED	LITERAL1
+PULL_UP_ENABLED	LITERAL1
+DEFAULT_STATE_LOW	LITERAL1
+DEFAULT_STATE_HIGH	LITERAL1
