Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
higedura
Date:
Tue May 15 08:31:45 2012 +0000
Commit message:

Changed in this revision

ITG3200.cpp Show annotated file Show diff for this revision Revisions of this file
ITG3200.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r b89a41f096ae ITG3200.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.cpp	Tue May 15 08:31:45 2012 +0000
@@ -0,0 +1,280 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * ITG-3200 triple axis, digital interface, gyroscope.
+ *
+ * Datasheet:
+ *
+ * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "ITG3200.h"
+
+ITG3200::ITG3200(PinName sda, PinName scl) : i2c_(sda, scl) {
+
+    //400kHz, fast mode.
+    i2c_.frequency(400000);
+    
+    //Set FS_SEL to 0x03 for proper operation.
+    //See datasheet for details.
+    char tx[2];
+    tx[0] = DLPF_FS_REG;
+    //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
+    tx[1] = 0x03 << 3;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char ITG3200::getWhoAmI(void){
+
+    //WhoAmI Register address.
+    char tx = WHO_AM_I_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    return rx;
+
+}
+
+void ITG3200::setWhoAmI(char address){
+
+    char tx[2];
+    tx[0] = WHO_AM_I_REG;
+    tx[1] = address;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char ITG3200::getSampleRateDivider(void){
+
+    char tx = SMPLRT_DIV_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+    return rx;
+
+}
+
+void ITG3200::setSampleRateDivider(char divider){
+
+    char tx[2];
+    tx[0] = SMPLRT_DIV_REG;
+    tx[1] = divider;
+
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+int ITG3200::getInternalSampleRate(void){
+
+    char tx = DLPF_FS_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //DLPF_CFG == 0 -> sample rate = 8kHz.
+    if(rx == 0){
+        return 8;
+    } 
+    //DLPF_CFG = 1..7 -> sample rate = 1kHz.
+    else if(rx >= 1 && rx <= 7){
+        return 1;
+    }
+    //DLPF_CFG = anything else -> something's wrong!
+    else{
+        return -1;
+    }
+    
+}
+
+void ITG3200::setLpBandwidth(char bandwidth){
+
+    char tx[2];
+    tx[0] = DLPF_FS_REG;
+    //Bits 4,3 are required to be 0x03 for proper operation.
+    tx[1] = bandwidth | (0x03 << 3);
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char ITG3200::getInterruptConfiguration(void){
+
+    char tx = INT_CFG_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    return rx;
+
+}
+
+void ITG3200::setInterruptConfiguration(char config){
+
+    char tx[2];
+    tx[0] = INT_CFG_REG;
+    tx[1] = config;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+bool ITG3200::isPllReady(void){
+
+    char tx = INT_STATUS;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //ITG_RDY bit is bit 4 of INT_STATUS register.
+    if(rx & 0x04){
+        return true;
+    }
+    else{
+        return false;
+    }
+    
+}
+
+bool ITG3200::isRawDataReady(void){
+
+    char tx = INT_STATUS;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
+    if(rx & 0x01){
+        return true;
+    }
+    else{
+        return false;
+    }
+    
+}
+
+float ITG3200::getTemperature(void){
+
+    char tx = TEMP_OUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t temperature = ((int) rx[0] << 8) | ((int) rx[1]);
+    //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
+    return 35.0 + ((temperature + 13200)/280.0);
+    
+}
+
+int ITG3200::getGyroX(void){
+
+    char tx = GYRO_XOUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+    return output;
+
+}
+
+int ITG3200::getGyroY(void){
+
+    char tx = GYRO_YOUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+    return output;
+
+}
+
+int ITG3200::getGyroZ(void){
+
+    char tx = GYRO_ZOUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+    return output;
+    
+}
+
+char ITG3200::getPowerManagement(void){
+
+    char tx = PWR_MGM_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    return rx;
+
+}
+
+void ITG3200::setPowerManagement(char config){
+
+    char tx[2];
+    tx[0] = PWR_MGM_REG;
+    tx[1] = config;
+
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
diff -r 000000000000 -r b89a41f096ae ITG3200.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.h	Tue May 15 08:31:45 2012 +0000
@@ -0,0 +1,339 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * ITG-3200 triple axis, digital interface, gyroscope.
+ *
+ * Datasheet:
+ *
+ * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
+ */
+
+#ifndef ITG3200_H
+#define ITG3200_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define ITG3200_I2C_ADDRESS 0x68 //7-bit address.
+
+//-----------
+// Registers
+//-----------
+#define WHO_AM_I_REG    0x00
+#define SMPLRT_DIV_REG  0x15
+#define DLPF_FS_REG     0x16
+#define INT_CFG_REG     0x17
+#define INT_STATUS      0x1A
+#define TEMP_OUT_H_REG  0x1B
+#define TEMP_OUT_L_REG  0x1C
+#define GYRO_XOUT_H_REG 0x1D
+#define GYRO_XOUT_L_REG 0x1E
+#define GYRO_YOUT_H_REG 0x1F
+#define GYRO_YOUT_L_REG 0x20
+#define GYRO_ZOUT_H_REG 0x21
+#define GYRO_ZOUT_L_REG 0x22
+#define PWR_MGM_REG     0x3E
+
+//----------------------------
+// Low Pass Filter Bandwidths
+//----------------------------
+#define LPFBW_256HZ 0x00
+#define LPFBW_188HZ 0x01
+#define LPFBW_98HZ  0x02
+#define LPFBW_42HZ  0x03
+#define LPFBW_20HZ  0x04
+#define LPFBW_10HZ  0x05
+#define LPFBW_5HZ   0x06
+
+/**
+ * ITG-3200 triple axis digital gyroscope.
+ */
+class ITG3200 {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * Sets FS_SEL to 0x03 for proper opertaion.
+     *
+     * @param sda - mbed pin to use for the SDA I2C line.
+     * @param scl - mbed pin to use for the SCL I2C line.
+     */
+    ITG3200(PinName sda, PinName scl);
+
+    /**
+     * Get the identity of the device.
+     *
+     * @return The contents of the Who Am I register which contains the I2C
+     *         address of the device.
+     */
+    char getWhoAmI(void);
+
+    /**
+     * Set the address of the device.
+     *
+     * @param address The I2C slave address to write to the Who Am I register
+     *        on the device.
+     */
+    void setWhoAmI(char address);
+
+    /**
+     * Get the sample rate divider.
+     *
+     * @return The sample rate divider as a number from 0-255.
+     */
+    char getSampleRateDivider(void);
+
+    /**
+     * Set the sample rate divider.
+     *
+     * Fsample = Finternal / (divider + 1), where Finternal = 1kHz or 8kHz,
+     * as decidied by the DLPF_FS register.
+     *
+     * @param The sample rate divider as a number from 0-255.
+     */
+    void setSampleRateDivider(char divider);
+
+    /**
+     * Get the internal sample rate.
+     *
+     * @return The internal sample rate in kHz - either 1 or 8.
+     */
+    int getInternalSampleRate(void);
+
+    /**
+     * Set the low pass filter bandwidth.
+     *
+     * Also used to set the internal sample rate.
+     * Pass the #define bandwidth codes as a parameter.
+     *
+     * 256Hz -> 8kHz internal sample rate.
+     * Everything else -> 1kHz internal rate.
+     *
+     * @param bandwidth Low pass filter bandwidth code
+     */
+    void setLpBandwidth(char bandwidth);
+
+    /**
+     * Get the interrupt configuration.
+     *
+     * See datasheet for register contents details.
+     *
+     *    7      6           5                 4
+     * +------+------+--------------+------------------+
+     * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR |
+     * +------+------+--------------+------------------+
+     *
+     *   3        2            1       0
+     * +---+------------+------------+---+
+     * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 |
+     * +---+------------+------------+---+
+     *
+     * ACTL Logic level for INT output pin; 1 = active low, 0 = active high.
+     * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull.
+     * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared,
+     *                          0 = 50us pulse.
+     * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read,
+     *                                      0 = status register read only.
+     * ITG_RDY_EN Enable interrupt when device is ready,
+     *            (PLL ready after changing clock source).
+     * RAW_RDY_EN Enable interrupt when data is available.
+     * 0 Bits 1 and 3 of the INT_CFG register should be zero.
+     *
+     * @return the contents of the INT_CFG register.
+     */
+    char getInterruptConfiguration(void);
+
+    /**
+     * Set the interrupt configuration.
+     *
+     * See datasheet for configuration byte details.
+     *
+     *    7      6           5                 4
+     * +------+------+--------------+------------------+
+     * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR |
+     * +------+------+--------------+------------------+
+     *
+     *   3        2            1       0
+     * +---+------------+------------+---+
+     * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 |
+     * +---+------------+------------+---+
+     *
+     * ACTL Logic level for INT output pin; 1 = active low, 0 = active high.
+     * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull.
+     * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared,
+     *                          0 = 50us pulse.
+     * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read,
+     *                                      0 = status register read only.
+     * ITG_RDY_EN Enable interrupt when device is ready,
+     *            (PLL ready after changing clock source).
+     * RAW_RDY_EN Enable interrupt when data is available.
+     * 0 Bits 1 and 3 of the INT_CFG register should be zero.
+     *
+     * @param config Configuration byte to write to INT_CFG register.
+     */
+    void setInterruptConfiguration(char config);
+
+    /**
+     * Check the ITG_RDY bit of the INT_STATUS register.
+     *
+     * @return True if the ITG_RDY bit is set, corresponding to PLL ready,
+     *         false if the ITG_RDY bit is not set, corresponding to PLL not
+     *         ready.
+     */
+    bool isPllReady(void);
+
+    /**
+     * Check the RAW_DATA_RDY bit of the INT_STATUS register.
+     *
+     * @return True if the RAW_DATA_RDY bit is set, corresponding to new data
+     *         in the sensor registers, false if the RAW_DATA_RDY bit is not
+     *         set, corresponding to no new data yet in the sensor registers.
+     */
+    bool isRawDataReady(void);
+
+    /**
+     * Get the temperature of the device.
+     *
+     * @return The temperature in degrees celsius.
+     */
+    float getTemperature(void);
+
+    /**
+     * Get the output for the x-axis gyroscope.
+     *
+     * Typical sensitivity is 14.375 LSB/(degrees/sec).
+     *
+     * @return The output on the x-axis in raw ADC counts.
+     */
+    int getGyroX(void);
+
+    /**
+     * Get the output for the y-axis gyroscope.
+     *
+     * Typical sensitivity is 14.375 LSB/(degrees/sec).
+     *
+     * @return The output on the y-axis in raw ADC counts.
+     */
+    int getGyroY(void);
+
+    /**
+     * Get the output on the z-axis gyroscope.
+     *
+     * Typical sensitivity is 14.375 LSB/(degrees/sec).
+     * 
+     * @return The output on the z-axis in raw ADC counts.
+     */
+    int getGyroZ(void);
+
+    /**
+     * Get the power management configuration.
+     *
+     * See the datasheet for register contents details.
+     *
+     *     7        6        5         4
+     * +---------+-------+---------+---------+
+     * | H_RESET | SLEEP | STBY_XG | STBY_YG |
+     * +---------+-------+---------+---------+
+     *
+     *      3          2         1          0
+     * +---------+----------+----------+----------+
+     * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 |
+     * +---------+----------+----------+----------+
+     *
+     * H_RESET Reset device and internal registers to the power-up-default settings.
+     * SLEEP Enable low power sleep mode.
+     * STBY_XG Put gyro X in standby mode (1=standby, 0=normal).
+     * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal).
+     * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal).
+     * CLK_SEL Select device clock source:
+     *
+     * CLK_SEL | Clock Source
+     * --------+--------------
+     *    0      Internal oscillator
+     *    1      PLL with X Gyro reference
+     *    2      PLL with Y Gyro reference
+     *    3      PLL with Z Gyro reference
+     *    4      PLL with external 32.768kHz reference
+     *    5      PLL with external 19.2MHz reference
+     *    6      Reserved
+     *    7      Reserved
+     *
+     * @return The contents of the PWR_MGM register.
+     */
+    char getPowerManagement(void);
+
+    /**
+     * Set power management configuration.
+     *
+     * See the datasheet for configuration byte details
+     *
+     *      7        6        5         4
+     * +---------+-------+---------+---------+
+     * | H_RESET | SLEEP | STBY_XG | STBY_YG |
+     * +---------+-------+---------+---------+
+     *
+     *      3          2         1          0
+     * +---------+----------+----------+----------+
+     * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 |
+     * +---------+----------+----------+----------+
+     *
+     * H_RESET Reset device and internal registers to the power-up-default settings.
+     * SLEEP Enable low power sleep mode.
+     * STBY_XG Put gyro X in standby mode (1=standby, 0=normal).
+     * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal).
+     * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal).
+     * CLK_SEL Select device clock source:
+     *
+     * CLK_SEL | Clock Source
+     * --------+--------------
+     *    0      Internal oscillator
+     *    1      PLL with X Gyro reference
+     *    2      PLL with Y Gyro reference
+     *    3      PLL with Z Gyro reference
+     *    4      PLL with external 32.768kHz reference
+     *    5      PLL with external 19.2MHz reference
+     *    6      Reserved
+     *    7      Reserved
+     *
+     * @param config The configuration byte to write to the PWR_MGM register.
+     */
+    void setPowerManagement(char config);
+
+private:
+
+    I2C i2c_;
+
+};
+
+#endif /* ITG3200_H */
diff -r 000000000000 -r b89a41f096ae main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 15 08:31:45 2012 +0000
@@ -0,0 +1,51 @@
+#include "ITG3200.h"
+
+Serial pc(USBTX, USBRX);
+ITG3200 gyro(p9, p10);
+      
+int main() {
+    
+    float  t    =   0;
+    float dt    =   0.1;
+    double Gyro0 [3] = {0, 0, 0};
+    double Gyro1 [3] = {0, 0, 0};
+    double  Ang0 [3] = {0, 0, 0};
+    double  Ang1 [3] = {0, 0, 0};
+    
+    pc.printf("Now starting ITG-3200 test\n");
+    pc.printf(" Unit: Gyro[deg/s] Angle[deg]\n");
+    pc.printf("  Time  GyroX  GyroY  GyroZ    AngX    AngY    AngZ\n");
+    
+    //Set highest bandwidth.
+    gyro.setLpBandwidth(LPFBW_42HZ);
+    
+    //Arbitrary wait for printf clarity.
+    wait(0.1);
+    
+    while (1) {
+        //pc.printf("%x\n",gyro.getWhoAmI());
+        // YAL 9DOF: x+16, y-39, z+40
+        //  My 9DOF:  x, y, z
+        Gyro1[0] = (gyro.getGyroX()+16)/14.375;      Gyro1[1] = (gyro.getGyroY()-39)/14.375;      Gyro1[2] = (gyro.getGyroZ()+40)/14.375;
+        
+        // Low pass filter for gyro
+        if( -1.0<Gyro1[0] && Gyro1[0]<1.0 ){    Gyro1[0]=0;  }
+        if( -1.0<Gyro1[1] && Gyro1[1]<1.0 ){    Gyro1[1]=0;  }
+        if( -1.0<Gyro1[2] && Gyro1[2]<1.0 ){    Gyro1[2]=0;  }
+        
+        // Trapezoidal integration
+        Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2;
+        Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2;
+        Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2;
+        
+        pc.printf("%6.1f, %5.0f, %5.0f, %5.0f, %6.1f, %6.1f, %6.1f\n\r", 
+                  t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]);
+        
+        Gyro0[0] = Gyro1[0];  Gyro0[1] = Gyro1[1];  Gyro0[2] = Gyro1[2];
+        Ang0[0]  = Ang1[0];   Ang0[1]  = Ang1[1];   Ang0[2]  = Ang1[2];
+        t   =   t+dt;
+        wait(0.1);
+
+    }
+
+}
diff -r 000000000000 -r b89a41f096ae mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 15 08:31:45 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da