init

GYRO.cpp

@@ -0,0 +1,72 @@
+#include "Arduino.h"
+// GYRO.cpp
+#include "GYRO.h"
+#include "HardwareSerial.h"
+
+
+bool GYRO::initGyro() {
+  if (!mpu.begin()) {
+      Serial.println("Failed to find MPU6050 chip");
+      return false;
+  }
+
+  // normal readings
+  mpu.enableSleep(false);
+  mpu.enableCycle(false);
+  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
+  return true;
+}
+
+float* GYRO::getAverages() {
+  sensors_event_t a, g, temp; 
+  mpu.getEvent(&a, &g, &temp);
+
+  float* mpuValues = new float[5]{0};
+  int epochs = 15; 
+  for(int i = 0; i < epochs; i++) {
+    mpuValues[0] += a.gyro.pitch - errorMpuValues[0];
+    mpuValues[1] += a.gyro.roll - errorMpuValues[1];
+    mpuValues[2] += a.acceleration.x - errorMpuValues[2];
+    mpuValues[3] += a.acceleration.y - errorMpuValues[3];
+    mpuValues[4] += a.acceleration.z - errorMpuValues[4];
+
+    delay(5);
+  }
+
+  for(int i = 0; i < 5; i++) {
+    mpuValues[i] = (mpuValues[i] / epochs + previousAvgs[i]) / 2.0;
+    previousAvgs[i] = mpuValues[i];
+  }
+
+  return mpuValues;
+}
+
+void GYRO::calculateError() {
+  delay(1000); // needs to be delayed so that the sensor can read accurate data
+
+  float emv[5] = {0};
+
+  int errors = 5;
+  for (int i = 0; i < errors; i++) {
+    float* mpuValues = getAverages();
+
+    for (int j = 0; j < 5; j++) {
+      emv[j] += mpuValues[j];
+    }
+
+    delete[] mpuValues;
+
+    delay(500);
+  }
+
+  for(int i = 0; i < 5; i++) {
+    errorMpuValues[i] = emv[i] / errors;
+  }
+
+  Serial.println("pitchError: " + String(errorMpuValues[0]));
+  Serial.println("rollError: " + String(errorMpuValues[1]));
+  Serial.println("xAccError: " + String(errorMpuValues[2]));
+  Serial.println("yAccError: " + String(errorMpuValues[3]));
+  Serial.println("zAccError: " + String(errorMpuValues[4]));
+}
+