Revision 0:70d405bfdc8b, committed 2015-10-18

Comitter:

yal_kaiyo

Date:

Sun Oct 18 09:58:59 2015 +0000

Commit message:

2015??????????????

Changed in this revision

diff -r 000000000000 -r 70d405bfdc8b MPU6050.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.cpp	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,375 @@
+/*   @author: Baser Kandehir 
+*    @date: July 16, 2015
+*    @license: MIT license
+*     
+*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+*   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.
+*
+*/
+
+// Most of the code is adapted from Kris Winer's MPU6050 library
+
+#include "MPU6050.h"
+
+I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
+
+/* Set initial input parameters */
+
+// Acc Full Scale Range  +-2G 4G 8G 16G  
+enum Ascale
+{
+    AFS_2G=0,  
+    AFS_4G,
+    AFS_8G,
+    AFS_16G
+};
+
+// Gyro Full Scale Range +-250 500 1000 2000 Degrees per second
+enum Gscale
+{
+    GFS_250DPS=0,   
+    GFS_500DPS,
+    GFS_1000DPS,
+    GFS_2000DPS
+};
+
+// Sensor datas
+float ax,ay,az;
+float gx,gy,gz;
+int16_t accelData[3],gyroData[3],tempData;
+float accelBias[3] = {0, 0, 0};  // Bias corrections for acc
+float gyroBias[3] = {0, 0, 0};   // Bias corrections for gyro 
+
+// Specify sensor full scale range
+int Ascale = AFS_2G;
+int Gscale = GFS_250DPS;
+
+// Scale resolutions per LSB for the sensors
+float aRes, gRes; 
+
+// Calculates Acc resolution
+void MPU6050::getAres()
+{
+    switch(Ascale)
+    {
+        case AFS_2G:
+            aRes = 2.0/32768.0;
+            break;
+        case AFS_4G:
+            aRes = 4.0/32768.0;
+            break;
+        case AFS_8G:
+            aRes = 8.0/32768.0;
+            break;
+        case AFS_16G:
+            aRes = 16.0/32768.0;
+            break;         
+    }
+}
+
+// Calculates Gyro resolution
+void MPU6050::getGres()
+{
+    switch(Gscale)
+    {
+        case GFS_250DPS:
+            gRes = 250.0/32768.0;
+            break;
+        case GFS_500DPS:
+            gRes = 500.0/32768.0;
+            break;
+        case GFS_1000DPS:
+            gRes = 1000.0/32768.0;
+            break;
+        case GFS_2000DPS:
+            gRes = 2000.0/32768.0;
+            break;
+    }
+}
+    
+void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{
+    char data_write[2];
+    data_write[0]=subAddress;           // I2C sends MSB first. Namely  >>|subAddress|>>|data|
+    data_write[1]=data;
+    i2c.write(address,data_write,2,0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
+}
+
+char MPU6050::readByte(uint8_t address, uint8_t subAddress)
+{
+    char data_read[1];  // will store the register data    
+    char data_write[1];
+    data_write[0]=subAddress;
+    i2c.write(address,data_write,1,1);  // have not stopped yet
+    i2c.read(address,data_read,1,0);    // read the data and stop
+    return data_read[0];
+} 
+
+void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest)
+{
+    char data[14],data_write[1];  
+    data_write[0]=subAddress;      
+    i2c.write(address,data_write,1,1);
+    i2c.read(address,data,byteNum,0);
+    for(int i=0;i<byteNum;i++)         // equate the addresses
+        dest[i]=data[i];
+}
+
+// Communication test: WHO_AM_I register reading 
+void MPU6050::whoAmI()
+{
+    uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);   // Should return 0x68
+    pc.printf("I AM 0x%x \r\n",whoAmI);
+    
+    if(whoAmI==0x68)
+    {
+        pc.printf("MPU6050 is online... \r\n");  
+        led2=1;
+    }
+    else
+    {
+        pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");  
+        toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
+    }  
+}
+
+// Initializes MPU6050 with the following config:
+// PLL with X axis gyroscope reference 
+// Sample rate: 200Hz for gyro and acc
+// Interrupts are disabled
+void MPU6050::init()
+{        
+    /* Wake up the device */
+    writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);  // wake up the device by clearing the sleep bit (bit6) 
+    writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x02);
+    wait_ms(100); // wait 100 ms to stabilize  
+    
+    /* Get stable time source */
+    // PLL with X axis gyroscope reference is used to improve stability
+    writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
+    writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x02);
+    
+    /* Configure Gyroscope and Accelerometer */
+    // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz
+    // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate)
+    writeByte(MPU6050_ADDRESS, CONFIG, 0x03);
+    
+    /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */
+    // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above)
+    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
+    
+    /* Accelerometer configuration */
+    uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
+    writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0);      // Clear self-test bits [7:5]
+    writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18);      // Clear AFS bits [4:3]
+    writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3);  // Set full scale range 
+    
+    /* Gyroscope configuration */       
+    temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG);
+    writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0);      // Clear self-test bits [7:5]
+    writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18);      // Clear FS bits [4:3]
+    writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3);  // Set full scale range 
+}
+
+// Resets the device
+void MPU6050::reset()
+{
+    writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80);  // set bit7 to reset the device
+    wait_ms(100);    // wait 100 ms to stabilize        
+}
+
+void MPU6050::readAccelData(int16_t* dest)
+{
+    uint8_t rawData[6];  // x,y,z acc data            
+    readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);   // read six raw data registers sequentially and write them into data array
+    
+    /* Turn the MSB LSB into signed 16-bit value */
+    dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // ACCEL_XOUT
+    dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // ACCEL_YOUT
+    dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // ACCEL_ZOUT
+}
+
+void MPU6050::readGyroData(int16_t* dest)
+{
+    uint8_t rawData[6];  // x,y,z gyro data            
+    readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);   // read the six raw data registers sequentially and write them into data array
+    
+    /* Turn the MSB LSB into signed 16-bit value */
+    dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // GYRO_XOUT
+    dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // GYRO_YOUT
+    dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // GYRO_ZOUT    
+}
+    
+int16_t MPU6050::readTempData()
+{
+    uint8_t rawData[2];  // temperature data
+    readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);   // read the two raw data registers sequentially and write them into data array 
+    return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);   // turn the MSB LSB into signed 16-bit value
+}
+
+/* Function which accumulates gyro and accelerometer data after device initialization. 
+   It calculates the average of the at-rest readings and 
+   then loads the resulting offsets into accelerometer and gyro bias registers. */
+/* 
+    IMPORTANT NOTE: In this function;
+         Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program.
+         Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program.
+         Resulting data seems satisfactory.
+*/
+// dest1: accelBias dest2: gyroBias
+void MPU6050::calibrate(float* dest1, float* dest2)
+{
+    uint8_t data[12];       // data array to hold acc and gyro x,y,z data
+    uint16_t fifo_count, packet_count, count;   
+    int32_t accel_bias[3] = {0,0,0}; 
+    int32_t gyro_bias[3] = {0,0,0};
+    float aRes = 2.0/32768.0;   
+    float gRes = 250.0/32768.0;
+    uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g
+    //uint16_t gyrosensitivity = 131;    // = 1/gRes = 131 LSB/dps
+    
+    reset();     // Reset device
+    
+    /* Get stable time source */
+    writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);    // PLL with X axis gyroscope reference is used to improve stability
+    writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);    // Disable accel only low power mode 
+    wait(0.2);
+      
+    /* Configure device for bias calculation */
+    writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
+    writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO
+    writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source
+    writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
+    writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
+    writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04);    // Reset FIFO
+    wait(0.015);   
+    
+    /* Configure accel and gyro for bias calculation */
+    writeByte(MPU6050_ADDRESS, CONFIG, 0x01);       // Set low-pass filter to 188 Hz
+    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00);   // Set sample rate to 1 kHz
+    writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+    writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+     
+    /* Configure FIFO to capture accelerometer and gyro data for bias calculation */
+    writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
+    writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78);     // Enable accelerometer and gyro for FIFO  (max size 1024 bytes in MPU-6050)
+    wait(0.08);                                    // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. 
+    // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte.   12*80=960 byte < 1024 byte                                               
+                                                                                               
+    /* At end of sample accumulation, turn off FIFO sensor read */
+    writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00);             // Disable FIFO
+    readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]);  // Read FIFO sample count
+    fifo_count = ((uint16_t)data[0] << 8) | data[1];
+    packet_count = fifo_count/12;                          // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case
+    
+    for(count=0; count<packet_count; count++)
+    {
+        int16_t accel_temp[3]={0,0,0}; 
+        int16_t gyro_temp[3]={0,0,0};
+        readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
+        
+        /* Form signed 16-bit integer for each sample in FIFO */
+        accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ; 
+        accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
+        accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    
+        gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
+        gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
+        gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
+        
+        /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */
+        accel_bias[0] += (int32_t) accel_temp[0]; 
+        accel_bias[1] += (int32_t) accel_temp[1];
+        accel_bias[2] += (int32_t) accel_temp[2];  
+        gyro_bias[0]  += (int32_t) gyro_temp[0];
+        gyro_bias[1]  += (int32_t) gyro_temp[1];
+        gyro_bias[2]  += (int32_t) gyro_temp[2];
+    }
+    
+    /* Normalize sums to get average count biases */
+    accel_bias[0] /= (int32_t) packet_count; 
+    accel_bias[1] /= (int32_t) packet_count;
+    accel_bias[2] /= (int32_t) packet_count;
+    gyro_bias[0]  /= (int32_t) packet_count;
+    gyro_bias[1]  /= (int32_t) packet_count;
+    gyro_bias[2]  /= (int32_t) packet_count;
+      
+    /* Remove gravity from the z-axis accelerometer bias calculation */  
+    if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;}  
+    else {accel_bias[2] += (int32_t) accelsensitivity;}
+    
+    /* Output scaled accelerometer biases for manual subtraction in the main program */
+    dest1[0] = accel_bias[0]*aRes;
+    dest1[1] = accel_bias[1]*aRes;
+    dest1[2] = accel_bias[2]*aRes;
+    
+    /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */
+    data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+    data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
+    data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
+    data[3] = (-gyro_bias[1]/4)       & 0xFF;
+    data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
+    data[5] = (-gyro_bias[2]/4)       & 0xFF;
+
+    /* Push gyro biases to hardware registers */
+    writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); 
+    writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]);
+    writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]);
+    writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]);
+    writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]);
+    writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]);
+
+    /* Construct gyro bias in deg/s for later manual subtraction */
+    dest2[0] = gyro_bias[0]*gRes;   
+    dest2[1] = gyro_bias[1]*gRes;
+    dest2[2] = gyro_bias[2]*gRes;
+}
+
+void MPU6050::complementaryFilter(double* pitch, double* roll)
+//void MPU6050::complementaryFilter()
+{
+    /* Get actual acc value */
+    readAccelData(accelData);
+    getAres();
+    ax = accelData[0]*aRes - accelBias[0];
+    ay = accelData[1]*aRes - accelBias[1];
+    az = accelData[2]*aRes - accelBias[2]; 
+
+    /* Get actual gyro value */
+    readGyroData(gyroData);
+    getGres();     
+    gx = gyroData[0]*gRes;  // - gyroBias[0];      // Results are better without extracting gyroBias[i]
+    gy = gyroData[1]*gRes;  // - gyroBias[1]; 
+    gz = gyroData[2]*gRes;  // - gyroBias[2]; 
+
+    double pitchAcc, rollAcc;
+
+    /* Integrate the gyro data(deg/s) over time to get angle */
+    *pitch += gx * dt;  // Angle around the X-axis
+    *roll -=  gy * dt;  // Angle around the Y-axis
+    
+    /* Turning around the X-axis results in a vector on the Y-axis
+    whereas turning around the Y-axis results in a vector on the X-axis. */
+    pitchAcc = atan2f(accelData[1], accelData[2])*180/PI;
+    rollAcc  = atan2f(accelData[0], accelData[2])*180/PI;
+  
+    /* Apply Complementary Filter */
+    *pitch = *pitch * 0.98 + pitchAcc * 0.02;
+    *roll  = *roll  * 0.98 + rollAcc  * 0.02;  
+}
\ No newline at end of file

