Diff: Accelerometer.cpp

Revision:

5:de016948db9c

Parent:

4:437778dbdf8c

Child:

6:754d21f44075

diff -r 437778dbdf8c -r de016948db9c Accelerometer.cpp
--- a/Accelerometer.cpp	Sun Feb 28 03:25:13 2021 +0000
+++ b/Accelerometer.cpp	Wed Mar 10 20:54:10 2021 +0000
@@ -3,22 +3,21 @@
 #include "PinAssignment.h"
 
 //Constructor
-Accelerometer::Accelerometer() : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
-    R2(PIN_ACCEL_R2),acc(PIN_SDA,PIN_SCL)
+Accelerometer::Accelerometer(I2C *i2cIn) : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
+    R2(PIN_ACCEL_R2),acc(i2cIn)
 {    
     //Set Output Control
     Panel = 0; //Panel Array
     R1 = 1; //Reflector 2
     R2 = 1; //Reflector 1
-    
-    //Initialize I2C
-    I2C i2c(D14,D15);
-    i2c.frequency(ACC_FREQ);
+    mul = 1;
     
     //Set Range
     acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
-    
-    //Calibration
+}
+
+//calibrate
+void Accelerometer::calibrate(){
     AOffset[S_PANEL] = getAngleCal(S_PANEL,N_CAL);
     AOffset[S_R1] = getAngleCal(S_R1,N_CAL);
     AOffset[S_R2] = getAngleCal(S_R2,N_CAL);
@@ -34,18 +33,21 @@
             R1 = 1;
             R2 = 1;
             curOffset = AOffset[S_PANEL];
+            mul = MUL_P;
             break;
         case S_R1: //Reflector 1
             Panel = 1;
             R1 = 0;
             R2 = 1;
             curOffset = AOffset[S_R1];
+            mul = MUL_R1;
             break;
         case S_R2: //Reflector 2
             Panel = 1;
             R1 = 1;
             R2 = 0;
             curOffset = AOffset[S_R2];
+            mul = MUL_R2;
             break;
     }
 }
@@ -68,7 +70,11 @@
     {
         acc.getAcceleroAngle(ang);
         a[i] = ang[MEAS_AXIS];
+        wait_us(100);
     }
+    
+    sort(a,a+n);
+    
     return a[(int)n/2];
 }
 
@@ -84,8 +90,12 @@
     {
         acc.getAcceleroAngle(ang);
         a[i] = ang[MEAS_AXIS];
+        wait_us(100);
     }
-    return a[(int)N_AVG/2];
+    
+    sort(a,a+N_AVG);
+    
+    return a[(int)N_AVG/2]*mul;
 }