File content as of revision 2:8eb96314ced5:

/* shalabh bhatnagar and neeraj rathi
 
 Hardware setup:
 MPU6050 Breakout --------- Arduino
 3.3V --------------------- 3.3V
 SDA ----------------------- A4
 SCL ----------------------- A5
 GND ---------------------- GND
 
  Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library. 
 Because the sensor is not 5V tolerant, we are using a 3.3 V
 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L
 */
 
#include "mbed.h"
#include "MPU6050.h"
#include "Servo.h"
#include "ADXL345.h"
ADXL345 accelerometer(PTD2, PTD3, PTD1,PTB9);
Servo myservo(PTA1);
float idif;
float ax1;
float srb = 7;
   MPU6050 mpu6050;

   Timer t;

   Serial pc(USBTX, USBRX); 

int main()
{
  
    int readings[3] = {0, 0, 0};
    
    pc.printf("Starting ADXL345 test...\n");
    pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());

    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);

    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
    
    //3.2kHz data rate.
    accelerometer.setDataRate(ADXL345_3200HZ);

    //Measurement mode.
    accelerometer.setPowerControl(0x08);
    
    pc.baud(9600);  

  //Set up I2C
  i2c.frequency(400000);  // use fast (400 kHz) I2C   
  
  t.start();        
      
  // Read the WHO_AM_I register, this is a good test of communication
  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
  pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");

  if (whoami == 0x68) // WHO_AM_I should always be 0x68
  { 
    pc.printf("MPU6050 is online...");
    wait(1);
    
    mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
    pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
    pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
    pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
    pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
    pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
    pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
    wait(1);
ax1=(float)accelCount[0]*aRes - accelBias[0];
    if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
    {
    mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
    mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
    mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

    wait(2);
       }
    else
    {
    pc.printf("Device did not the pass self-test!\n\r");
      }
    }
    else
    {
    pc.printf("Could not connect to MPU6050: \n\r");
    pc.printf("%#x \n",  whoami);
 
    
 
    while(1) ; // Loop forever if communication doesn't happen
  }

 while(1) {
     
        wait(0.1);
        
        accelerometer.getOutput(readings);
        
        //13-bit, sign extended values.
        pc.printf("%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);

  //idif = ax - ax1;
  //if (idif > srb)
 //pc.printf( "Up Up Up = %d", idif);
 //else if (idif < 0)
 //pc.printf("down down down = %d", idif);
  //else
  //pc.printf("it is less %d", idif);  
  // If data ready bit set, all data registers have new data
  if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
    mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
    mpu6050.getAres();
    // Now we'll calculate the accleration value into actual g's
    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
    ay = (float)accelCount[1]*aRes - accelBias[1];   
    az = (float)accelCount[2]*aRes - accelBias[2];  
    Now = t.read_us();
    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
    lastUpdate = Now;
    if(lastUpdate - firstUpdate > 10000000.0f)
     {
     beta = 0.04;  // decrease filter gain after stabilized
     zeta = 0.015; // increasey bias drift gain after stabilized
    }
    // Serial print and/or display at 0.5 s rate independent of data rates
    delt_t = t.read_ms() - count;
    if (delt_t > 500) { // update LCD once per half-second independent of read rate
    pc.printf("ax = %f", 100*ax); 
    pc.printf(" ay = %f", 100*ay); 
    pc.printf(" az = %f  mg\n\r", 100*az); 
idif = ax - ax1;
pc.printf( "earlier ax1 = %f", ax1);
pc.printf("difference = %f", idif);
if (idif <7 && myservo != 1)
{
pc.printf("up up up = %f", idif);
for(int i=0; i<100;i++)
{
       myservo = i/100.0;
wait(0.1);
}
myservo = 1;
idif = 0;
//myservo = 100.0;
}
//pc.printf("the value of my servo for up up up = %d",myservo);}}
//pc.printf("up up up = %f", idif);
if (idif <-7.0&& myservo != 0)
{pc.printf("down down down = %f", idif);
for (int i= 100;i>0;i--)
{myservo = i/100.0;
wait(0.1);
}
myservo = 0.0;
idif = 0.0;}
//pc.printf("the value of my servo for down = %d",myservo);
//pc.printf("down down down = %f", idif);
ax1 = (float)ax*100;
//ax1 = (float)accelCount[0]*aRes - accelBias[0];
pc.printf("current ax1= %f",ax1);   
 
 wait(1);
 
   }
    }
 } 
}