Revision 2:01ca44dd3908, committed 2018-12-04

Comitter:

sunninety1

Date:

Tue Dec 04 20:24:04 2018 +0000

Parent:

1:f8ed70330216

Commit message:

kk

Changed in this revision

diff -r f8ed70330216 -r 01ca44dd3908 MPU6050.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.cpp	Tue Dec 04 20:24:04 2018 +0000
@@ -0,0 +1,275 @@
+/**
+ * Includes
+ */
+#include "MPU6050.h"
+
+MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
+    this->setSleepMode(false);
+    
+    //Initializations:
+    currentGyroRange = 0;
+    currentAcceleroRange=0;
+    MPU6050_ADDRESS=0x68;
+    //connection.frequency(400000); //mesures ne fonctionnent pas
+    // test I2C connexion and update MPU6050_ADDRESS
+    isFindMPU6050=I2C_finder();
+    // if MPU6050 not found stop the program !!!!!
+    if(!isFindMPU6050)
+    {
+         DigitalOut myled(LED1);
+        printf("MPU6050 doesn't exist\n\r");
+        printf("Check Connexion SCL and SDA and Power supply of your sensor\n\r");    
+        while(1) {
+            myled = 1;
+            wait(0.2);
+            myled = 0;
+            wait(0.2);
+        }
+    }
+}
+
+//--------------------------------------------------
+//-------------------General------------------------
+//--------------------------------------------------
+
+bool MPU6050::I2C_finder(){
+    int i,isOK=1;
+    char data;
+    bool isFindSlave=false;
+    printf("test I2C\n\r");
+    for(i=1;i<127;i++)
+    {   wait(0.001);
+        isOK=connection.read(i*2,&data,1,false);   
+        if(isOK==0){
+            printf("slave I2C find : 0X%X\n\r",i);
+            if(i==MPU6050_ADDRESS || i==MPU6050_ADDRESS+1){
+                printf("MPU6050 has been found : 0x%X\n\r",i);
+                MPU6050_ADDRESS=i; // on met l'adresse du MPU6050 = 0x68 ou 0x69
+                isFindSlave=true;
+            }
+        }        
+    } 
+    return isFindSlave;  
+}
+
+void MPU6050::write(char address, char data) {
+    char temp[2];
+    temp[0]=address;
+    temp[1]=data;
+    
+    connection.write(MPU6050_ADDRESS * 2,temp,2);
+}
+
+char MPU6050::read(char address) {
+    char retval;
+    connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
+    connection.read(MPU6050_ADDRESS * 2, &retval, 1);
+    return retval;
+}
+
+void MPU6050::read(char address, char *data, int length) {
+    connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
+    connection.read(MPU6050_ADDRESS * 2, data, length);
+}
+
+void MPU6050::setSleepMode(bool state) {
+    char temp;
+    temp = this->read(MPU6050_PWR_MGMT_1_REG);
+    if (state == true)
+        temp |= 1<<MPU6050_SLP_BIT;
+    if (state == false)
+        temp &= ~(1<<MPU6050_SLP_BIT);
+    this->write(MPU6050_PWR_MGMT_1_REG, temp);
+}
+
+bool MPU6050::testConnection( void ) {
+    char temp=0;
+    temp = this->read(MPU6050_WHO_AM_I_REG);
+    printf(" WHO AM I : 0X%X\n\r",temp);
+    return (temp == (MPU6050_ADDRESS & 0xFE));
+}
+
+void MPU6050::setBW(char BW) {
+    char temp;
+    BW=BW & 0x07;
+    temp = this->read(MPU6050_CONFIG_REG);
+    temp &= 0xF8;
+    temp = temp + BW;
+    this->write(MPU6050_CONFIG_REG, temp);
+}
+
+void MPU6050::setI2CBypass(bool state) {
+    char temp;
+    temp = this->read(MPU6050_INT_PIN_CFG);
+    if (state == true)
+        temp |= 1<<MPU6050_BYPASS_BIT;
+    if (state == false)
+        temp &= ~(1<<MPU6050_BYPASS_BIT);
+    this->write(MPU6050_INT_PIN_CFG, temp);
+}
+
+//--------------------------------------------------
+//----------------Accelerometer---------------------
+//--------------------------------------------------
+
+void MPU6050::setAcceleroRange( char range ) {
+    char temp;
+    range = range & 0x03;
+    currentAcceleroRange = range;
+    
+    temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
+    temp &= ~(3<<3);
+    temp = temp + (range<<3);
+    this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
+}
+
+int MPU6050::getAcceleroRawX( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+    
+int MPU6050::getAcceleroRawY( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU6050::getAcceleroRawZ( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU6050::getAcceleroRaw( int *data ) {
+    char temp[6];
+    this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
+    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU6050::getAccelero( float *data ) {
+    int temp[3];
+    this->getAcceleroRaw(temp);
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
+        data[0]=(float)temp[0] / 16384.0 * 9.81;
+        data[1]=(float)temp[1] / 16384.0 * 9.81;
+        data[2]=(float)temp[2] / 16384.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
+        data[0]=(float)temp[0] / 8192.0 * 9.81;
+        data[1]=(float)temp[1] / 8192.0 * 9.81;
+        data[2]=(float)temp[2] / 8192.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
+        data[0]=(float)temp[0] / 4096.0 * 9.81;
+        data[1]=(float)temp[1] / 4096.0 * 9.81;
+        data[2]=(float)temp[2] / 4096.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
+        data[0]=(float)temp[0] / 2048.0 * 9.81;
+        data[1]=(float)temp[1] / 2048.0 * 9.81;
+        data[2]=(float)temp[2] / 2048.0 * 9.81;
+        }
+    
+    #ifdef DOUBLE_ACCELERO
+        data[0]*=2;
+        data[1]*=2;   
+        data[2]*=2;
+    #endif   
+}
+
+//--------------------------------------------------
+//------------------Gyroscope-----------------------
+//--------------------------------------------------
+void MPU6050::setGyroRange( char range ) {
+    char temp;
+    currentGyroRange = range;
+    range = range & 0x03;
+    temp = this->read(MPU6050_GYRO_CONFIG_REG);
+    temp &= ~(3<<3);
+    temp = temp + range<<3;
+    this->write(MPU6050_GYRO_CONFIG_REG, temp);
+}
+
+int MPU6050::getGyroRawX( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+    
+int MPU6050::getGyroRawY( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU6050::getGyroRawZ( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU6050::getGyroRaw( int *data ) {
+    char temp[6];
+    this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
+    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU6050::getGyro( float *data ) {
+    int temp[3];
+    this->getGyroRaw(temp);
+    if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
+        data[0]=(float)temp[0] / 7505.7;
+        data[1]=(float)temp[1] / 7505.7;
+        data[2]=(float)temp[2] / 7505.7;
+        }
+    if (currentGyroRange == MPU6050_GYRO_RANGE_500){
+        data[0]=(float)temp[0] / 3752.9;
+        data[1]=(float)temp[1] / 3752.9;
+        data[2]=(float)temp[2] / 3752.9;
+        }
+    if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
+        data[0]=(float)temp[0] / 1879.3;;
+        data[1]=(float)temp[1] / 1879.3;
+        data[2]=(float)temp[2] / 1879.3;
+        }
+    if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
+        data[0]=(float)temp[0] / 939.7;
+        data[1]=(float)temp[1] / 939.7;
+        data[2]=(float)temp[2] / 939.7;
+        }
+}
+//--------------------------------------------------
+//-------------------Temperature--------------------
+//--------------------------------------------------
+int MPU6050::getTempRaw( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_TEMP_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+float MPU6050::getTemp( void ) {
+    float retval;
+    retval=(float)this->getTempRaw();
+    retval=(retval+521.0)/340.0+35.0;
+    return retval;
+}
+

diff -r f8ed70330216 -r 01ca44dd3908 MPU6050.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.h	Tue Dec 04 20:24:04 2018 +0000
@@ -0,0 +1,282 @@
+/*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output. 
+If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
+*/
+
+
+#ifndef MPU6050_H
+#define MPU6050_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+
+/**
+ * Defines */
+//modif prog by salvat 
+#ifndef _MPU6050_ADDRESS
+    #define _MPU6050_ADDRESS     
+
+
+#endif
+
+#ifdef MPU6050_ES
+        #define DOUBLE_ACCELERO
+#endif  
+
+/**
+ * Registers
+ */
+ #define MPU6050_CONFIG_REG         0x1A
+ #define MPU6050_GYRO_CONFIG_REG    0x1B
+ #define MPU6050_ACCELERO_CONFIG_REG    0x1C
+  
+ #define MPU6050_INT_PIN_CFG        0x37
+ 
+ #define MPU6050_ACCEL_XOUT_H_REG   0x3B
+ #define MPU6050_ACCEL_YOUT_H_REG   0x3D
+ #define MPU6050_ACCEL_ZOUT_H_REG   0x3F
+ 
+ #define MPU6050_TEMP_H_REG         0x41
+ 
+ #define MPU6050_GYRO_XOUT_H_REG    0x43
+ #define MPU6050_GYRO_YOUT_H_REG    0x45
+ #define MPU6050_GYRO_ZOUT_H_REG    0x47
+ 
+ 
+ 
+ #define MPU6050_PWR_MGMT_1_REG     0x6B
+ #define MPU6050_WHO_AM_I_REG       0x75
+ 
+                 
+ 
+ /**
+  * Definitions
+  */
+#define MPU6050_SLP_BIT             6
+#define MPU6050_BYPASS_BIT         1
+
+#define MPU6050_BW_256              0
+#define MPU6050_BW_188              1
+#define MPU6050_BW_98               2
+#define MPU6050_BW_42               3
+#define MPU6050_BW_20               4
+#define MPU6050_BW_10               5
+#define MPU6050_BW_5                6
+
+#define MPU6050_ACCELERO_RANGE_2G   0
+#define MPU6050_ACCELERO_RANGE_4G   1
+#define MPU6050_ACCELERO_RANGE_8G   2
+#define MPU6050_ACCELERO_RANGE_16G  3
+
+#define MPU6050_GYRO_RANGE_250      0
+#define MPU6050_GYRO_RANGE_500      1
+#define MPU6050_GYRO_RANGE_1000     2
+#define MPU6050_GYRO_RANGE_2000     3
+
+
+/** MPU6050 IMU library.
+  *
+  * Example:
+  * @code
+  * Later, maybe
+  * @endcode
+  */
+class MPU6050 {
+    public:
+     /**
+     * Constructor.
+     *
+     * Sleep mode of MPU6050 is immediatly disabled
+     *
+     * @param sda - mbed pin to use for the SDA I2C line.
+     * @param scl - mbed pin to use for the SCL I2C line.
+     */
+     MPU6050(PinName sda, PinName scl);
+     
+
+     /**
+     * Tests the I2C connection by reading the WHO_AM_I register. 
+     *
+     * @return True for a working connection, false for an error
+     */     
+     bool testConnection( void );
+     
+     /**
+     * Sets the bandwidth of the digital low-pass filter 
+     *
+     * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5
+     * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
+     *
+     * @param BW - The three bits that set the bandwidth (use the predefined macros)
+     */     
+     void setBW( char BW );
+     
+     /**
+     * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus) 
+     *
+     * @param state - Enables/disables the I2C bypass mode
+     */     
+     void setI2CBypass ( bool state );
+     
+     /**
+     * Sets the Accelero full-scale range
+     *
+     * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G
+     *
+     * @param range - The two bits that set the full-scale range (use the predefined macros)
+     */
+     void setAcceleroRange(char range);
+     
+     /**
+     * Reads the accelero x-axis.
+     *
+     * @return 16-bit signed integer x-axis accelero data
+     */   
+     int getAcceleroRawX( void );
+     
+     /**
+     * Reads the accelero y-axis.
+     *
+     * @return 16-bit signed integer y-axis accelero data
+     */   
+     int getAcceleroRawY( void );
+     
+     /**
+     * Reads the accelero z-axis.
+     *
+     * @return 16-bit signed integer z-axis accelero data
+     */   
+     int getAcceleroRawZ( void );
+     
+     /**
+     * Reads all accelero data.
+     *
+     * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getAcceleroRaw( int *data );
+     
+     /**
+     * Reads all accelero data, gives the acceleration in m/s2
+     *
+     * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+     *
+     * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getAccelero( float *data );
+     
+     /**
+     * Sets the Gyro full-scale range
+     *
+     * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000
+     *
+     * @param range - The two bits that set the full-scale range (use the predefined macros)
+     */
+     void setGyroRange(char range);
+
+     /**
+     * Reads the gyro x-axis.
+     *
+     * @return 16-bit signed integer x-axis gyro data
+     */   
+     int getGyroRawX( void );
+     
+     /**
+     * Reads the gyro y-axis.
+     *
+     * @return 16-bit signed integer y-axis gyro data
+     */   
+     int getGyroRawY( void );
+     
+     /**
+     * Reads the gyro z-axis.
+     *
+     * @return 16-bit signed integer z-axis gyro data
+     */   
+     int getGyroRawZ( void );
+     
+     /**
+     * Reads all gyro data.
+     *
+     * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getGyroRaw( int *data );  
+     
+     /**
+     * Reads all gyro data, gives the gyro in rad/s
+     *
+     * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+     *
+     * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getGyro( float *data);     
+     
+     /**
+     * Reads temperature data.
+     *
+     * @return 16 bit signed integer with the raw temperature register value
+     */  
+     int getTempRaw( void );
+     
+     /**
+     * Returns current temperature
+     *
+     * @returns float with the current temperature
+     */  
+     float getTemp( void );
+
+     /**
+     * Sets the sleep mode of the MPU6050 
+     *
+     * @param state - true for sleeping, false for wake up
+     */     
+     void setSleepMode( bool state );
+     
+     
+     /**
+     * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU. 
+     *
+     * @param adress - register address to write to
+     * @param data - data to write
+     */
+     void write( char address, char data);
+     
+     /**
+     * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU. 
+     *
+     * @param adress - register address to write to
+     * @return - data from the register specified by RA
+     */
+     char read( char adress);
+     
+     /**
+     * Read multtiple regigsters from the device, more efficient than using multiple normal reads. 
+     *
+     * @param adress - register address to write to
+     * @param length - number of bytes to read
+     * @param data - pointer where the data needs to be written to 
+     */
+     void read( char adress, char *data, int length);
+     
+     /**
+     * Find all I2C devices and print all devices I2C + update MPU6050_ADDRESS (choice between 0x68 and 0X69)
+     *
+     */
+    bool I2C_finder();    
+        
+     private:
+     I2C connection;
+     char currentAcceleroRange;
+     char currentGyroRange;
+// add by salvat : permet de trouver l'adresse du MPU6050 dans testConnexion
+     char MPU6050_ADDRESS;// address pin low (GND), default for InvenSense evaluation board
+     bool isFindMPU6050;
+     
+     
+
+};
+
+
+
+#endif

diff -r f8ed70330216 -r 01ca44dd3908 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 04 20:24:04 2018 +0000
@@ -0,0 +1,150 @@
+
+
+#include "mbed.h"
+#include "MPU6050.h"
+#include "math.h"
+MPU6050 mpu(D4,D5);
+Serial pc(USBTX, USBRX); // tx, rx
+Timer t;
+Serial  serial(PA_9,PA_10);
+char    data[30];
+
+//DigitalIn res(USER_BUTTON);
+
+float ark[3];
+    float gy[3];
+    
+    int timestamp=0;
+    float Now,lastupdate;
+float cali_ax,cali_ay,cali_az,cali_gx,cali_gy,cali_gz;
+    static float v[3]={0,0,0};
+    
+float PI = 3.14159265358979323846f;
+void MPUcalibate(){
+    static float axx=0,ayy=0,azz=0,gxx=0,gyy=0,gzz=0;
+    int x=0;
+    while(x<=9999) {
+        mpu.getAccelero(ark);
+        mpu.getGyro(gy);
+        axx+=ark[0];
+        ayy+=ark[1];
+        azz+=ark[2];
+        gxx+=gy[0];
+        gyy+=gy[1];
+        gzz+=gy[2];
+        x+=1;
+        wait(0.0001);
+    }
+    cali_ax=axx/10000.0f;
+    cali_ay=ayy/10000.0f;
+    cali_az=azz/10000.0f;
+    cali_gx=gxx/10000.0f;
+    cali_gy=gyy/10000.0f;
+    cali_gz=gzz/10000.0f;
+    //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay,cali_az-9.80665);
+    //pc.printf("%.10f %.10f %.10f\n",cali_ax-9.80665,cali_ay,cali_az);
+    //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay-9.80665,cali_az);
+    //pc.printf("%f %f %f\n",cali_gx,cali_gy,cali_gz);
+    }
+void AccelToVelo(float ax,float ay,float az,float dt){
+    v[0]+=(ax*dt/129); //(-156.10655212402344+150.74868774414062);
+    v[1]+=(ay*dt/180); //(-208.04095458984375+194.97883605957031);
+    v[2]+=(az*dt/6); //(151.52879333496094-144.20004272460938);
+    }
+/*float CalculateAngle(float ax,float ay,float az,float gx,float gy,float gz){
+    float pitch;
+    gx=0;
+    gy=0;
+    gz=0;
+    static float fillter_pitch=0;
+    fillter_pitch=(fillter_pitch*0.8)+(pitch*0.2);
+    pitch=atan((ax)/sqrt((ay)*(ay)+(az)*(az))*(180/PI));
+    //pc.printf("%f\n",pitch);
+    return fillter_pitch;
+    }*/
+int main()
+{
+    pc.baud(115200);
+    static float calibate_acc[3]={0.6465099045, 0.14739338841, -0.2634094528};
+    static float calibate_gy[3]={-0.022125, 0.005254 ,-0.009761};
+    mpu.getAccelero(ark);
+        mpu.getGyro(gy);
+    //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
+    static float fillter_pitch=0;
+    int co=0;
+    float pitch;
+    float delay=0.0001,current=0.0,deltat;
+    t.start();
+    
+    wait(0.5);
+    
+    //MPUcalibate();
+    //cali_err=atan((-(ark[1]-cali_ay))/(ark[2]-cali_az));
+    //cali_p=atan2(((ark[2]-cali_az)),(ark[0]-cali_ax));
+    //cali_p=atan((ark[0]-cali_ax)/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)));
+    //pc.printf("cali P %f \n",cali_err);
+    //wait(1);
+      /*  fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
+    fillter_pitch3=(fillter_pitch3*0.75)+(fillter_pitch2*0.25);
+    fillter_pitch4=(fillter_pitch4*0.75)+(fillter_pitch3*0.25);
+    fillter_pitch5=(fillter_pitch5*0.75)+(fillter_pitch4*0.25);
+    fillter_pitch6=(fillter_pitch6*0.75)+(fillter_pitch5*0.25);
+    fillter_pitch7=(fillter_pitch7*0.75)+(fillter_pitch6*0.25);
+    fillter_pitch8=(fillter_pitch8*0.75)+(fillter_pitch7*0.25);
+    fillter_pitch9=(fillter_pitch9*0.75)+(fillter_pitch8*0.25);
+    fillter_pitch10=(fillter_pitch10*0.75)+(fillter_pitch9*0.25);*/
+    while(1) {
+        Now = t.read();
+        deltat=Now-current;
+        if (deltat>0.1){
+        mpu.getAccelero(ark);
+        mpu.getGyro(gy);
+        ark[0] -= calibate_acc[0];
+        ark[1] -= calibate_acc[1];
+        ark[2] -= calibate_acc[2];
+        gy[0] -= calibate_gy[0];
+        gy[1] -= calibate_gy[1];
+        gy[2] -= calibate_gy[2];
+        float g=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
+        while(co<1000){
+        AccelToVelo((ark[0]),ark[1],ark[2]-9.8065,deltat);
+        co+=1;
+        }
+        co=0;
+        ark[0]+=0.5;
+        float gra=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
+        pc.printf("Q AX %f AY %f AZ %f G %f \n",ark[0],ark[1],ark[2],gra);
+        if(serial.writeable()) {
+            serial.printf("%f %f %f %f \n",ark[0],ark[1],ark[2],gra);
+            //serial.printf("------------------------ \n");
+            //serial.printf("   Gyx:%f_Gyy:%f_Gyz:%f \n",gy[0],gy[1],gy[2]);
+            wait_ms(500);   // lag to debounce the button
+    }
+        //pc.printf("Q GyX %f GyY %f GyZ %f \n",gy[0],gy[1],gy[2]);
+        //pc.printf("Q Vx %f Vy %f Vz %f \n",v[0],v[1],v[2]);
+        //pc.printf("%.14f\n",v[1]/100);
+        pc.printf("Q AX %f AY %f AZ %f GyX %f GyY %f GyZ %f \n",ark[0]+0.5,ark[1],ark[2],gy[0],gy[1],gy[2]);
+        //timestamp+=1;
+        //pc.printf("%f\n",atan((-(ark[1]-cali_ay))/(ark[2]-cali_az)));
+        //pc.printf("Ax %f\n",(atan2((ark[2]-cali_az),(ark[0]-cali_ax))-cali_p)*(180/PI));
+        //pitch=atan((ark[0])/sqrt((ark[1])*(ark[1])+(ark[2])*ark[2]))*(180/PI);
+        //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
+        //pc.printf("%lf\n",CalculateAngle(ark[0],ark[1],ark[2],0,0,0));
+        pitch=atan(ark[0]/sqrt(pow(ark[1],2)+pow(ark[2],2)))*(180/PI);
+    //fillter_pitch=(fillter_pitch*0.75)+(pitch*0.25);
+   // fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
+   // pc.printf("%f\n",fillter_pitch);
+        //pc.printf("Ax %f\n",(atan((ark[0])/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)))-cali_p)*(180/PI));
+       // pc.printf("%.10f %.10f %.10f\n",ark[0],ark[1],ark[2]);
+        //pc.printf("%f %f %f\n",gy[0],gy[1],gy[2]);
+        //pc.printf("%.2f\n",t.read());
+        wait(0.02);
+        /*if (res==0) {
+            //lastupdate=t.read()-Now;
+            //Now=lastupdate;
+            t.reset();
+        }*/
+current=Now;
+}
+    }
+}

diff -r f8ed70330216 -r 01ca44dd3908 test.cpp
--- a/test.cpp	Tue Dec 04 04:38:15 2018 +0000
+++ b/test.cpp	Tue Dec 04 20:24:04 2018 +0000
@@ -1,5 +1,5 @@
 
-
+/*
 #include "mbed.h"
 
 Serial        serial(D8, D2);
@@ -17,7 +17,7 @@
         
     while(1) {}
 }
-
+*/
 /*
 #include "mbed.h"