![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This acceleration sensor and gyro sensor is attached to human arm and hand which detect the motion of human hand and arm in 6 axis.Signal of acceleration sensor and gyro sensor is very volatile. To convert it into readable form gravitational equation were used and to make it stable several filters were used. This all processing was done using FRDM K64 board and an output signal was sent to Servo motor to give motion to robotic arm. Robotic arm is capable of picking an object and placing it in nearby location with 3 DOF by replicating motion of human arm.
Dependencies: ADXL345 Servo mbed
Fork of frdm_robotiarm by
/media/uploads/sdloh/img_0220.mov
Diff: main.cpp
- Revision:
- 0:6100f19d230b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Apr 12 05:38:00 2016 +0000 @@ -0,0 +1,174 @@ +/* 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); + + } + } + } +} + +