diff -r 000000000000 -r 70d405bfdc8b MPU6050.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.h	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,72 @@
+/*     
+*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+*   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.
+*
+*/
+
+// Most of the code is adapted from Kris Winer's MPU6050 library
+
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "mbed.h"
+#include "math.h"
+#include "MPU6050RegDef.h"
+
+#define PI 3.14159265359    // This value will be used when calculating angles
+#define dt 0.005            // 200 Hz sampling period
+
+extern I2C i2c;             // extern the i2c in order to able to use from other files
+extern float aRes, gRes; 
+
+/* whoAmI func uses this func, variables etc */
+extern Ticker toggler1;  
+extern Serial pc;   
+extern DigitalOut led2;
+extern void toggle_led1();
+
+/* Sensor datas to be used in program */
+extern float ax,ay,az;
+extern float gx,gy,gz;
+extern int16_t accelData[3],gyroData[3],tempData;
+extern float accelBias[3], gyroBias[3];
+ 
+/* Function Prototypes */
+class MPU6050 
+{
+    protected:
+    public: 
+    void getAres();
+    void getGres();
+    void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+    char readByte(uint8_t address, uint8_t subAddress); 
+    void readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest);
+    void whoAmI();
+    void init();
+    void reset();
+    void readAccelData(int16_t* dest);
+    void readGyroData(int16_t* dest);
+    int16_t readTempData();
+    void calibrate(float* dest1, float* dest2);
+    void complementaryFilter(double* pitch, double* roll);
+    //void complementaryFilter(float* pitch, float* roll);
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 70d405bfdc8b MPU6050RegDef.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050RegDef.h	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,153 @@
+/*     
+*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+*   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.
+*
+*/
+
+// Taken from Kris Winer's MPU6050 library
+ 
+// Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device
+// Invensense Inc., www.invensense.com
+// See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in 
+// above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
+//
+#define XGOFFS_TC        0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD                 
+#define YGOFFS_TC        0x01                                                                          
+#define ZGOFFS_TC        0x02
+#define X_FINE_GAIN      0x03 // [7:0] fine gain
+#define Y_FINE_GAIN      0x04
+#define Z_FINE_GAIN      0x05
+#define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer
+#define XA_OFFSET_L_TC   0x07
+#define YA_OFFSET_H      0x08
+#define YA_OFFSET_L_TC   0x09
+#define ZA_OFFSET_H      0x0A
+#define ZA_OFFSET_L_TC   0x0B
+#define SELF_TEST_X      0x0D
+#define SELF_TEST_Y      0x0E    
+#define SELF_TEST_Z      0x0F
+#define SELF_TEST_A      0x10
+#define XG_OFFS_USRH     0x13  // User-defined trim values for gyroscope; supported in MPU-6050?
+#define XG_OFFS_USRL     0x14
+#define YG_OFFS_USRH     0x15
+#define YG_OFFS_USRL     0x16
+#define ZG_OFFS_USRH     0x17
+#define ZG_OFFS_USRL     0x18
+#define SMPLRT_DIV       0x19
+#define CONFIG           0x1A
+#define GYRO_CONFIG      0x1B
+#define ACCEL_CONFIG     0x1C
+#define FF_THR           0x1D  // Free-fall
+#define FF_DUR           0x1E  // Free-fall
+#define MOT_THR          0x1F  // Motion detection threshold bits [7:0]
+#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+#define FIFO_EN          0x23
+#define I2C_MST_CTRL     0x24   
+#define I2C_SLV0_ADDR    0x25
+#define I2C_SLV0_REG     0x26
+#define I2C_SLV0_CTRL    0x27
+#define I2C_SLV1_ADDR    0x28
+#define I2C_SLV1_REG     0x29
+#define I2C_SLV1_CTRL    0x2A
+#define I2C_SLV2_ADDR    0x2B
+#define I2C_SLV2_REG     0x2C
+#define I2C_SLV2_CTRL    0x2D
+#define I2C_SLV3_ADDR    0x2E
+#define I2C_SLV3_REG     0x2F
+#define I2C_SLV3_CTRL    0x30
+#define I2C_SLV4_ADDR    0x31
+#define I2C_SLV4_REG     0x32
+#define I2C_SLV4_DO      0x33
+#define I2C_SLV4_CTRL    0x34
+#define I2C_SLV4_DI      0x35
+#define I2C_MST_STATUS   0x36
+#define INT_PIN_CFG      0x37
+#define INT_ENABLE       0x38
+#define DMP_INT_STATUS   0x39  // Check DMP interrupt
+#define INT_STATUS       0x3A
+#define ACCEL_XOUT_H     0x3B
+#define ACCEL_XOUT_L     0x3C
+#define ACCEL_YOUT_H     0x3D
+#define ACCEL_YOUT_L     0x3E
+#define ACCEL_ZOUT_H     0x3F
+#define ACCEL_ZOUT_L     0x40
+#define TEMP_OUT_H       0x41
+#define TEMP_OUT_L       0x42
+#define GYRO_XOUT_H      0x43
+#define GYRO_XOUT_L      0x44
+#define GYRO_YOUT_H      0x45
+#define GYRO_YOUT_L      0x46
+#define GYRO_ZOUT_H      0x47
+#define GYRO_ZOUT_L      0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO      0x63
+#define I2C_SLV1_DO      0x64
+#define I2C_SLV2_DO      0x65
+#define I2C_SLV3_DO      0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET  0x68
+#define MOT_DETECT_CTRL   0x69
+#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1       0x6B  // Device defaults to the SLEEP mode
+#define PWR_MGMT_2       0x6C
+#define DMP_BANK         0x6D  // Activates a specific bank in the DMP
+#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
+#define DMP_REG_1        0x70
+#define DMP_REG_2        0x71
+#define FIFO_COUNTH      0x72
+#define FIFO_COUNTL      0x73
+#define FIFO_R_W         0x74
+#define WHO_AM_I_MPU6050 0x75 // Should return 0x68
+
+// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+#define ADO 0
+#if ADO
+#define MPU6050_ADDRESS 0x69<<1  // Device address when ADO = 1
+#else
+#define MPU6050_ADDRESS 0x68<<1  // Device address when ADO = 0
+#endif
\ No newline at end of file

