Revision 0:561f7672eaad, committed 2017-06-14

Comitter:

tzxl10000

Date:

Wed Jun 14 20:22:45 2017 +0000

Commit message:

fish;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D/L3G4200D.cpp	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,99 @@
+/**
+ * Copyright (c) 2011 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.
+ */
+ 
+#include "mbed.h"
+#include <L3G4200D.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
+// mbed I2C libraries take the 7-bit address shifted left 1 bit
+// #define GYR_ADDRESS (0xD2 >> 1)
+#define GYR_ADDRESS 0xD2
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+// Constructor
+L3G4200D::L3G4200D(PinName sda, PinName scl):
+    _device(sda, scl)
+{
+    _device.frequency(400000);
+    // Turns on the L3G4200D's gyro and places it in normal mode.
+    // 0x0F = 0b00001111
+    // Normal power mode, all axes enabled
+    writeReg(L3G4200D_CTRL_REG1, 0x0F);
+    writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale
+
+}
+
+// Writes a gyro register
+void L3G4200D::writeReg(byte reg, byte value)
+{
+    data[0] = reg;
+    data[1] = value;
+    
+    _device.write(GYR_ADDRESS, data, 2);
+}
+
+// Reads a gyro register
+byte L3G4200D::readReg(byte reg)
+{
+    byte value = 0;
+    
+    _device.write(GYR_ADDRESS, &reg, 1);
+    _device.read(GYR_ADDRESS, &value, 1);
+
+    return value;
+}
+
+// Reads the 3 gyro channels and stores them in vector g
+void L3G4200D::read(int g[3])
+{
+    // assert the MSB of the address to get the gyro 
+    // to do slave-transmit subaddress updating.
+    data[0] = L3G4200D_OUT_X_L | (1 << 7);
+    _device.write(GYR_ADDRESS, data, 1); 
+
+//    Wire.requestFrom(GYR_ADDRESS, 6);
+//    while (Wire.available() < 6);
+    
+    _device.read(GYR_ADDRESS, data, 6); 
+
+    uint8_t xla = data[0];
+    uint8_t xha = data[1];
+    uint8_t yla = data[2];
+    uint8_t yha = data[3];
+    uint8_t zla = data[4];
+    uint8_t zha = data[5];
+
+    g[0] = (short) (xha << 8 | xla);
+    g[1] = (short) (yha << 8 | yla);
+    g[2] = (short) (zha << 8 | zla);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D/L3G4200D.h	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,106 @@
+/* Copyright (c) 2011 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.
+ */
+
+#ifndef __L3G4200D_H
+#define __L3G4200D_H
+
+#include "mbed.h"
+
+// register addresses
+
+#define L3G4200D_WHO_AM_I      0x0F
+
+#define L3G4200D_CTRL_REG1     0x20
+#define L3G4200D_CTRL_REG2     0x21
+#define L3G4200D_CTRL_REG3     0x22
+#define L3G4200D_CTRL_REG4     0x23
+#define L3G4200D_CTRL_REG5     0x24
+#define L3G4200D_REFERENCE     0x25
+#define L3G4200D_OUT_TEMP      0x26
+#define L3G4200D_STATUS_REG    0x27
+
+#define L3G4200D_OUT_X_L       0x28
+#define L3G4200D_OUT_X_H       0x29
+#define L3G4200D_OUT_Y_L       0x2A
+#define L3G4200D_OUT_Y_H       0x2B
+#define L3G4200D_OUT_Z_L       0x2C
+#define L3G4200D_OUT_Z_H       0x2D
+
+#define L3G4200D_FIFO_CTRL_REG 0x2E
+#define L3G4200D_FIFO_SRC_REG  0x2F
+
+#define L3G4200D_INT1_CFG      0x30
+#define L3G4200D_INT1_SRC      0x31
+#define L3G4200D_INT1_THS_XH   0x32
+#define L3G4200D_INT1_THS_XL   0x33
+#define L3G4200D_INT1_THS_YH   0x34
+#define L3G4200D_INT1_THS_YL   0x35
+#define L3G4200D_INT1_THS_ZH   0x36
+#define L3G4200D_INT1_THS_ZL   0x37
+#define L3G4200D_INT1_DURATION 0x38
+
+typedef char byte;
+
+/** Interface library for the ST L3G4200D 3-axis gyro
+ *
+ * Ported from Pololu L3G4200D library for Arduino by
+ * Michael Shimniok http://bot-thoughts.com
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "L3G4200D.h"
+ * L3G4200D gyro(p28, p27);
+ * ...
+ * int g[3];
+ * gyro.read(g);
+ * @endcode
+ */
+class L3G4200D
+{
+    public:
+        /** Create a new L3G4200D I2C interface
+         * @param sda is the pin for the I2C SDA line
+         * @param scl is the pin for the I2C SCL line
+         */
+        L3G4200D(PinName sda, PinName scl);
+        
+        /** Read gyro values
+         * @param g Array containing x, y, and z gyro values
+         * @return g Array containing x, y, and z gyro values
+         */
+        void read(int g[3]);
+        
+    private:
+        byte data[6];
+        int _rates[3];
+        I2C _device;
+        void writeReg(byte reg, byte value);
+        byte readReg(byte reg);
+        void enableDefault(void);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20/L3GD20.cpp	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,107 @@
+/**
+ * Copyright (c) 2011 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.
+ */
+ 
+ #include "mbed.h"
+ #include "L3GD20.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
+// mbed I2C libraries take the 7-bit address shifted left 1 bit
+// #define GYR_ADDRESS (0xD2 >> 1)
+#define GYR_ADDRESS 0xD6
+
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+// Constructor
+L3GD20::L3GD20(PinName sda, PinName scl):
+    _L3GD20(sda, scl)
+{
+    char reg_v;
+    _L3GD20.frequency(100000);
+    
+  // 0x0F = 0b00001111
+  // Normal power mode, all axes enabled
+    reg_v = 0;    
+    reg_v |= 0x0F;       
+    write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v);
+    
+
+
+}
+
+
+
+bool L3GD20::read(float *gx, float *gy, float *gz) {
+    char gyr[6];
+ 
+    if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
+    //scale is 8.75 mdps/digit
+        *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;  
+        *gy =  float(short(gyr[3] << 8 | gyr[2]))*0.00875;
+        *gz =  float(short(gyr[5] << 8 | gyr[4]))*0.00875;
+
+ 
+        return true;
+    }
+ 
+    return false;
+}
+
+
+
+
+bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v)
+{
+    char data[2] = {addr_reg, v}; 
+    return L3GD20::_L3GD20.write(addr_i2c, data, 2) == 0;
+}
+
+bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v)
+{
+    char data = addr_reg; 
+    bool result = false;
+    
+    __disable_irq();
+    if ((_L3GD20.write(addr_i2c, &data, 1) == 0) && (_L3GD20.read(addr_i2c, &data, 1) == 0)){
+        *v = data;
+        result = true;
+    }
+    __enable_irq();
+    return result;
+}
+
+
+bool L3GD20::recv(char sad, char sub, char *buf, int length) {
+    if (length > 1) sub |= 0x80;
+ 
+    return _L3GD20.write(sad, &sub, 1, true) == 0 && _L3GD20.read(sad, buf, length) == 0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20/L3GD20.h	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,103 @@
+/* Copyright (c) 2011 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.
+ */
+
+#ifndef __L3GD20_H
+#define __L3GD20_H
+
+#include "mbed.h"
+
+// register addresses
+
+#define L3GD20_WHO_AM_I      0x0F
+
+#define L3GD20_CTRL_REG1     0x20
+#define L3GD20_CTRL_REG2     0x21
+#define L3GD20_CTRL_REG3     0x22
+#define L3GD20_CTRL_REG4     0x23
+#define L3GD20_CTRL_REG5     0x24
+#define L3GD20_REFERENCE     0x25
+#define L3GD20_OUT_TEMP      0x26
+#define L3GD20_STATUS_REG    0x27
+
+#define L3GD20_OUT_X_L       0x28
+#define L3GD20_OUT_X_H       0x29
+#define L3GD20_OUT_Y_L       0x2A
+#define L3GD20_OUT_Y_H       0x2B
+#define L3GD20_OUT_Z_L       0x2C
+#define L3GD20_OUT_Z_H       0x2D
+
+#define L3GD20_FIFO_CTRL_REG 0x2E
+#define L3GD20_FIFO_SRC_REG  0x2F
+
+#define L3GD20_INT1_CFG      0x30
+#define L3GD20_INT1_SRC      0x31
+#define L3GD20_INT1_THS_XH   0x32
+#define L3GD20_INT1_THS_XL   0x33
+#define L3GD20_INT1_THS_YH   0x34
+#define L3GD20_INT1_THS_YL   0x35
+#define L3GD20_INT1_THS_ZH   0x36
+#define L3GD20_INT1_THS_ZL   0x37
+#define L3GD20_INT1_DURATION 0x38
+
+/** Interface library for the ST L3GD20 3-axis gyro
+ *
+ * Ported from Pololu L3GD20 library for Arduino by
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "L3GD20.h"
+ * L3GD20 gyro(p28, p27);
+ * ...
+ * int g[3];
+ * gyro.read(g);
+ * @endcode
+ */
+class L3GD20
+{
+    public:
+        /** Create a new L3GD20 I2C interface
+         * @param sda is the pin for the I2C SDA line
+         * @param scl is the pin for the I2C SCL line
+         */
+        L3GD20(PinName sda, PinName scl);
+        
+        /** Read gyro values
+         * @param g Array containing x, y, and z gyro values
+         * @return g Array containing x, y, and z gyro values
+         */
+        bool read(float *gx, float *gy, float *gz);
+        
+    private:
+            I2C _L3GD20;
+        float gx, gy, gz;
+
+        bool write_reg(int addr_i2c,int addr_reg, char v);
+        bool read_reg(int addr_i2c,int addr_reg, char *v);
+        bool recv(char sad, char sub, char *buf, int length);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC/LSM303DLHC.cpp	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,134 @@
+/** LSM303DLHC Interface Library
+ *
+ * base on code by Michael Shimniok http://bot-thoughts.com
+ *
+ *  and on test program by @tosihisa and 
+ *
+ *  and on Pololu sample library for LSM303DLHC breakout by ryantm:
+ *
+ * Copyright (c) 2011 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.
+ */
+#include "mbed.h"
+#include "LSM303DLHC.h"
+
+
+const int addr_acc = 0x32;
+const int addr_mag = 0x3c;
+
+enum REG_ADDRS {
+    /* --- Mag --- */
+    CRA_REG_M   = 0x00,
+    CRB_REG_M   = 0x01,
+    MR_REG_M    = 0x02,
+    OUT_X_M     = 0x03,
+    OUT_Y_M     = 0x05,
+    OUT_Z_M     = 0x07,
+    /* --- Acc --- */
+    CTRL_REG1_A = 0x20,
+    CTRL_REG4_A = 0x23,
+    OUT_X_A     = 0x28,
+    OUT_Y_A     = 0x2A,
+    OUT_Z_A     = 0x2C,
+};
+
+bool LSM303DLHC::write_reg(int addr_i2c,int addr_reg, char v)
+{
+    char data[2] = {addr_reg, v}; 
+    return LSM303DLHC::_LSM303.write(addr_i2c, data, 2) == 0;
+}
+
+bool LSM303DLHC::read_reg(int addr_i2c,int addr_reg, char *v)
+{
+    char data = addr_reg; 
+    bool result = false;
+    
+    __disable_irq();
+    if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){
+        *v = data;
+        result = true;
+    }
+    __enable_irq();
+    return result;
+}
+
+
+LSM303DLHC::LSM303DLHC(PinName sda, PinName scl):
+    _LSM303(sda, scl)
+{
+    char reg_v;
+    _LSM303.frequency(100000);
+        
+    reg_v = 0;
+    
+    reg_v |= 0x27;          /* X/Y/Z axis enable. */
+    write_reg(addr_acc,CTRL_REG1_A,reg_v);
+
+    reg_v = 0;
+   // reg_v |= 0x01 << 6;     /* 1: data MSB @ lower address */
+    reg_v = 0x01 << 4;     /* +/- 4g */
+    write_reg(addr_acc,CTRL_REG4_A,reg_v);
+
+    /* -- mag --- */
+    reg_v = 0;
+    reg_v |= 0x04 << 2;     /* Minimum data output rate = 15Hz */
+    write_reg(addr_mag,CRA_REG_M,reg_v);
+
+    reg_v = 0;
+    reg_v |= 0x01 << 5;     /* +-1.3Gauss */
+    //reg_v |= 0x07 << 5;     /* +-8.1Gauss */
+    write_reg(addr_mag,CRB_REG_M,reg_v);
+
+    reg_v = 0;              /* Continuous-conversion mode */
+    write_reg(addr_mag,MR_REG_M,reg_v);
+}
+
+
+bool LSM303DLHC::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
+    char acc[6], mag[6];
+ 
+    if (recv(addr_acc, OUT_X_A, acc, 6) && recv(addr_mag, OUT_X_M, mag, 6)) {
+        *ax = float(short(acc[1] << 8 | acc[0]))/8192;  //32768/4=8192
+        *ay =  float(short(acc[3] << 8 | acc[2]))/8192;
+        *az =  float(short(acc[5] << 8 | acc[4]))/8192;
+        //full scale magnetic readings are from -2048 to 2047
+        //gain is x,y =1100; z = 980 LSB/gauss
+        *mx = float(short(mag[0] << 8 | mag[1]))/1100;
+        *mz = float(short(mag[2] << 8 | mag[3]))/980;
+        *my = float(short(mag[4] << 8 | mag[5]))/1100;
+ 
+        return true;
+    }
+ 
+    return false;
+}
+
+
+bool LSM303DLHC::recv(char sad, char sub, char *buf, int length) {
+    if (length > 1) sub |= 0x80;
+ 
+    return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC/LSM303DLHC.h	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,38 @@
+
+#ifndef __LSM303DLHC_H
+#define __LSM303DLHC_H
+#include "mbed.h"
+
+
+
+class LSM303DLHC {
+    public:
+        /** Create a new interface for an LSM303DLHC
+         *
+         * @param sda is the pin for the I2C SDA line
+         * @param scl is the pin for the I2C SCL line
+         */
+        LSM303DLHC(PinName sda, PinName scl);
+
+  
+        /** read the raw accelerometer and compass values
+         *
+         * @param ax,ay,az is the accelerometer 3d vector, written by the function
+         * @param mx,my,mz is the magnetometer 3d vector, written by the function
+         */
+         bool read(float *ax, float *ay, float *az, float *mx, float *my, float *mz);
+
+
+    private:
+        I2C _LSM303;
+
+         
+        float ax, ay, az;
+        float mx, my, mz;         
+         
+        bool write_reg(int addr_i2c,int addr_reg, char v);
+        bool read_reg(int addr_i2c,int addr_reg, char *v);
+        bool recv(char sad, char sub, char *buf, int length);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLM/LSM303DLM.cpp	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "LSM303DLM.h"
+#include "stdio.h"
+
+#define MAG_ADDRESS  0x3C
+#define ACC_ADDRESS  0x30
+
+LSM303DLM::LSM303DLM(PinName sda, PinName scl): _device(sda, scl)
+{
+    _device.frequency(400000);
+    init();
+}
+
+void LSM303DLM::init()
+{
+    // init mag
+    // continuous conversion mode
+    _data[0] = MR_REG_M;
+    _data[1] = 0x00;
+    _device.write(MAG_ADDRESS, _data, 2);
+    // data rate 75hz
+    _data[0] = CRA_REG_M;
+    _data[1] = 0x18; // 0b00011000
+    _device.write(MAG_ADDRESS, _data, 2);
+    // init acc
+    // data rate 100hz 
+    _data[0] = CTRL_REG1_A;
+    _data[1] = 0x2F; // 0b00101111
+    _device.write(ACC_ADDRESS, _data, 2);
+}
+
+void LSM303DLM::read(int a[3], int m[3])
+{
+    readAcc(a);
+    readMag(m);   
+}
+
+void LSM303DLM::readAcc(int a[3])
+{
+    _data[0] = OUT_X_L_A | (1<<7);
+    _device.write(ACC_ADDRESS, _data, 1);
+    _device.read(ACC_ADDRESS, _data, 6);
+
+    // 12-bit values
+    a[0] = (short) (_data[1]<<8 | _data[0]) >> 4;
+    a[1] = (short) (_data[3]<<8 | _data[2]) >> 4;
+    a[2] = (short) (_data[5]<<8 | _data[4]) >> 4;
+}
+
+void LSM303DLM::readMag(int m[3])
+{
+    _data[0] = OUT_X_H_M;
+    _device.write(MAG_ADDRESS, _data, 1);
+    _device.read(MAG_ADDRESS, _data, 6);
+    
+    m[0] = (short) (_data[0]<<8 | _data[1]); // X
+    m[1] = (short) (_data[4]<<8 | _data[5]); // Y
+    m[2] = (short) (_data[2]<<8 | _data[3]); // Z
+ }
+
+void LSM303DLM::setScale(float x, float y, float z)
+{
+    scale[0] = x;
+    scale[1] = y;
+    scale[2] = z;
+}
+
+void LSM303DLM::setOffset(float x, float y, float z)
+{
+    offset[0] = x;
+    offset[1] = y;
+    offset[2] = z;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLM/LSM303DLM.h	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,147 @@
+#ifndef __LSM303DLM_H
+#define __LSM303DLM_H
+
+#include "mbed.h"
+
+// register addresses
+
+#define CTRL_REG1_A       0x20
+#define CTRL_REG2_A       0x21
+#define CTRL_REG3_A       0x22
+#define CTRL_REG4_A       0x23
+#define CTRL_REG5_A       0x24
+#define CTRL_REG6_A       0x25 // DLHC only
+#define HP_FILTER_RESET_A 0x25 // DLH, DLM only
+#define REFERENCE_A       0x26
+#define STATUS_REG_A      0x27
+
+#define OUT_X_L_A         0x28
+#define OUT_X_H_A         0x29
+#define OUT_Y_L_A         0x2A
+#define OUT_Y_H_A         0x2B
+#define OUT_Z_L_A         0x2C
+#define OUT_Z_H_A         0x2D
+
+#define INT1_CFG_A        0x30
+#define INT1_SRC_A        0x31
+#define INT1_THS_A        0x32
+#define INT1_DURATION_A   0x33
+#define INT2_CFG_A        0x34
+#define INT2_SRC_A        0x35
+#define INT2_THS_A        0x36
+#define INT2_DURATION_A   0x37
+
+#define CRA_REG_M         0x00
+#define CRB_REG_M         0x01
+#define MR_REG_M          0x02
+
+#define OUT_X_H_M         0x03
+#define OUT_X_L_M         0x04
+#define OUT_Y_H_M         0x07
+#define OUT_Y_L_M         0x08
+#define OUT_Z_H_M         0x05
+#define OUT_Z_L_M         0x06
+
+#define SR_REG_M          0x09
+#define IRA_REG_M         0x0A
+#define IRB_REG_M         0x0B
+#define IRC_REG_M         0x0C
+
+#define WHO_AM_I_M        0x0F
+
+#define OUT_Z_H_M         0x05
+#define OUT_Z_L_M         0x06
+#define OUT_Y_H_M         0x07
+#define OUT_Y_L_M         0x08
+
+/** Tilt-compensated compass interface Library for the STMicro LSM303DLm 3-axis magnetometer, 3-axis acceleromter
+ * @author Michael Shimniok http://www.bot-thoughts.com/
+ *
+ * This is an early revision; I've not yet implemented heading calculation and the interface differs from my
+ * earlier LSM303DLH; I hope to make this library drop in compatible at some point in the future.
+ * setScale() and setOffset() have no effect at this time.
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "LSM303DLM.h"
+ *
+ * LSM303DLM compass(p28, p27);
+ * ...
+ * int a[3], m[3];
+ * ...
+ * compass.readAcc(a);
+ * compass.readMag(m);
+ *
+ * @endcode
+ */
+class LSM303DLM {
+
+    public:
+        /** Create a new interface for an LSM303DLM
+         *
+         * @param sda is the pin for the I2C SDA line
+         * @param scl is the pin for the I2C SCL line
+         */
+        LSM303DLM(PinName sda, PinName scl);
+
+        /** sets the x, y, and z offset corrections for hard iron calibration
+         * 
+         * Calibration details here:
+         *  http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
+         *
+         * If you gather raw magnetometer data and find, for example, x is offset
+         * by hard iron by -20 then pass +20 to this member function to correct
+         * for hard iron.
+         *
+         * @param x is the offset correction for the x axis
+         * @param y is the offset correction for the y axis
+         * @param z is the offset correction for the z axis
+         */
+        void setOffset(float x, float y, float z);
+        
+        /** sets the scale factor for the x, y, and z axes
+         *
+         * Calibratio details here:
+         *  http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
+         *
+         * Sensitivity of the three axes is never perfectly identical and this
+         * function can help to correct differences in sensitivity.  You're
+         * supplying a multipler such that x, y and z will be normalized to the
+         * same max/min values
+         */
+        void setScale(float x, float y, float z);
+
+        /** read the calibrated accelerometer and magnetometer values
+         *
+         * @param a is the accelerometer 3d vector, written by the function
+         * @param m is the magnetometer 3d vector, written by the function
+         */
+        void read(int a[3], int m[3]);
+        
+        /** read the calibrated accelerometer values
+         *
+         * @param a is the accelerometer 3d vector, written by the function
+         */
+        void readAcc(int a[3]);
+
+        /** read the calibrated magnetometer values
+         *
+         * @param m is the magnetometer 3d vector, written by the function
+         */
+        void readMag(int m[3]);
+
+        /** sets the I2C bus frequency
+         *
+         * @param frequency is the I2C bus/clock frequency, either standard (100000) or fast (400000)
+         */
+        void frequency(int hz);
+     
+    private:
+        I2C _device;
+        char _data[6];
+        int offset[3];
+        int scale[3];
+        void init();
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo/Servo.cpp	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,74 @@
+/* mbed R/C Servo Library
+ *  
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * 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.
+ */
+ 
+#include "Servo.h"
+#include "mbed.h"
+
+static float clamp(float value, float min, float max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+
+Servo::Servo(PinName pin) : _pwm(pin) {
+    calibrate();
+    write(0.5);
+}
+
+void Servo::write(float percent) {
+    float offset = _range * 2.0 * (percent - 0.5);
+    _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+    _p = clamp(percent, 0.0, 1.0);
+}
+
+void Servo::position(float degrees) {
+    float offset = _range * (degrees / _degrees);
+    _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+}
+
+void Servo::calibrate(float range, float degrees) {
+    _range = range;
+    _degrees = degrees;
+}
+
+float Servo::read() {
+    return _p;
+}
+
+Servo& Servo::operator= (float percent) { 
+    write(percent);
+    return *this;
+}
+
+Servo& Servo::operator= (Servo& rhs) {
+    write(rhs.read());
+    return *this;
+}
+
+Servo::operator float() {
+    return read();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo/Servo.h	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,98 @@
+/* mbed R/C Servo Library
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * 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.
+ */
+  
+#ifndef MBED_SERVO_H
+#define MBED_SERVO_H
+
+#include "mbed.h"
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * // Continuously sweep the servo through it's full range
+ * #include "mbed.h"
+ * #include "Servo.h"
+ * 
+ * Servo myservo(p21);
+ * 
+ * int main() {
+ *     while(1) {
+ *         for(int i=0; i<100; i++) {
+ *             myservo = i/100.0;
+ *             wait(0.01);
+ *         }
+ *         for(int i=100; i>0; i--) {
+ *             myservo = i/100.0;
+ *             wait(0.01);
+ *         }
+ *     }
+ * }
+ * @endcode
+ */
+class Servo {
+
+public:
+    /** Create a servo object connected to the specified PwmOut pin
+     *
+     * @param pin PwmOut pin to connect to 
+     */
+    Servo(PinName pin);
+    
+    /** Set the servo position, normalised to it's full range
+     *
+     * @param percent A normalised number 0.0-1.0 to represent the full range.
+     */
+    void write(float percent);
+    
+    /**  Read the servo motors current position
+     *
+     * @param returns A normalised number 0.0-1.0  representing the full range.
+     */
+    float read();
+    
+    /** Set the servo position
+     *
+     * @param degrees Servo position in degrees
+     */
+    void position(float degrees);
+    
+    /**  Allows calibration of the range and angles for a particular servo
+     *
+     * @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds
+     * @param degrees Angle from centre to maximum/minimum position in degrees
+     */
+    void calibrate(float range = 0.0005, float degrees = 45.0); 
+        
+    /**  Shorthand for the write and read functions */
+    Servo& operator= (float percent);
+    Servo& operator= (Servo& rhs);
+    operator float();
+
+protected:
+    PwmOut _pwm;
+    float _range;
+    float _degrees;
+    float _p;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_copy.cpp	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,579 @@
+#include "mbed.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+#include "Servo.h"
+#include "math.h"
+#define TS 10                    // sample time [ms]
+#define SIMULATION_TIME 10        // simulation time [s]
+
+LocalFileSystem local("local");  
+
+
+//funtion declaration
+void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3]);
+void getEulerAngles(float angles[3], float RotMatrix[9]);
+void matPrint(float *A, int r, int c);
+void matSum(float *C, float *A, float *B, int r, int c);
+void matProd(float *C, float *A, float *B, int r, int cA, int cB);
+void matTimesVect(float *b, float *A, float *v, int r, int c);
+void transpose(float *B, float *A, int n);
+float norm(float *v, int r);
+void hat(float *B, float *A);
+void moveA();
+void moveB();
+void moveC();
+//variable definition
+float right_pectoral_freq;
+float left_pectoral_freq;
+float tail_freq;
+bool directionA=1;
+bool directionB=1;
+bool directionC=1;
+float speedA=1;
+float speedB=1;
+float speedC=1;
+
+float gx ;
+float gy ;
+float gz ;
+float ax ;
+float ay ;
+float az ;
+float mx ;
+float my ;
+float mz ;
+float GYRO[3], ACC[3], MAGN[3];
+float G[3], A[3], M[3], A_0[3], M_0[3];
+
+const float pi = 3.1415926;
+const float KRG = 57.2958;                   // radians to degrees
+
+//G offset: 0.918820 0.550812 -1.280107
+//A offset: -0.001172 0.989633 0.123766
+const float gx_static_offset=0.918820;
+const float gy_static_offset=0.550812;
+const float gz_static_offset=-1.280107;
+
+
+
+const float gain_gyro = 0.00013315;
+const float gain_acc = 0.0098;
+const float gain_magn = 0.005035;
+float Ka = 0.0001;                            // accelerometer estimation gain
+float Kg = 0.0004;                            // gyro estimation gain
+float Km = 0;                            // magnetometer estiamtion gain
+float g_0[3] = {0.0, 0.0, 9.8};                // gavity vector in S
+float m_0[3] = {0.0, 0.0, 0.0};
+float angles[3] = {0.0, 0.0, 0.0};
+float R_est[9] = {0, 0, 1, 1, 0, 0, 0, 1, 0};
+float R_old[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+float eye3[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+
+float rho=0;
+float pitch=0;
+float yaw=0;
+
+//sinusoidal trajectory parameters
+float freq_l=3;
+float freq_r=3;
+float freq_t=3;
+
+float Amp_l=0;
+float Amp_r=0;
+float Amp_t=0;
+
+float center_l=0;
+float center_r=0;
+float center_t=0;
+
+float phi_l=0;
+float phi_r=0;
+float phi_t=0;
+
+
+float IR_l_data=0.0;
+float IR_r_data=0.0;
+
+
+Ticker tickA;
+Ticker tickB;
+Ticker tickC;
+Ticker t3;
+//Timer realTime;
+
+//sensor declaration
+LSM303DLHC compass(p28, p27);
+L3GD20 gyro(p28, p27);
+Serial pc(USBTX, USBRX);
+//LocalFileSystem local("local");
+//actuator declaration
+//PwmOut fin_l(p22);//was 23
+//PwmOut fin_r(p25);//was 25
+//PwmOut fin_t(p26);//was 21
+
+AnalogIn IR_l(p19);
+AnalogIn IR_r(p20);
+
+
+PwmOut PWMA(p25);//RIGHT  FIN
+PwmOut PWMB(p23);//LEFT FIN
+PwmOut PWMC(p21);//TAIL  FIN
+DigitalOut STBY1 (p30);
+DigitalOut STBY2 (p29);
+DigitalOut AIN1 (p8);
+DigitalOut AIN2 (p11);
+DigitalOut BIN1 (p7);
+DigitalOut BIN2 (p6);
+DigitalOut CIN1 (p16);
+DigitalOut CIN2 (p17);
+int inPin1;
+int inPin2;
+
+float period_pwm;
+void move(int motor, float speed, int direction);
+void stop();
+void loop();
+
+//time parameters
+float t1=0.0;
+int sample_interval_us=TS*1000; // dt=TS sec
+float dt;
+
+
+void loop()
+{
+
+pc.printf("loop start");
+    gyro.read(&G[0],&G[1], &G[2]);
+    compass.read(&A[0],&A[1],&A[2],&M[0],&M[1],&M[2]);
+
+    G[0]=G[0] - gx_static_offset;
+    G[1]=G[1] - gy_static_offset;
+    G[2]=G[2] - gz_static_offset;
+
+    //M[0]=mx;
+    //M[1]=my;
+    //M[2]=mz;
+
+    A[0]=A[0];
+    A[1]=A[1];
+    A[2]=A[2];
+    pc.printf("a0: %f %f %f  \n \r",A[0], A[1], A[2]);
+    pc.printf("g0: %f %f %f  \n \r ",G[0], G[1], G[2]);
+    //pc.printf("m0: %f %f %f  \n \r \n \r",mx, my, mz);
+    pc.printf("\n\r");
+    GYRO[0] = G[0]*gain_gyro;
+    GYRO[1] = G[1]*gain_gyro;
+    GYRO[2] = G[2]*gain_gyro;
+    MAGN[0] = M[0]*gain_magn;
+    MAGN[1] = M[1]*gain_magn;
+    MAGN[2] = M[2]*gain_magn;
+    ACC[0] = -A[0]*gain_acc;
+    ACC[1] = -A[1]*gain_acc;
+    ACC[2] = -A[2]*gain_acc;
+    //pc.printf("a1: %f %f %f  \n \r",ACC[0], ACC[1], ACC[2]);
+    //pc.printf("g1: %f %f %f  \n \r",GYRO[0], GYRO[1], GYRO[2]);
+    // pc.printf("m1: %f %f %f  \n \r \n \r",MAGN[0], MAGN[1], MAGN[2]);
+    pc.printf("\n\r");
+    //matProd(R_old, R_est, eye3, 3, 3, 3);
+    sensorFusion(R_est, R_old, G, A, M);
+    getEulerAngles(angles, R_est);
+
+    rho=KRG*angles[0];
+    yaw=KRG*angles[1];
+    pitch=KRG*angles[2];
+    pc.printf("angles= %f %f %f \n\r \n \r",rho,pitch,yaw);
+    //float thisTime=realTime.read();
+//    FILE *fp = fopen("/local/test.txt", "w"); 
+//    fprintf(fp, "angles= %f %f %f %f \n\r \n \r",rho,pitch,yaw,thisTime);
+//    fclose(fp);
+//controller
+//time
+//    t1=t1+dt;
+}
+
+int main()
+{
+//initialization
+    pc.baud(9600);
+
+
+
+    //realTime.start();
+
+    //setup for PWM
+    period_pwm=0.01;
+    PWMA.period(period_pwm);          // servo requires a 20ms period
+    PWMB.period(period_pwm);          // servo requires a 20ms period
+
+    directionA=1;
+    directionB=1;
+    directionC=1;
+    speedA=1;
+    speedB=1;
+    speedC=1;
+    left_pectoral_freq=3;
+    right_pectoral_freq=0;
+    tail_freq=3;
+
+    //dt=sample_interval_us/1000000.0  ;
+    pc.printf("loop attach \n \r");
+    t3.attach(&loop, .5);
+    /*float d_freq=yaw*(.1);
+    if (d_freq<0) {
+        left_pectoral_freq=3-d_freq;
+        right_pectoral_freq=3;
+    }
+    if(d_freq>0) {
+        left_pectoral_freq=3;
+        right_pectoral_freq=3-d_freq;
+    }
+    
+    */
+    pc.printf("after attach \n \r");
+    if(left_pectoral_freq > 0) {
+        float right_interval=1/right_pectoral_freq;
+        tickA.attach(&moveA, right_interval);
+    }
+
+    if(right_pectoral_freq > 0) {
+        float left_interval=1/left_pectoral_freq;
+        tickB.attach(&moveB, left_interval);
+    }
+
+    if(tail_freq > 0) {
+        float tail_interval=1/tail_freq;
+        tickC.attach(&moveC, tail_interval);
+    }
+
+
+
+
+
+
+
+
+}
+
+void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3])
+
+{
+    // w = gyro + a + b
+    // a = KG*(acc x c)
+    // b = KB*(magn x d)
+    // c = RRt*gravity
+    // d = RRt*mfield
+
+    // Rest = Rold*WW
+    // WW = I+II+III
+    // I = eye(3)
+    // II = alpha*T_c*what
+    // III = beta*T_c*T_c*W2
+    // W2 = what*what
+
+
+    float RRt[9];
+    transpose(RRt, Rold, 3);
+
+    float a[3];
+    float b[3];
+    float c[3];
+    float d[3];
+    float acchat[9];
+    float magnhat[9];
+    float w[3];
+    float what[9];
+    float nw;
+    float alpha;
+    float beta;
+    float II[9];
+    float III[9];
+    float W2[9];
+    float WW[9];
+
+    matTimesVect(c, RRt, g_0, 3, 3);
+    hat(acchat, ac);
+    matTimesVect(a, acchat, c, 3, 3);
+
+    matTimesVect(d, RRt, m_0, 3, 3);
+    hat(magnhat, ma);
+    matTimesVect(b, magnhat, d, 3, 3);
+    int k;
+    for (k = 0; k < 3; k++) {
+        float sum = Kg*gy[k]+(Ka*a[k])+(Km*b[k]);
+        w[k] = sum;
+    }
+    //matPrint(w,k,1);
+    hat(what, w);
+    nw = norm(w, 3);
+    nw = TS*nw;
+
+    alpha = 0;
+    beta = 0;
+
+    if (nw < -0.000000001 || nw > 0.000000001) {
+        alpha = (sin(nw))/nw;
+        beta = (1-cos(nw))/(nw*nw);
+    }
+
+    // printf("\t %f \n", alpha);
+    // printf("\t %f \n", beta);
+
+    matProd(W2, what, what, 3, 3, 3);
+
+    for (int j = 0; j < 9; j++) {
+        II[j] = alpha*TS*what[j];
+        III[j] = beta*TS*TS*W2[j];
+        WW[j] = eye3[j]+II[j]+III[j];
+    }
+
+    matProd(Rest, Rold, WW, 3, 3, 3);
+    //matPrint(Rest,3,3);
+
+}
+
+
+void getEulerAngles(float angles[3], float RotMatrix[9])
+{
+    float theta = -asin(RotMatrix[2]);
+    float phi = atan2(RotMatrix[5]/cos(theta),RotMatrix[8]/cos(theta));
+    float psi = atan2(RotMatrix[1]/cos(theta),RotMatrix[0]/cos(theta));
+    angles[0] = phi;
+    angles[1] = theta;
+    angles[2] = psi;
+    return;
+}
+
+
+// Matrix operations (matrices are stored in a one dimensional array, columnwise)
+// e.g.: [1 2 3; 4 5 6; 7 8 9] = [1 4 7 2 5 8 3 6 9]
+
+
+void matPrint(float *A, int r, int c)
+
+// Print the content of the matrix A which is r x c
+
+{
+    int i, j;
+    pc.printf("printing \n \r");
+    for(i = 0; i < r; i++) {
+        for(j = 0; j < c; j++) {
+            pc.printf("%f",A[i + j*r]);
+            if(j == c-1) {
+                pc.printf("\n \r");
+            } else {
+                pc.printf("\t");
+            }
+        }
+    }
+
+    printf("\n");
+}
+
+void matSum(float *C, float *A, float *B, int r, int c)
+
+//  C = A + B, where A, B and C are r x c
+
+{
+    int i, j;
+    for(i = 0; i < r; i++) {
+        for(j = 0; j < c; j++) {
+            C[i + r*j] = A[i + r*j] + B[i + r*j];
+        }
+    }
+    return;
+}
+
+
+void matProd(float *C, float *A, float *B, int r, int cA, int cB)
+
+// C = A times B, where A is r x c, B is c x c and C is r x c
+
+{
+    float sum;
+    int i, j, k;
+
+    for(i = 0; i < r; i++) {
+        for(j = 0; j < cB; j++) {
+            sum = 0.0;
+            for(k = 0; k < cA; k++) {
+                sum = sum + A[i + k*r] * B[j*cA + k];
+            }
+            C[i + r*j] = sum;
+        }
+    }
+
+    return;
+}
+
+
+void matTimesVect(float *b, float *A, float *v, int r, int c)
+
+//   b = A times v, where A is r x c, and v and b are c-long column vectors
+
+{
+    float sum;
+    int i, k;
+
+    for(i = 0; i < r; i++) {
+        sum = 0.00;
+        for(k = 0; k < c; k++) {
+            sum = sum + A[i + k*r] * v[k];
+        }
+
+        b[i] = sum;
+    }
+
+    return;
+}
+
+
+void transpose(float *B, float *A, int n)
+
+// B = A^T, where A and B are n x n matrices
+
+{
+    int i, j;
+
+    for(i = 0; i < n; i++) {
+        for(j = 0; j < n; j++) {
+            B[n*i+j] = A[i + n*j];
+        }
+    }
+
+    return;
+}
+
+
+float norm(float *v, int r)
+
+// 2-norm of a r-long vector
+
+{
+    float n = 0;
+    int i;
+
+    for(i = 0; i < r; i++) {
+        n = n + v[i]*v[i];
+    }
+
+    n = sqrt(n);
+
+    return n;
+}
+
+
+void hat(float *B, float *A)
+
+// hat operator (for 3-D arrays only)
+
+{
+    B[0] = 0;
+    B[1] = A[2];
+    B[2] = -A[1];
+    B[3] = -A[2];
+    B[4] = 0;
+    B[5] = A[0];
+    B[6] = A[1];
+    B[7] = -A[0];
+    B[8] = 0;
+
+    return;
+}
+
+void move(int motor, float speed, int direction)
+{
+//Move specific motor at speed and direction
+//motor: 0 for B 1 for A
+//speed: 0 is off, and 1 is full speed
+//direction: 0 clockwise, 1 counter-clockwise
+
+    STBY1=1; //disable standby
+    STBY2=1; //disable standby
+
+
+    if(direction == 0) {
+        inPin1 = 0;
+        inPin2 = 1;
+    } else if(direction == 1) {
+
+        inPin1 = 1;
+        inPin2 = 0;
+
+    }
+
+    if(motor == 1) {
+        AIN1=inPin1;
+        AIN2=inPin2;
+        PWMA.pulsewidth(period_pwm*speed*1.0);
+    } else if(motor == 0) {
+        BIN1=inPin1;
+        BIN2=inPin2;
+        PWMB.pulsewidth(period_pwm*speed*1.0);
+    }
+}
+
+
+void stop()
+{
+//enable standby
+    STBY1=0;
+    STBY2=0;
+
+}
+
+void moveA()
+{
+    STBY1=1; //disable standby
+    int inPin1=1;
+    int inPin2=0;
+    if(directionA==0) {
+        inPin1 = 0;
+        inPin2 = 1;
+    }
+    AIN1=inPin1;
+    AIN2=inPin2;
+    directionA=!directionA;
+
+    //pc.printf("dirA = %d\n\r",directionA);
+
+
+
+    PWMA.pulsewidth(period_pwm*speedA);
+
+}
+void moveB()
+{
+    STBY1=1; //disable standby
+    int inPin1=0;
+    int inPin2=1;
+    if(directionB==0) {
+        inPin1 = 1;
+        inPin2 = 0;
+    }
+    //pc.printf("dirB = %d\n\r",directionB);
+    BIN1=inPin1;
+    BIN2=inPin2;
+    directionB=!directionB;
+
+    PWMB.pulsewidth(period_pwm*speedB);
+
+}
+
+void moveC()
+{
+    STBY1=1; //disable standby
+    int inPin1=0;
+    int inPin2=1;
+    if(directionC==0) {
+        inPin1 = 1;
+        inPin2 = 0;
+    }
+    //pc.printf("dirC = %d\n\r",directionC);
+    CIN1=inPin1;
+    CIN2=inPin2;
+    directionC=!directionC;
+
+    PWMC.pulsewidth(period_pwm*speedC);
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479
\ No newline at end of file