In this video lesson I show you how you can measure rotational velocity using the gyroscopes on the MPU6050 IMU module on the GY-87 board.
Below is the code which we develop in this lesson.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 | #include <Adafruit_MPU6050.h> #include <Adafruit_Sensor.h> #include <Wire.h> float Ax; float Ay; float Az; float Gx; float Gy; float Gz; // If You want Calibrated results, you must rotate // your board to all possible orientations, and // record your velues of xMax, xMin, yMax, yMin // zMax, zMin. I show you how to do that in LESSON // 79 on my youtube channel. Put in your values and then // uncomment the code below. Don't use my values, you have // to measure your own for it to work. // float xMax= 1.03; // float xMin= -.97; // float yMax= .98; // float yMin= -1.01; // float zMax=.96 ; // float zMin= -1.08; // float xOffset = (xMax+xMin)/2; // float yOffset = (yMax+yMin)/2; // float zOffset = (zMax+zMin)/2; // float xScale = 2/(xMax -xMin); // float yScale = 2/(yMax - yMin); // float zScale = 2/(zMax -zMin); float roll = 0; float pitch = 0; float rollRAW; float pitchRAW; Adafruit_MPU6050 mpu; void setup() { // put your setup code here, to run once: Serial.begin(115200); mpu.begin(); Serial.println("MPU6050 Started!"); mpu.setAccelerometerRange(MPU6050_RANGE_2_G); mpu.setGyroRange(MPU6050_RANGE_250_DEG); mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); } void loop() { // put your main code here, to run repeatedly: sensors_event_t a, g, temp; mpu.getEvent(&a, &g, &temp); Ax = a.acceleration.x / 9.81; Ay = a.acceleration.y / 9.81; Az = a.acceleration.z / 9.81; Gx = g.gyro.x; Gy = g.gyro.y; Gz = g.gyro.z; //Uncomment these lines of code to do the callibration //This will only work if you have measured your max //and min values, and put them in at the top of the code // Ax = xScale*(Ax-xOffset); // Ay = yScale*(Ay-yOffset); // Az = zScale*(Az-zOffset); pitchRAW = atan2(Ay, sqrt(Az * Az + Ax * Ax)) * 360 / (2 * 3.14); rollRAW = atan2(Ax, sqrt(Az * Az + Ay * Ay)) * 360 / (2 * 3.14); pitch = .75 * pitch + .25 * pitchRAW; roll = .75 * roll + .25 * rollRAW; Serial.print("Gx:"); Serial.print(Gx); Serial.print(','); Serial.print("Gy:"); Serial.print(Gy); Serial.print(','); Serial.print("Gz:"); Serial.print(Gz); Serial.print(','); Serial.print("UL:"); Serial.print(5); Serial.print(','); Serial.print("LL:"); Serial.println(-5); // Serial.print("Ax:"); // Serial.print(Ax); // Serial.print(','); // Serial.print("Ay:"); // Serial.print(Ay); // Serial.print(','); // Serial.print("Az:"); // Serial.print(Az); // Serial.print(','); // Serial.print("UL:"); // Serial.print(1); // Serial.print(','); // Serial.print("LL:"); // Serial.println(-1); delay(10); } |