shalabh bhatnagar / Mbed 2 deprecated frdm_Helpinhand

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ADXL345 Servo mbed

Fork of frdm_robotiarm by shalabh bhatnagar

main.cpp@2:8eb96314ced5, 2016-04-12 (annotated)

Committer:
sdloh
Date:
Tue Apr 12 16:24:49 2016 +0000
Revision:
2:8eb96314ced5
Parent:
0:6100f19d230b 
MPU6050 with FRDMk64

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sdloh 0:6100f19d230b 1 /* shalabh bhatnagar and neeraj rathi
sdloh 0:6100f19d230b 2
sdloh 0:6100f19d230b 3 Hardware setup:
sdloh 0:6100f19d230b 4 MPU6050 Breakout --------- Arduino
sdloh 0:6100f19d230b 5 3.3V --------------------- 3.3V
sdloh 0:6100f19d230b 6 SDA ----------------------- A4
sdloh 0:6100f19d230b 7 SCL ----------------------- A5
sdloh 0:6100f19d230b 8 GND ---------------------- GND
sdloh 0:6100f19d230b 9
sdloh 0:6100f19d230b 10 Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library.
sdloh 0:6100f19d230b 11 Because the sensor is not 5V tolerant, we are using a 3.3 V
sdloh 0:6100f19d230b 12 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L
sdloh 0:6100f19d230b 13 */
sdloh 0:6100f19d230b 14
sdloh 0:6100f19d230b 15 #include "mbed.h"
sdloh 0:6100f19d230b 16 #include "MPU6050.h"
sdloh 0:6100f19d230b 17 #include "Servo.h"
sdloh 0:6100f19d230b 18 #include "ADXL345.h"
sdloh 0:6100f19d230b 19 ADXL345 accelerometer(PTD2, PTD3, PTD1,PTB9);
sdloh 0:6100f19d230b 20 Servo myservo(PTA1);
sdloh 0:6100f19d230b 21 float idif;
sdloh 0:6100f19d230b 22 float ax1;
sdloh 0:6100f19d230b 23 float srb = 7;
sdloh 0:6100f19d230b 24 MPU6050 mpu6050;
sdloh 0:6100f19d230b 25
sdloh 0:6100f19d230b 26 Timer t;
sdloh 0:6100f19d230b 27
sdloh 0:6100f19d230b 28 Serial pc(USBTX, USBRX);
sdloh 0:6100f19d230b 29
sdloh 0:6100f19d230b 30 int main()
sdloh 0:6100f19d230b 31 {
sdloh 0:6100f19d230b 32
sdloh 0:6100f19d230b 33 int readings[3] = {0, 0, 0};
sdloh 0:6100f19d230b 34
sdloh 0:6100f19d230b 35 pc.printf("Starting ADXL345 test...\n");
sdloh 0:6100f19d230b 36 pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
sdloh 0:6100f19d230b 37
sdloh 0:6100f19d230b 38 //Go into standby mode to configure the device.
sdloh 0:6100f19d230b 39 accelerometer.setPowerControl(0x00);
sdloh 0:6100f19d230b 40
sdloh 0:6100f19d230b 41 //Full resolution, +/-16g, 4mg/LSB.
sdloh 0:6100f19d230b 42 accelerometer.setDataFormatControl(0x0B);
sdloh 0:6100f19d230b 43
sdloh 0:6100f19d230b 44 //3.2kHz data rate.
sdloh 0:6100f19d230b 45 accelerometer.setDataRate(ADXL345_3200HZ);
sdloh 0:6100f19d230b 46
sdloh 0:6100f19d230b 47 //Measurement mode.
sdloh 0:6100f19d230b 48 accelerometer.setPowerControl(0x08);
sdloh 0:6100f19d230b 49
sdloh 0:6100f19d230b 50 pc.baud(9600);
sdloh 0:6100f19d230b 51
sdloh 0:6100f19d230b 52 //Set up I2C
sdloh 0:6100f19d230b 53 i2c.frequency(400000); // use fast (400 kHz) I2C
sdloh 0:6100f19d230b 54
sdloh 0:6100f19d230b 55 t.start();
sdloh 0:6100f19d230b 56
sdloh 0:6100f19d230b 57 // Read the WHO_AM_I register, this is a good test of communication
sdloh 0:6100f19d230b 58 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
sdloh 0:6100f19d230b 59 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
sdloh 0:6100f19d230b 60
sdloh 0:6100f19d230b 61 if (whoami == 0x68) // WHO_AM_I should always be 0x68
sdloh 0:6100f19d230b 62 {
sdloh 0:6100f19d230b 63 pc.printf("MPU6050 is online...");
sdloh 0:6100f19d230b 64 wait(1);
sdloh 0:6100f19d230b 65
sdloh 0:6100f19d230b 66 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
sdloh 0:6100f19d230b 67 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
sdloh 0:6100f19d230b 68 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
sdloh 0:6100f19d230b 69 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
sdloh 0:6100f19d230b 70 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
sdloh 0:6100f19d230b 71 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
sdloh 0:6100f19d230b 72 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
sdloh 0:6100f19d230b 73 wait(1);
sdloh 0:6100f19d230b 74 ax1=(float)accelCount[0]*aRes - accelBias[0];
sdloh 0:6100f19d230b 75 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)
sdloh 0:6100f19d230b 76 {
sdloh 0:6100f19d230b 77 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
sdloh 0:6100f19d230b 78 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
sdloh 0:6100f19d230b 79 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
sdloh 0:6100f19d230b 80
sdloh 0:6100f19d230b 81 wait(2);
sdloh 0:6100f19d230b 82 }
sdloh 0:6100f19d230b 83 else
sdloh 0:6100f19d230b 84 {
sdloh 0:6100f19d230b 85 pc.printf("Device did not the pass self-test!\n\r");
sdloh 0:6100f19d230b 86 }
sdloh 0:6100f19d230b 87 }
sdloh 0:6100f19d230b 88 else
sdloh 0:6100f19d230b 89 {
sdloh 0:6100f19d230b 90 pc.printf("Could not connect to MPU6050: \n\r");
sdloh 0:6100f19d230b 91 pc.printf("%#x \n", whoami);
sdloh 0:6100f19d230b 92
sdloh 0:6100f19d230b 93
sdloh 0:6100f19d230b 94
sdloh 0:6100f19d230b 95 while(1) ; // Loop forever if communication doesn't happen
sdloh 0:6100f19d230b 96 }
sdloh 0:6100f19d230b 97
sdloh 0:6100f19d230b 98 while(1) {
sdloh 0:6100f19d230b 99
sdloh 0:6100f19d230b 100 wait(0.1);
sdloh 0:6100f19d230b 101
sdloh 0:6100f19d230b 102 accelerometer.getOutput(readings);
sdloh 0:6100f19d230b 103
sdloh 0:6100f19d230b 104 //13-bit, sign extended values.
sdloh 0:6100f19d230b 105 pc.printf("%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
sdloh 0:6100f19d230b 106
sdloh 0:6100f19d230b 107 //idif = ax - ax1;
sdloh 0:6100f19d230b 108 //if (idif > srb)
sdloh 0:6100f19d230b 109 //pc.printf( "Up Up Up = %d", idif);
sdloh 0:6100f19d230b 110 //else if (idif < 0)
sdloh 0:6100f19d230b 111 //pc.printf("down down down = %d", idif);
sdloh 0:6100f19d230b 112 //else
sdloh 0:6100f19d230b 113 //pc.printf("it is less %d", idif);
sdloh 0:6100f19d230b 114 // If data ready bit set, all data registers have new data
sdloh 0:6100f19d230b 115 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
sdloh 0:6100f19d230b 116 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
sdloh 0:6100f19d230b 117 mpu6050.getAres();
sdloh 0:6100f19d230b 118 // Now we'll calculate the accleration value into actual g's
sdloh 0:6100f19d230b 119 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
sdloh 0:6100f19d230b 120 ay = (float)accelCount[1]*aRes - accelBias[1];
sdloh 0:6100f19d230b 121 az = (float)accelCount[2]*aRes - accelBias[2];
sdloh 0:6100f19d230b 122 Now = t.read_us();
sdloh 0:6100f19d230b 123 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
sdloh 0:6100f19d230b 124 lastUpdate = Now;
sdloh 0:6100f19d230b 125 if(lastUpdate - firstUpdate > 10000000.0f)
sdloh 0:6100f19d230b 126 {
sdloh 0:6100f19d230b 127 beta = 0.04; // decrease filter gain after stabilized
sdloh 0:6100f19d230b 128 zeta = 0.015; // increasey bias drift gain after stabilized
sdloh 0:6100f19d230b 129 }
sdloh 0:6100f19d230b 130 // Serial print and/or display at 0.5 s rate independent of data rates
sdloh 0:6100f19d230b 131 delt_t = t.read_ms() - count;
sdloh 0:6100f19d230b 132 if (delt_t > 500) { // update LCD once per half-second independent of read rate
sdloh 0:6100f19d230b 133 pc.printf("ax = %f", 100*ax);
sdloh 0:6100f19d230b 134 pc.printf(" ay = %f", 100*ay);
sdloh 0:6100f19d230b 135 pc.printf(" az = %f mg\n\r", 100*az);
sdloh 0:6100f19d230b 136 idif = ax - ax1;
sdloh 0:6100f19d230b 137 pc.printf( "earlier ax1 = %f", ax1);
sdloh 0:6100f19d230b 138 pc.printf("difference = %f", idif);
sdloh 0:6100f19d230b 139 if (idif <7 && myservo != 1)
sdloh 0:6100f19d230b 140 {
sdloh 0:6100f19d230b 141 pc.printf("up up up = %f", idif);
sdloh 0:6100f19d230b 142 for(int i=0; i<100;i++)
sdloh 0:6100f19d230b 143 {
sdloh 0:6100f19d230b 144 myservo = i/100.0;
sdloh 0:6100f19d230b 145 wait(0.1);
sdloh 0:6100f19d230b 146 }
sdloh 0:6100f19d230b 147 myservo = 1;
sdloh 0:6100f19d230b 148 idif = 0;
sdloh 0:6100f19d230b 149 //myservo = 100.0;
sdloh 0:6100f19d230b 150 }
sdloh 0:6100f19d230b 151 //pc.printf("the value of my servo for up up up = %d",myservo);}}
sdloh 0:6100f19d230b 152 //pc.printf("up up up = %f", idif);
sdloh 0:6100f19d230b 153 if (idif <-7.0&& myservo != 0)
sdloh 0:6100f19d230b 154 {pc.printf("down down down = %f", idif);
sdloh 0:6100f19d230b 155 for (int i= 100;i>0;i--)
sdloh 0:6100f19d230b 156 {myservo = i/100.0;
sdloh 0:6100f19d230b 157 wait(0.1);
sdloh 0:6100f19d230b 158 }
sdloh 0:6100f19d230b 159 myservo = 0.0;
sdloh 0:6100f19d230b 160 idif = 0.0;}
sdloh 0:6100f19d230b 161 //pc.printf("the value of my servo for down = %d",myservo);
sdloh 0:6100f19d230b 162 //pc.printf("down down down = %f", idif);
sdloh 0:6100f19d230b 163 ax1 = (float)ax*100;
sdloh 0:6100f19d230b 164 //ax1 = (float)accelCount[0]*aRes - accelBias[0];
sdloh 0:6100f19d230b 165 pc.printf("current ax1= %f",ax1);
sdloh 0:6100f19d230b 166
sdloh 0:6100f19d230b 167 wait(1);
sdloh 0:6100f19d230b 168
sdloh 0:6100f19d230b 169 }
sdloh 0:6100f19d230b 170 }
sdloh 0:6100f19d230b 171 }
sdloh 0:6100f19d230b 172 }
sdloh 0:6100f19d230b 173
sdloh 0:6100f19d230b 174