Diff: Accelerometer.cpp

Revision:

0:ab4465742afd

Child:

2:f81ab5d2f0a2

Child:

3:de79cb7b645b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Accelerometer.cpp	Tue Feb 09 23:53:14 2021 +0000
@@ -0,0 +1,74 @@
+//INCLUDES
+#include "Accelerometer.h"
+#include "PinAssignment.h"
+
+//Constructor
+Accelerometer::Accelerometer(int freq) : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
+    R2(PIN_ACCEL_R2),acc(PIN_SDA,PIN_SCL)
+{    
+    //Set Output Control
+    Panel = 0; //Panel Array
+    R1 = 1; //Reflector 2
+    R2 = 1; //Reflector 1
+    
+    //Initialize I2C
+    I2C i2c(D14,D15);
+    i2c.frequency(freq);
+    
+    //Set Range
+    acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);  
+}
+
+//setSource
+void Accelerometer::setSource(int n)
+{
+    switch(n)
+    {
+        case S_PANEL: //Panel Array
+            Panel = 0;
+            R1 = 1;
+            R2 = 1;
+            break;
+        case S_R1: //Reflector 1
+            Panel = 1;
+            R1 = 0;
+            R2 = 1;
+            break;
+        case S_R2: //Reflector 2
+            Panel = 1;
+            R1 = 1;
+            R2 = 0;
+            break;
+    }
+}
+
+//checkConnection
+bool Accelerometer::checkConnection()
+{
+    return acc.testConnection();
+}
+
+//getAngle
+float Accelerometer::getAngle(int s)
+{
+    float ang[3];
+    float ang_out = 0;
+    Accelerometer::setSource(s);
+    
+    for(int i = 0;i < N_AVG;i++)
+    {
+        acc.getAcceleroAngle(ang);
+        ang_out += ang[MEAS_AXIS];
+    }
+    return ang_out/N_AVG;
+}
+
+//checkAngle
+bool Accelerometer::checkAngle(float ref, float cur)
+{    
+    if(cur >= (ref-ANGLE_TOL) && cur <= (ref+ANGLE_TOL))
+    {
+        return 1;
+    }
+    return 0;
+}
\ No newline at end of file