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
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
Generated on Fri Jul 22 2022 11:29:03 by 1.7.2