![](/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
main.cpp
- Committer:
- sdloh
- Date:
- 2016-04-12
- Revision:
- 2:8eb96314ced5
- Parent:
- 0:6100f19d230b
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); } } } }