diff -r 000000000000 -r 70d405bfdc8b ledControl.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ledControl.cpp	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,67 @@
+/*     
+*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+*   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 "ledControl.h"
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+// Controls the 4 LEDs on the LPC1768
+void ledControl(int ledNum,int value)
+{
+  switch(ledNum)
+  {
+    case 1: led1.write(value);
+    break;
+    
+    case 2: led2.write(value);
+    break;
+    
+    case 3: led3.write(value);
+    break;
+    
+    case 4: led4.write(value);
+    break;
+  }  
+}
+
+// Toggles the specified led
+void ledToggle(int ledNum)
+{
+  switch(ledNum)
+  {
+    case 1: led1=!led1;
+    break;
+    
+    case 2: led2=!led2;
+    break;
+    
+    case 3: led3=!led3;
+    break;
+    
+    case 4: led4=!led4;
+    break;
+  }     
+}
\ No newline at end of file

diff -r 000000000000 -r 70d405bfdc8b ledControl.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ledControl.h	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,37 @@
+/*     
+*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+*   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 LEDCONTROL_H  // "if not defined": to ensure that header code is only ever included once by the linker.
+#define LEDCONTROL_H
+
+#include "mbed.h"
+
+extern DigitalOut led1;   // allow led1 to be manipulated by other files
+extern DigitalOut led2;
+extern DigitalOut led3;
+extern DigitalOut led4;
+
+void ledControl(int ledNum,int value); // function prototype
+void ledToggle(int ledNum);
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 70d405bfdc8b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,415 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "ledControl.h"
+
+#define chan    5         // the numbers of channels
+
+Serial pc(USBTX,USBRX);    // default baud rate: 9600
+
+DigitalOut ex_led(p29);
+
+Ticker toggler1;
+
+PwmOut esc1(p21);
+PwmOut esc2(p22);
+PwmOut esc3(p23);
+PwmOut esc4(p24);
+
+void toggle_led1();
+void toggle_led2();
+
+Timer t_main;
+
+//////////////////////////////////////////////////
+// MPU6050
+//////////////////////////////////////////////////
+
+MPU6050 mpu6050;           // class: MPU6050, object: mpu6050 
+
+void compFilter();
+
+double pitch = 0;
+double roll  = 0;
+
+//////////////////////////////////////////////////
+// Read reciever
+//////////////////////////////////////////////////
+
+InterruptIn ch1(p15);       // yaw
+InterruptIn ch2(p16);       // pitch
+InterruptIn ch3(p17);       // throttle
+InterruptIn ch4(p18);       // roll
+InterruptIn ch5(p13);       // aux1
+//InterruptIn ch6(p15);       // aux2
+
+void ch1_rise();    void ch1_fall();
+void ch2_rise();    void ch2_fall();
+void ch3_rise();    void ch3_fall();
+void ch4_rise();    void ch4_fall();
+void ch5_rise();    void ch5_fall();
+//void ch6_rise();    void ch6_fall();
+
+Timer t_ch1;
+Timer t_ch2;
+Timer t_ch3;
+Timer t_ch4;
+Timer t_ch5;
+//Timer t_ch6;
+
+double pwm_in_ch1      =   0;
+double pwm_in_ch2      =   0;
+double pwm_in_ch3      =   0;
+double pwm_in_ch4      =   0;
+double pwm_in_ch5      =   0;
+//double pwm_in_ch6      =   0;
+
+int main()
+{
+    
+    int flag_mode   =   0;  // 0: disarmed, 1: armed, 2: auto
+    
+    int count_print  =   0;
+    
+    double pwm_in_mod_ch1   =   0;
+    double pwm_in_mod_ch2   =   0;
+    double pwm_in_mod_ch3   =   0;
+    double pwm_in_mod_ch4   =   0;
+    
+    //double pwm_in_cen_ch1   =   0;
+    //double pwm_in_cen_ch2   =   0;
+    double pwm_in_low_ch3   =   0;
+    //double pwm_in_cen_ch4   =   0;
+    
+    //double pwm_in_trim_ch1;
+    //double pwm_in_trim_ch2;
+    double pwm_in_trim_ch3;
+    //double pwm_in_trim_ch4;
+    
+    double input_phi    =   0;
+    double input_the    =   0;
+    double input_ome    =   0;
+    
+    // Gains
+    double GAIN_P   =   0.0000003;
+    double GAIN_Q   =   0.0000003;
+    double GAIN_R   =   0.0000026;
+    
+    double GAIN_PHI   =   0.00000213;
+    double GAIN_THE   =   0.00000213;
+    
+    double GAIN_PWM2ANGLE  =   25 / 0.0004;    // Max input -> 30 [deg] (0.0011~0.0019 [s], center: 0.0015 [s])
+    
+    double GAIN_OME =   1.2;
+    
+    //
+    double PWM_OUT_MIN  =   0.0011;
+    double pwm_out_ch1  =   PWM_OUT_MIN;
+    double pwm_out_ch2  =   PWM_OUT_MIN;
+    double pwm_out_ch3  =   PWM_OUT_MIN;
+    double pwm_out_ch4  =   PWM_OUT_MIN;
+    
+    // Rocking wings
+    double pwm_in_mod_ch3_prev  =   pwm_out_ch3;
+    
+    int count_rw     =   0;
+    int flag_rw_on   =   0;
+    int flag_rw_off  =   0;
+    
+    double phi_target_rw    =   0;
+    double roll_target_rw   =   0;
+    
+    double DT_RW_1    =   0.3;     // For 1st tilt
+    double DT_RW_2    =   0.3;     // For stable
+    double DT_RW_3    =   0.3;     // For 2nd tilt
+    
+    int COUNT_1_RW    =   DT_RW_1 * 200;              // 200 Hz
+    int COUNT_2_RW    =   COUNT_1_RW + DT_RW_2 * 200;
+    int COUNT_3_RW    =   COUNT_2_RW + DT_RW_3 * 200;
+    int COUNT_4_RW    =   COUNT_3_RW + DT_RW_2 * 200;
+    
+    double TARGET_ANGLE_RW_1 =   15;    // For 1st tilt
+    double TARGET_ANGLE_RW_2 =   5;     // for 2nd tilt
+    
+    // Set baud rate for PC
+    pc.baud(115200);                            // baud rate: 115200
+    
+    // Set reciever function
+    ch1.rise(&ch1_rise);    ch1.fall(&ch1_fall);
+    ch2.rise(&ch2_rise);    ch2.fall(&ch2_fall);
+    ch3.rise(&ch3_rise);    ch3.fall(&ch3_fall);
+    ch4.rise(&ch4_rise);    ch4.fall(&ch4_fall);
+    ch5.rise(&ch5_rise);    ch5.fall(&ch5_fall);
+    //ch6.rise(&ch6_rise);   ch6.fall(&ch6_fall);
+    
+    // Set ESC period
+    esc1.period(0.020);         // servo requires a 20ms period
+    esc2.period(0.020);         // servo requires a 20ms period
+    esc3.period(0.020);         // servo requires a 20ms period
+    esc4.period(0.020);         // servo requires a 20ms period
+    wait(1);
+    
+    led4    =   1;
+    esc1.pulsewidth(0);
+    esc2.pulsewidth(0);
+    esc3.pulsewidth(0);
+    esc4.pulsewidth(0);
+    wait(3);
+    led4    =   0;
+    
+    // Set MPU6050
+    i2c.frequency(400000);                      // fast i2c: 400 kHz
+    wait(1);
+    mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
+    pc.printf("\r\nCalibration is completed. \r\n");
+    wait(0.5);
+    mpu6050.init();                             // Initialize the sensor
+    wait(1);
+    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+    wait_ms(500);
+    
+    //////////////////////////////////////////////////
+    // Preprocessing
+    //////////////////////////////////////////////////
+    // Get trim reciever
+    for(int i_trim_rc=0; i_trim_rc<10; i_trim_rc++)
+    {
+        wait(.1);
+        //pwm_in_cen_ch1  +=   pwm_in_ch1;
+        //pwm_in_cen_ch2  +=   pwm_in_ch2;
+        pwm_in_low_ch3  +=   pwm_in_ch3;
+        //pwm_in_cen_ch4  +=   pwm_in_ch4;
+        //pc.printf("%f %f %f %f\r\n", pwm_in_ch1, pwm_in_ch2, pwm_in_ch3, pwm_in_ch4);
+    }
+    //pwm_in_cen_ch1  =   pwm_in_cen_ch1 / 10;
+    //pwm_in_cen_ch2  =   pwm_in_cen_ch2 / 10;
+    pwm_in_low_ch3  =   pwm_in_low_ch3 / 10;
+    //pwm_in_cen_ch4  =   pwm_in_cen_ch4 / 10;
+    
+    //pwm_in_trim_ch1    =   pwm_in_cen_ch1 - 0.0015;
+    //pwm_in_trim_ch2    =   pwm_in_cen_ch2 - 0.0015;
+    pwm_in_trim_ch3    =   pwm_in_low_ch3 - 0.0011;
+    //pwm_in_trim_ch4    =   pwm_in_cen_ch4 - 0.0015;
+    
+    //pc.printf("%f %f %f %f\r\n", pwm_in_cen_ch1, pwm_in_cen_ch2, pwm_in_low_ch3, pwm_in_cen_ch4);
+    
+    // Prepare ESC
+    led3    =   1;
+    esc1.pulsewidth(PWM_OUT_MIN);
+    esc2.pulsewidth(PWM_OUT_MIN);
+    esc3.pulsewidth(PWM_OUT_MIN);
+    esc4.pulsewidth(PWM_OUT_MIN);
+    wait(5);
+    led3    =   0;
+    
+    // LED for auto
+    ex_led  =   0;
+    
+    //double dt_check =   0;
+    //t_main.start();
+    
+    // Main loop
+    while(1)
+    {
+        
+        if((0.00165<pwm_in_ch5 || flag_rw_on==1) && flag_rw_off==0)
+        {
+            flag_mode   =   2;
+            flag_rw_on  =   1;
+            ex_led      =   1;
+        }
+        else if(0.00165<pwm_in_ch5 && flag_rw_off==1) { flag_mode   =   1;  }
+        else if(0.00135<pwm_in_ch5 && pwm_in_ch5<0.00165)
+        {
+            flag_mode   =   1;
+            flag_rw_off =   0;
+        }
+        else { flag_mode   =   0;  }
+        
+        // Update sensors
+        mpu6050.complementaryFilter(&pitch, &roll);
+        
+        //
+        pwm_in_mod_ch1  =   pwm_in_ch1;
+        pwm_in_mod_ch2  =   pwm_in_ch2;
+        pwm_in_mod_ch3  =   pwm_in_ch3 - pwm_in_trim_ch3;
+        pwm_in_mod_ch4  =   pwm_in_ch4;
+        
+        switch(flag_mode)
+        {
+            case 0:
+                
+                esc1.pulsewidth(PWM_OUT_MIN);
+                esc2.pulsewidth(PWM_OUT_MIN);
+                esc3.pulsewidth(PWM_OUT_MIN);
+                esc4.pulsewidth(PWM_OUT_MIN);
+                
+                wait(0.004468);
+                
+                break;
+                
+            case 1:
+                
+                // Control law (ch1: yaw, ch2: pitch, ch3: thr, ch4: roll)
+                input_phi =   - GAIN_P * gx + GAIN_PHI * (GAIN_PWM2ANGLE*(pwm_in_mod_ch2-0.0015) - pitch);
+                input_the =   - GAIN_Q * gy + GAIN_THE * (GAIN_PWM2ANGLE*(pwm_in_mod_ch4-0.0015) + roll);
+                input_ome =   - GAIN_R * gz + GAIN_OME * (pwm_in_mod_ch1-0.0015);
+                
+                // ch1: left_front, ch3: right_front, ch4: left_rear, ch2: right_rear 
+                pwm_out_ch1  =   pwm_in_mod_ch3 + input_phi + input_the + input_ome;
+                pwm_out_ch3  =   pwm_in_mod_ch3 + input_phi - input_the - input_ome;
+                pwm_out_ch4  =   pwm_in_mod_ch3 - input_phi + input_the - input_ome;
+                pwm_out_ch2  =   pwm_in_mod_ch3 - input_phi - input_the + input_ome;
+                
+                // Limit pwm out
+                if(pwm_out_ch1<0.00121){ pwm_out_ch1 =   0.00121; }
+                if(pwm_out_ch2<0.00121){ pwm_out_ch2 =   0.00121; }
+                if(pwm_out_ch3<0.00121){ pwm_out_ch3 =   0.00121; }
+                if(pwm_out_ch4<0.00121){ pwm_out_ch4 =   0.00121; }
+                
+                if(0.0019<pwm_out_ch1){ pwm_out_ch1 =   0.0019; }
+                if(0.0019<pwm_out_ch2){ pwm_out_ch2 =   0.0019; }
+                if(0.0019<pwm_out_ch3){ pwm_out_ch3 =   0.0019; }
+                if(0.0019<pwm_out_ch4){ pwm_out_ch4 =   0.0019; }
+                
+                // Set pwm out (1-2ms)
+                esc1.pulsewidth(pwm_out_ch1);
+                esc2.pulsewidth(pwm_out_ch2);
+                esc3.pulsewidth(pwm_out_ch3);
+                esc4.pulsewidth(pwm_out_ch4);
+                
+                wait(0.004436);
+                
+                break;
+                
+            case 2:
+                
+                // Inputs
+                if(count_rw<COUNT_1_RW) {  roll_target_rw  =   - TARGET_ANGLE_RW_1;   }
+                else if(COUNT_1_RW<count_rw && count_rw<COUNT_2_RW) {  roll_target_rw  =   0; }
+                else if(COUNT_2_RW<=count_rw && count_rw<COUNT_3_RW) {  roll_target_rw  =   TARGET_ANGLE_RW_1;  }
+                else if(COUNT_3_RW<=count_rw && count_rw<COUNT_4_RW) {  roll_target_rw  =   -TARGET_ANGLE_RW_2; }
+                
+                // Control law (ch1: yaw, ch2: pitch, ch3: thr, ch4: roll)
+                input_phi =   - GAIN_P * gx + GAIN_PHI * (phi_target_rw - pitch);
+                input_the =   - GAIN_Q * gy + GAIN_THE * (roll_target_rw + roll);
+                input_ome =   - GAIN_R * gz + GAIN_OME * (pwm_in_mod_ch1-0.0015);
+                
+                // ch1: left_front, ch3: right_front, ch4: left_rear, ch2: right_rear 
+                pwm_out_ch1  =   pwm_in_mod_ch3_prev + input_phi + input_the + input_ome;
+                pwm_out_ch3  =   pwm_in_mod_ch3_prev + input_phi - input_the - input_ome;
+                pwm_out_ch4  =   pwm_in_mod_ch3_prev - input_phi + input_the - input_ome;
+                pwm_out_ch2  =   pwm_in_mod_ch3_prev - input_phi - input_the + input_ome;
+                
+                // Limit pwm out
+                if(pwm_out_ch1<0.00121){ pwm_out_ch1 =   0.00121; }
+                if(pwm_out_ch2<0.00121){ pwm_out_ch2 =   0.00121; }
+                if(pwm_out_ch3<0.00121){ pwm_out_ch3 =   0.00121; }
+                if(pwm_out_ch4<0.00121){ pwm_out_ch4 =   0.00121; }
+                
+                if(0.0019<pwm_out_ch1){ pwm_out_ch1 =   0.0019; }
+                if(0.0019<pwm_out_ch2){ pwm_out_ch2 =   0.0019; }
+                if(0.0019<pwm_out_ch3){ pwm_out_ch3 =   0.0019; }
+                if(0.0019<pwm_out_ch4){ pwm_out_ch4 =   0.0019; }
+                
+                // Set pwm out (1-2ms)
+                esc1.pulsewidth(pwm_out_ch1);
+                esc2.pulsewidth(pwm_out_ch2);
+                esc3.pulsewidth(pwm_out_ch3);
+                esc4.pulsewidth(pwm_out_ch4);
+                
+                count_rw++;
+                
+                wait(0.004445);
+                
+                if(COUNT_3_RW<=count_rw)
+                {
+                    flag_rw_on  =   0;
+                    flag_rw_off =   1;
+                    count_rw    =   0;
+                    ex_led      =   0;
+                }
+                
+                break;
+                
+            default:
+                
+                led1    =   1;                  led2    =   1;                  led3    =   1;                  led4    =   1;
+                esc1.pulsewidth(PWM_OUT_MIN);   esc2.pulsewidth(PWM_OUT_MIN);   esc3.pulsewidth(PWM_OUT_MIN);   esc4.pulsewidth(PWM_OUT_MIN);
+                
+            }
+            
+            pwm_in_mod_ch3_prev  =   pwm_in_mod_ch3;
+            /*
+            if(200<count_print)
+            {
+                //pc.printf("%9.6f, %9.6f, %9.6f, %9.6f\r\n", pwm_in_mod_ch1, pwm_in_mod_ch2, pwm_in_mod_ch3, pwm_in_mod_ch4);
+                //pc.printf("%7.3f, %7.3f, %7.3f, %7.3f, %7.3f\r\n", gx, gy, gz, pitch, roll);
+                //pc.printf("%d, %3.0f, %7.3f, %7.3f, %13.10f, %13.10f\r\n", flag_mode, roll_target_rw, pitch, roll, input_phi, input_the);
+                pc.printf("%d, %3.0f, %4.0f, %4.0f, %4.0f, %4.0f\r\n", flag_mode, roll_target_rw, pwm_out_ch1*1000000, pwm_out_ch2*1000000, pwm_out_ch3*1000000, pwm_out_ch4*1000000);
+                
+                //t_main.stop();
+                //dt_check    =   t_main.read();
+                //pc.printf("%8.6f\r\n", dt_check);
+                //t_main.reset();
+                //t_main.start();
+                
+                count_print =   0;
+            }
+            count_print++;
+            */
+    }
+}
+
+void toggle_led1() {ledToggle(1);}
+void toggle_led2() {ledToggle(2);}
+
+//////////////////////////////////////////////////
+// Functions for read ESC
+//////////////////////////////////////////////////
+
+void ch1_rise() {    t_ch1.start();   }
+void ch2_rise() {    t_ch2.start();   }
+void ch3_rise() {    t_ch3.start();   }
+void ch4_rise() {    t_ch4.start();   }
+void ch5_rise() {    t_ch5.start();   }
+//void ch6_rise() {    t_ch6.start();   }
+
+void ch1_fall(){
+    t_ch1.stop();
+    pwm_in_ch1    =   t_ch1.read();
+    t_ch1.reset();
+}
+
+void ch2_fall(){
+    t_ch2.stop();
+    pwm_in_ch2    =   t_ch2.read();
+    t_ch2.reset();
+}
+
+void ch3_fall(){
+    t_ch3.stop();
+    pwm_in_ch3    =   t_ch3.read();
+    t_ch3.reset();
+    //pc.printf("%f\r\n", pwm_in_ch3);
+}
+
+void ch4_fall(){
+    t_ch4.stop();
+    pwm_in_ch4    =   t_ch4.read();
+    t_ch4.reset();
+}
+
+void ch5_fall()
+{
+    t_ch5.stop();
+    pwm_in_ch5    =   t_ch5.read();
+    t_ch5.reset();
+}
+/*
+void ch6_fall()
+{
+    t_ch6.stop();
+    pwm_in_ch6    =   t_ch6.read();
+    t_ch6.reset();
+}
+*/
\ No newline at end of file

diff -r 000000000000 -r 70d405bfdc8b mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Oct 18 09:58:59 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/ba1f97679dad
\ No newline at end of file