shalabh bhatnagar / Mbed 2 deprecated frdm_robotiarm

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

main.cpp@0:6100f19d230b, 2016-04-12 (annotated)

Committer:
sdloh
Date:
Tue Apr 12 05:38:00 2016 +0000
Revision:
0:6100f19d230b
HELPING HAND-A Robotic arm replicating motion of human body;

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