main.cpp

00001 /* shalabh bhatnagar and neeraj rathi
00002  
00003  Hardware setup:
00004  MPU6050 Breakout --------- Arduino
00005  3.3V --------------------- 3.3V
00006  SDA ----------------------- A4
00007  SCL ----------------------- A5
00008  GND ---------------------- GND
00009  
00010   Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library. 
00011  Because the sensor is not 5V tolerant, we are using a 3.3 V
00012  We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L
00013  */
00014  
00015 #include "mbed.h"
00016 #include "MPU6050.h"
00017 #include "Servo.h"
00018 #include "ADXL345.h"
00019 ADXL345 accelerometer(PTD2, PTD3, PTD1,PTB9);
00020 Servo myservo(PTA1);
00021 float idif;
00022 float ax1;
00023 float srb = 7;
00024    MPU6050 mpu6050;
00025 
00026    Timer t;
00027 
00028    Serial pc(USBTX, USBRX); 
00029 
00030 int main()
00031 {
00032   
00033     int readings[3] = {0, 0, 0};
00034     
00035     pc.printf("Starting ADXL345 test...\n");
00036     pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
00037 
00038     //Go into standby mode to configure the device.
00039     accelerometer.setPowerControl(0x00);
00040 
00041     //Full resolution, +/-16g, 4mg/LSB.
00042     accelerometer.setDataFormatControl(0x0B);
00043     
00044     //3.2kHz data rate.
00045     accelerometer.setDataRate(ADXL345_3200HZ);
00046 
00047     //Measurement mode.
00048     accelerometer.setPowerControl(0x08);
00049     
00050     pc.baud(9600);  
00051 
00052   //Set up I2C
00053   i2c.frequency(400000);  // use fast (400 kHz) I2C   
00054   
00055   t.start();        
00056       
00057   // Read the WHO_AM_I register, this is a good test of communication
00058   uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00059   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
00060 
00061   if (whoami == 0x68) // WHO_AM_I should always be 0x68
00062   { 
00063     pc.printf("MPU6050 is online...");
00064     wait(1);
00065     
00066     mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00067     pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
00068     pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
00069     pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
00070     pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
00071     pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
00072     pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
00073     wait(1);
00074 ax1=(float)accelCount[0]*aRes - accelBias[0];
00075     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) 
00076     {
00077     mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00078     mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00079     mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00080 
00081     wait(2);
00082        }
00083     else
00084     {
00085     pc.printf("Device did not the pass self-test!\n\r");
00086       }
00087     }
00088     else
00089     {
00090     pc.printf("Could not connect to MPU6050: \n\r");
00091     pc.printf("%#x \n",  whoami);
00092  
00093     
00094  
00095     while(1) ; // Loop forever if communication doesn't happen
00096   }
00097 
00098  while(1) {
00099      
00100         wait(0.1);
00101         
00102         accelerometer.getOutput(readings);
00103         
00104         //13-bit, sign extended values.
00105         pc.printf("%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
00106 
00107   //idif = ax - ax1;
00108   //if (idif > srb)
00109  //pc.printf( "Up Up Up = %d", idif);
00110  //else if (idif < 0)
00111  //pc.printf("down down down = %d", idif);
00112   //else
00113   //pc.printf("it is less %d", idif);  
00114   // If data ready bit set, all data registers have new data
00115   if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00116     mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
00117     mpu6050.getAres();
00118     // Now we'll calculate the accleration value into actual g's
00119     ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00120     ay = (float)accelCount[1]*aRes - accelBias[1];   
00121     az = (float)accelCount[2]*aRes - accelBias[2];  
00122     Now = t.read_us();
00123     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00124     lastUpdate = Now;
00125     if(lastUpdate - firstUpdate > 10000000.0f)
00126      {
00127      beta = 0.04;  // decrease filter gain after stabilized
00128      zeta = 0.015; // increasey bias drift gain after stabilized
00129     }
00130     // Serial print and/or display at 0.5 s rate independent of data rates
00131     delt_t = t.read_ms() - count;
00132     if (delt_t > 500) { // update LCD once per half-second independent of read rate
00133     pc.printf("ax = %f", 100*ax); 
00134     pc.printf(" ay = %f", 100*ay); 
00135     pc.printf(" az = %f  mg\n\r", 100*az); 
00136 idif = ax - ax1;
00137 pc.printf( "earlier ax1 = %f", ax1);
00138 pc.printf("difference = %f", idif);
00139 if (idif <7 && myservo != 1)
00140 {
00141 pc.printf("up up up = %f", idif);
00142 for(int i=0; i<100;i++)
00143 {
00144        myservo = i/100.0;
00145 wait(0.1);
00146 }
00147 myservo = 1;
00148 idif = 0;
00149 //myservo = 100.0;
00150 }
00151 //pc.printf("the value of my servo for up up up = %d",myservo);}}
00152 //pc.printf("up up up = %f", idif);
00153 if (idif <-7.0&& myservo != 0)
00154 {pc.printf("down down down = %f", idif);
00155 for (int i= 100;i>0;i--)
00156 {myservo = i/100.0;
00157 wait(0.1);
00158 }
00159 myservo = 0.0;
00160 idif = 0.0;}
00161 //pc.printf("the value of my servo for down = %d",myservo);
00162 //pc.printf("down down down = %f", idif);
00163 ax1 = (float)ax*100;
00164 //ax1 = (float)accelCount[0]*aRes - accelBias[0];
00165 pc.printf("current ax1= %f",ax1);   
00166  
00167  wait(1);
00168  
00169    }
00170     }
00171  } 
00172 } 
00173 
00174