diff --git a/include/bno085.cpp b/include/bno085.cpp
deleted file mode 100644
index 3bd3ac8..0000000
--- a/include/bno085.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-#include "motionbase.h"
-#include "BNO080.cpp"
-
-BNO080 imu;
-
-void motionSetup()
-{
-    delay(500);
-    if(!imu.begin(BNO080_DEFAULT_ADDRESS, Wire)) {
-        Serial.println("Can't connect to BNO08X");
-        for(int i = 0; i < 500; ++i) {
-            delay(50);
-            digitalWrite(LOADING_LED, LOW);
-            delay(50);
-            digitalWrite(LOADING_LED, HIGH);
-        }
-    }
-    Serial.println("Connected to BNO08X");
-    Wire.setClock(400000);
-    imu.enableARVRStabilizedRotationVector(30);
-}
-
-void motionLoop()
-{
-    //Look for reports from the IMU
-    if (imu.dataAvailable() == true)
-    {
-        cq.x = imu.getQuatI();
-        cq.y = imu.getQuatJ();
-        cq.z = imu.getQuatK();
-        cq.w = imu.getQuatReal();
-        cq *= rotationQuat;
-    }
-}
-
-void sendData() {
-    sendQuat(&cq, PACKET_ROTATION);
-}
-
-void performCalibration() {
-    for(int i = 0; i < 10; ++i) {
-        digitalWrite(CALIBRATING_LED, LOW);
-        delay(20);
-        digitalWrite(CALIBRATING_LED, HIGH);
-        delay(20);
-    }
-    digitalWrite(CALIBRATING_LED, LOW);
-    delay(2000);
-    digitalWrite(CALIBRATING_LED, HIGH);
-    imu.calibrateGyro();
-    imu.requestCalibrationStatus(); // use this
-    while(!imu.calibrationComplete()) {
-        digitalWrite(CALIBRATING_LED, LOW);
-        delay(20);
-        digitalWrite(CALIBRATING_LED, HIGH);
-        delay(20);
-        imu.getReadings();
-    }
-    digitalWrite(CALIBRATING_LED, LOW);
-    delay(2000);
-    digitalWrite(CALIBRATING_LED, HIGH);
-    imu.calibrateAccelerometer();
-    while(!imu.calibrationComplete()) {
-        digitalWrite(CALIBRATING_LED, LOW);
-        delay(20);
-        digitalWrite(CALIBRATING_LED, HIGH);
-        delay(20);
-        imu.getReadings();
-    }
-    digitalWrite(CALIBRATING_LED, LOW);
-    delay(2000);
-    digitalWrite(CALIBRATING_LED, HIGH);
-    imu.calibrateMagnetometer();
-    while(!imu.calibrationComplete()) {
-        digitalWrite(CALIBRATING_LED, LOW);
-        delay(20);
-        digitalWrite(CALIBRATING_LED, HIGH);
-        delay(20);
-        imu.getReadings();
-    }
-    imu.saveCalibration();
-}
\ No newline at end of file
diff --git a/include/credentials.h b/include/credentials.h
deleted file mode 100644
index 673c871..0000000
--- a/include/credentials.h
+++ /dev/null
@@ -1 +0,0 @@
-const char* otaPassword = "YOUROTAPASSWORDHERE";
\ No newline at end of file
diff --git a/include/motionbase.h b/include/motionbase.h
deleted file mode 100644
index 972038f..0000000
--- a/include/motionbase.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef _MOTIONBASE_H_
-#define _MOTIONBASE_H_
-
-#include "I2Cdev.cpp"
-#include "MPU9250MotionApps.h"
-#include "defines.h"
-#include "quat.cpp"
-#include "configuration.cpp"
-#include "util.cpp"
-#include "udpclient.cpp"
-
-MPU9250 accelgyro;
-DeviceConfig config;
-const CalibrationConfig &calibration = config.calibration;
-
-// Vector to hold quaternion
-float q[4] = {1.0, 0.0, 0.0, 0.0};
-Quaternion rawQuat = Quaternion();
-const Quat rotationQuat = Quat(Vector3(0, 0, 1), PI / 2.0); // Adjust rotation to match Android rotations
-Quat cq = Quat();
-
-void motionSetup();
-
-void motionLoop();
-
-void sendData();
-
-#endif
\ No newline at end of file
diff --git a/include/util.h b/include/util.h
deleted file mode 100644
index 7a66923..0000000
--- a/include/util.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef _OWO_UTIL_H_
-#define _OWO_UTIL_H_
-
-float vector_dot(float a[3], float b[3]);
-
-void vector_normalize(float a[3]);
-
-#endif // _OWO_UTIL_H_
\ No newline at end of file
diff --git a/include/BNO080.cpp b/lib/bno080/BNO080.cpp
similarity index 99%
rename from include/BNO080.cpp
rename to lib/bno080/BNO080.cpp
index 1e64615..e41ec0c 100644
--- a/include/BNO080.cpp
+++ b/lib/bno080/BNO080.cpp
@@ -453,7 +453,7 @@ float BNO080::getPitch()
 	dqy = dqy/norm;
 	dqz = dqz/norm;
 
-	float ysqr = dqy * dqy;
+	//float ysqr = dqy * dqy;
 
 	// pitch (y-axis rotation)
 	float t2 = +2.0 * (dqw * dqy - dqz * dqx);
diff --git a/include/BNO080.h b/lib/bno080/BNO080.h
similarity index 100%
rename from include/BNO080.h
rename to lib/bno080/BNO080.h
diff --git a/include/I2Cdev.cpp b/lib/i2cdev/I2Cdev.cpp
similarity index 100%
rename from include/I2Cdev.cpp
rename to lib/i2cdev/I2Cdev.cpp
diff --git a/include/I2Cdev.h b/lib/i2cdev/I2Cdev.h
similarity index 100%
rename from include/I2Cdev.h
rename to lib/i2cdev/I2Cdev.h
diff --git a/include/basis.h b/lib/math/basis.h
similarity index 100%
rename from include/basis.h
rename to lib/math/basis.h
diff --git a/include/util.cpp b/lib/math/helper_3dmath.cpp
similarity index 86%
rename from include/util.cpp
rename to lib/math/helper_3dmath.cpp
index 6110420..02eee03 100644
--- a/include/util.cpp
+++ b/lib/math/helper_3dmath.cpp
@@ -1,5 +1,4 @@
-#include "util.h"
-#include <math.h>
+#include "helper_3dmath.h"
 
 float vector_dot(float a[3], float b[3])
 {
diff --git a/include/helper_3dmath.h b/lib/math/helper_3dmath.h
similarity index 98%
rename from include/helper_3dmath.h
rename to lib/math/helper_3dmath.h
index e27362f..a6d3c79 100644
--- a/include/helper_3dmath.h
+++ b/lib/math/helper_3dmath.h
@@ -215,4 +215,8 @@ class VectorFloat {
         }
 };
 
+float vector_dot(float a[3], float b[3]);
+
+void vector_normalize(float a[3]);
+
 #endif /* _HELPER_3DMATH_H_ */
\ No newline at end of file
diff --git a/include/quat.cpp b/lib/math/quat.cpp
similarity index 100%
rename from include/quat.cpp
rename to lib/math/quat.cpp
diff --git a/include/quat.h b/lib/math/quat.h
similarity index 100%
rename from include/quat.h
rename to lib/math/quat.h
diff --git a/include/shared.h b/lib/math/shared.h
similarity index 100%
rename from include/shared.h
rename to lib/math/shared.h
diff --git a/include/vector3.h b/lib/math/vector3.h
similarity index 100%
rename from include/vector3.h
rename to lib/math/vector3.h
diff --git a/include/MPU6050OffsetFinder.cpp b/lib/mpu6050offsetfinder/MPU6050OffsetFinder.cpp
similarity index 98%
rename from include/MPU6050OffsetFinder.cpp
rename to lib/mpu6050offsetfinder/MPU6050OffsetFinder.cpp
index 5c35707..ba70084 100644
--- a/include/MPU6050OffsetFinder.cpp
+++ b/lib/mpu6050offsetfinder/MPU6050OffsetFinder.cpp
@@ -80,7 +80,6 @@ and so on.
 // for both classes must be in the include path of your project
 #include "I2Cdev.h"
 #include "MPU9250.h"
-#include "motionbase.h"
 
 // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
 // is used in I2Cdev.h
@@ -114,6 +113,8 @@ int Target[6];
 int LinesOut;
 int N;
 
+MPU9250 accelgyro;
+
 void ForceHeader()
 {
     LinesOut = 99;
@@ -150,8 +151,9 @@ void GetSmoothed()
     }
 } // GetSmoothed
 
-void Initialize()
+void Initialize(MPU9250 &mpu9250)
 {
+    accelgyro = mpu9250;
     Serial.println("PID tuning Each Dot = 100 readings");
     /*A tidbit on how PID (PI actually) tuning works. 
     When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and 
@@ -348,9 +350,9 @@ void PullBracketsOut()
     } // keep going
 } // PullBracketsOut
 
-void findOffset()
+void findOffset(MPU9250 &mpu9250)
 {
-    Initialize();
+    Initialize(mpu9250);
     for (int i = iAx; i <= iGz; i++)
     {                  // set targets and initial guesses
         Target[i] = 0; // must fix for ZAccel
diff --git a/include/MPU9250.cpp b/lib/mpu9250/MPU9250.cpp
similarity index 100%
rename from include/MPU9250.cpp
rename to lib/mpu9250/MPU9250.cpp
diff --git a/include/MPU9250.h b/lib/mpu9250/MPU9250.h
similarity index 99%
rename from include/MPU9250.h
rename to lib/mpu9250/MPU9250.h
index f38fd6f..69526fa 100644
--- a/include/MPU9250.h
+++ b/lib/mpu9250/MPU9250.h
@@ -38,8 +38,12 @@ THE SOFTWARE.
 #define _MPU9250_H_
 
 #include "I2Cdev.h"
+#include "helper_3dmath.h"
 #include <avr/pgmspace.h>
 
+// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
+#define MPU9250_INCLUDE_DMP_MOTIONAPPS20
+
 //Magnetometer Registers
 #define MPU9150_RA_MAG_ADDRESS		0x0C
 #define MPU9150_RA_MAG_XOUT_L		0x03
diff --git a/include/MPU9250MotionApps.h b/lib/mpu9250/MPU9250MotionApps.cpp
similarity index 99%
rename from include/MPU9250MotionApps.h
rename to lib/mpu9250/MPU9250MotionApps.cpp
index 3cc28f9..2cabd83 100644
--- a/include/MPU9250MotionApps.h
+++ b/lib/mpu9250/MPU9250MotionApps.cpp
@@ -35,13 +35,7 @@ THE SOFTWARE.
 #ifndef _MPU9250_6AXIS_MOTIONAPPS20_H_
 #define _MPU9250_6AXIS_MOTIONAPPS20_H_
 
-#include "I2Cdev.h"
-#include "helper_3dmath.h"
-
-// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
-#define MPU9250_INCLUDE_DMP_MOTIONAPPS20
-
-#include "MPU9250.cpp"
+#include "MPU9250.h"
 
 // Tom Carpenter's conditional PROGMEM code
 // http://forum.arduino.cc/index.php?topic=129407.0
@@ -49,8 +43,8 @@ THE SOFTWARE.
     #include <avr/pgmspace.h>
 #else
     // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
-    #ifndef __PGMSPACE_H_
-        #define __PGMSPACE_H_ 1
+    #ifndef _PGMSPACE_H_
+        #define _PGMSPACE_H_ 1
         #include <inttypes.h>
 
         #define PROGMEM
diff --git a/include/ota.cpp b/lib/ota/ota.cpp
similarity index 94%
rename from include/ota.cpp
rename to lib/ota/ota.cpp
index 4c801b1..a2f0506 100644
--- a/include/ota.cpp
+++ b/lib/ota/ota.cpp
@@ -1,7 +1,6 @@
-#include "credentials.h"
-#include <ArduinoOTA.h>
+#include "ota.h"
 
-void otaSetup() {
+void otaSetup(const char * const otaPassword) {
     if(otaPassword[0] == '\0')
         return; // No password set up, disable OTA
     ArduinoOTA.setPassword(otaPassword);
diff --git a/lib/ota/ota.h b/lib/ota/ota.h
new file mode 100644
index 0000000..88f07ef
--- /dev/null
+++ b/lib/ota/ota.h
@@ -0,0 +1,9 @@
+#ifndef _OTA_H_
+#define _OTA_H 1
+
+#include <ArduinoOTA.h>
+
+void otaSetup(const char * const otaPassword);
+void otaUpdate();
+
+#endif // _OTA_H_
\ No newline at end of file
diff --git a/src/bno080sensor.cpp b/src/bno080sensor.cpp
new file mode 100644
index 0000000..a33110b
--- /dev/null
+++ b/src/bno080sensor.cpp
@@ -0,0 +1,64 @@
+#include "BNO080.h"
+#include "sensor.h"
+#include "udpclient.h"
+
+void BNO080Sensor::motionSetup(DeviceConfig * config)
+{
+    delay(500);
+    if(!imu.begin(BNO080_DEFAULT_ADDRESS, Wire)) {
+        Serial.println("Can't connect to BNO08X");
+        for(int i = 0; i < 500; ++i) {
+            delay(50);
+            digitalWrite(LOADING_LED, LOW);
+            delay(50);
+            digitalWrite(LOADING_LED, HIGH);
+        }
+    }
+    Serial.println("Connected to BNO08X");
+    Wire.setClock(400000);
+    imu.enableARVRStabilizedGameRotationVector(10);
+}
+
+void BNO080Sensor::motionLoop()
+{
+    //Look for reports from the IMU
+    if (imu.dataAvailable() == true)
+    {
+        quaternion.x = imu.getQuatI();
+        quaternion.y = imu.getQuatJ();
+        quaternion.z = imu.getQuatK();
+        quaternion.w = imu.getQuatReal();
+        quaternion *= sensorOffset;
+        newData = true;
+    }
+}
+
+void BNO080Sensor::sendData() {
+    if(newData) {
+        newData = false;
+        sendQuat(&quaternion, PACKET_ROTATION);
+    }
+}
+
+void BNO080Sensor::startCalibration(int calibrationType) {
+    // TODO It only calibrates gyro, it should have multiple calibration modes, and check calibration status in motionLoop()
+    for(int i = 0; i < 10; ++i) {
+        digitalWrite(CALIBRATING_LED, LOW);
+        delay(20);
+        digitalWrite(CALIBRATING_LED, HIGH);
+        delay(20);
+    }
+    digitalWrite(CALIBRATING_LED, LOW);
+    delay(2000);
+    digitalWrite(CALIBRATING_LED, HIGH);
+    imu.calibrateGyro();
+    do {
+        digitalWrite(CALIBRATING_LED, LOW);
+        imu.requestCalibrationStatus();
+        delay(20);
+        imu.getReadings();
+        digitalWrite(CALIBRATING_LED, HIGH);
+        delay(20);
+    } while(!imu.calibrationComplete());
+    imu.saveCalibration();
+}
\ No newline at end of file
diff --git a/include/configuration.cpp b/src/configuration.cpp
similarity index 100%
rename from include/configuration.cpp
rename to src/configuration.cpp
diff --git a/include/configuration.h b/src/configuration.h
similarity index 100%
rename from include/configuration.h
rename to src/configuration.h
diff --git a/src/defines.h b/src/defines.h
index 6a764d2..208e687 100644
--- a/src/defines.h
+++ b/src/defines.h
@@ -24,6 +24,7 @@
 //Determines how often we sample and send data
 ///////////////////////////////////////////////////////////////////
 #define samplingRateInMillis 10
+#define batterySampleRate 10000
 
 ///////////////////////////////////////////////////////////////////
 //Setup for the Magnetometer
diff --git a/src/main.cpp b/src/main.cpp
index dcc9d04..20c933a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -19,13 +19,21 @@
 // This version must be compiled with library routines in subfolder "libs"
 
 #include "Wire.h"
-#include "ota.cpp"
-#include "bno085.cpp"
+#include "ota.h"
+#include "sensor.h"
+#include "configuration.h"
+#include "udpclient.h"
+#include "defines.h"
+#include "credentials.h"
+
+BNO080Sensor sensor{};
+DeviceConfig config{};
 
 bool isCalibrating = false;
 bool blinking = false;
 unsigned long blinkStart = 0;
 unsigned long now_ms, last_ms = 0; //millis() timers
+unsigned long last_battery_sample = 0;
 
 void setConfig(DeviceConfig newConfig)
 {
@@ -74,11 +82,11 @@ void setup()
         {{  1.01176, -0.00048,  0.00230},
         { -0.00048,  1.00366, -0.00535},
         {  0.00230, -0.00535,  0.99003}},
-        {    9.85,   47.89,  -10.94},
-        {{  1.11351, -0.01290, -0.00077},
-        { -0.01290,  1.18048,  0.01839},
-        { -0.00077,  0.01839,  1.15212}},
-        {   24.59,    -4.96,   -93.38}};
+        {   24.95,   81.77,  -10.36},
+        {{  1.44652,  0.02739, -0.02186},
+        {  0.02739,  1.48749, -0.00547},
+        { -0.02186, -0.00547,  1.42441}},
+        {   22.97,    13.56,   -90.65}};
     if (hasConfigStored())
     {
         loadConfig(&config);
@@ -86,7 +94,7 @@ void setup()
     setConfigRecievedCallback(setConfig);
     setCommandRecievedCallback(commandRecieved);
 
-    motionSetup();
+    sensor.motionSetup(&config);
 
     // Don't start if not connected to MPU
     /*while(!accelgyro.testConnection()) {
@@ -98,7 +106,7 @@ void setup()
     //*/
 
     setUpWiFi(&config);
-    otaSetup();
+    otaSetup(otaPassword);
     digitalWrite(LOADING_LED, HIGH);
 }
 
@@ -108,14 +116,14 @@ void loop()
 {
     otaUpdate();
     clientUpdate();
-    if(connected)
+    if(isConnected())
     {
         if (isCalibrating)
         {
-            performCalibration();
+            sensor.startCalibration(0);
             isCalibrating = false;
         }
-        motionLoop();
+        sensor.motionLoop();
         // Send updates
         now_ms = millis();
         if (now_ms - last_ms >= samplingRateInMillis)
@@ -123,7 +131,12 @@ void loop()
             last_ms = now_ms;
             processBlinking();
 
-            sendData();
+            sensor.sendData();
+        }
+        if(now_ms - last_battery_sample >= batterySampleRate) {
+            last_battery_sample = now_ms;
+            float battery = ((float) analogRead(A0)) / 1024.0 * 14.0;
+            sendFloat(battery, PACKET_BATTERY_LEVEL);
         }
     }
 }
diff --git a/include/6axismotion.cpp b/src/mpu6050sensor.cpp
similarity index 67%
rename from include/6axismotion.cpp
rename to src/mpu6050sensor.cpp
index f31f8dc..e676a63 100644
--- a/include/6axismotion.cpp
+++ b/src/mpu6050sensor.cpp
@@ -1,39 +1,34 @@
-#include "motionbase.h"
-#include "MPU6050OffsetFinder.cpp"
+#include "MPU9250.h"
+#include "sensor.h"
+#include "udpclient.h"
 
-// MPU control/status vars
-bool dmpReady = false;  // set true if DMP init was successful
-uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
-uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64]; // FIFO storage buffer
+void gatherCalibrationData(MPU9250 &imu);
 
-void motionSetup() {
+void MPU6050Sensor::motionSetup(DeviceConfig * config) {
     // initialize device
-    accelgyro.initialize();
-    devStatus = accelgyro.dmpInitialize();
+    imu.initialize();
+    devStatus = imu.dmpInitialize();
 
-    accelgyro.setXGyroOffset(calibration.G_off[0]);
-    accelgyro.setYGyroOffset(calibration.G_off[1]);
-    accelgyro.setZGyroOffset(calibration.G_off[2]);
-    accelgyro.setXAccelOffset(calibration.A_B[0]);
-    accelgyro.setYAccelOffset(calibration.A_B[1]);
-    accelgyro.setZAccelOffset(calibration.A_B[2]);
+    imu.setXGyroOffset(config->calibration.G_off[0]);
+    imu.setYGyroOffset(config->calibration.G_off[1]);
+    imu.setZGyroOffset(config->calibration.G_off[2]);
+    imu.setXAccelOffset(config->calibration.A_B[0]);
+    imu.setYAccelOffset(config->calibration.A_B[1]);
+    imu.setZAccelOffset(config->calibration.A_B[2]);
 
     // make sure it worked (returns 0 if so)
     if (devStatus == 0) {
         // turn on the DMP, now that it's ready
         Serial.println(F("Enabling DMP..."));
-        accelgyro.setDMPEnabled(true);
-        mpuIntStatus = accelgyro.getIntStatus();
+        imu.setDMPEnabled(true);
+        mpuIntStatus = imu.getIntStatus();
 
         // set our DMP Ready flag so the main loop() function knows it's okay to use it
         Serial.println(F("DMP ready! Waiting for first interrupt..."));
         dmpReady = true;
 
         // get expected DMP packet size for later comparison
-        packetSize = accelgyro.dmpGetFIFOPacketSize();
+        packetSize = imu.dmpGetFIFOPacketSize();
     } else {
         // ERROR!
         // 1 = initial memory load failed
@@ -45,55 +40,55 @@ void motionSetup() {
     }
 }
 
-void motionLoop() {
+void MPU6050Sensor::motionLoop() {
     // if programming failed, don't try to do anything
     if (!dmpReady) return;
-    mpuIntStatus = accelgyro.getIntStatus();
+    mpuIntStatus = imu.getIntStatus();
 
     // get current FIFO count
-    fifoCount = accelgyro.getFIFOCount();
+    fifoCount = imu.getFIFOCount();
 
     // check for overflow (this should never happen unless our code is too inefficient)
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         // reset so we can continue cleanly
-        accelgyro.resetFIFO();
+        imu.resetFIFO();
         Serial.println(F("FIFO overflow!"));
 
         // otherwise, check for DMP data ready interrupt (this should happen frequently)
     } else if (mpuIntStatus & 0x02) {
         // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = accelgyro.getFIFOCount();
+        while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
 
         // read a packet from FIFO
-        accelgyro.getFIFOBytes(fifoBuffer, packetSize);
+        imu.getFIFOBytes(fifoBuffer, packetSize);
 
         // track FIFO count here in case there is > 1 packet available
         // (this lets us immediately read more without waiting for an interrupt)
         fifoCount -= packetSize;
 
-        accelgyro.dmpGetQuaternion(&rawQuat, fifoBuffer);
+        imu.dmpGetQuaternion(&rawQuat, fifoBuffer);
         q[0] = rawQuat.x;
         q[1] = rawQuat.y;
         q[2] = rawQuat.z;
         q[3] = rawQuat.w;
-        cq.set(-q[1], q[0], q[2], q[3]);
-        cq *= rotationQuat;
+        quaternion.set(-q[1], q[0], q[2], q[3]);
+        quaternion *= sensorOffset;
     }
 }
 
-void sendData() {
-    sendQuat(&cq, PACKET_ROTATION);
+void MPU6050Sensor::sendData() {
+    sendQuat(&quaternion, PACKET_ROTATION);
 }
 
-void performCalibration() {
+void MPU6050Sensor::startCalibration(int calibrationType) {
     digitalWrite(CALIBRATING_LED, LOW);
     Serial.println("Starting offset finder");
-    findOffset();
+    gatherCalibrationData(imu);
     Serial.println("Process is over");
     digitalWrite(CALIBRATING_LED, HIGH);
 }
 
-void gatherCalibrationData() {
+void gatherCalibrationData(MPU9250 &imu) {
     Serial.println("Gathering raw data for device calibration...");
     int calibrationSamples = 500;
     // Reset values
@@ -110,7 +105,7 @@ void gatherCalibrationData() {
     delay(2000);
     for (int i = 0; i < calibrationSamples; i++)
     {
-        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
         Gxyz[0] += float(gx);
         Gxyz[1] += float(gy);
         Gxyz[2] += float(gz);
@@ -135,7 +130,7 @@ void gatherCalibrationData() {
     for (int i = 0; i < calibrationSamples; i++)
     {
         digitalWrite(CALIBRATING_LED, LOW);
-        accelgyro.getAcceleration(&ax, &ay, &az);
+        imu.getAcceleration(&ax, &ay, &az);
         calibrationData[0] = ax;
         calibrationData[1] = ay;
         calibrationData[2] = az;
diff --git a/include/9axismotion.cpp b/src/mpu9250sensor.cpp
similarity index 74%
rename from include/9axismotion.cpp
rename to src/mpu9250sensor.cpp
index 5d48d00..2aaf487 100644
--- a/include/9axismotion.cpp
+++ b/src/mpu9250sensor.cpp
@@ -1,62 +1,54 @@
-#include "motionbase.h"
+#include "MPU9250.h"
+#include "sensor.h"
+#include "udpclient.h"
+#include "defines.h"
+#include "helper_3dmath.h"
 
-//raw data and scaled as vector
-int16_t ax, ay, az;
-int16_t gx, gy, gz;
-int16_t mx, my, mz;
-float Axyz[3];
-float Gxyz[3];
-float Mxyz[3];
-float rawMag[3];
 #define gscale (250. / 32768.0) * (PI / 180.0) //gyro default 250 LSB per d/s -> rad/s
-
-// NOW USING MAHONY FILTER
-
 // These are the free parameters in the Mahony filter and fusion scheme,
 // Kp for proportional feedback, Ki for integral
 // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
 #define Kp 10.0
 #define Ki 0.0
 
-// Loop timing globals
-unsigned long now = 0, last = 0;   //micros() timers
-float deltat = 0;                  //loop time in seconds
-
 void get_MPU_scaled();
 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
 
-void motionSetup() {
+CalibrationConfig * calibration;
+
+void MPU9250Sensor::motionSetup(DeviceConfig * config) {
+    calibration = &config->calibration;
     // initialize device
-    accelgyro.initialize();
+    imu.initialize();
 }
 
-void motionLoop() {
+void MPU9250Sensor::motionLoop() {
     // Update quaternion
     now = micros();
     deltat = (now - last) * 1.0e-6; //seconds since last update
     last = now;
-    get_MPU_scaled();
+    getMPUScaled();
     MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[1], Mxyz[0], -Mxyz[2], deltat);
-    cq.set(-q[1], -q[2], -q[0], q[3]);
-    cq *= rotationQuat;
+    quaternion.set(-q[1], -q[2], -q[0], q[3]);
+    quaternion *= sensorOffset;
 }
 
-void sendData() {
-    sendQuat(&cq, PACKET_ROTATION);
+void MPU9250Sensor::sendData() {
+    sendQuat(&quaternion, PACKET_ROTATION);
     sendVector(rawMag, PACKET_RAW_MAGENTOMETER);
     sendVector(Axyz, PACKET_ACCEL);
     sendVector(Mxyz, PACKET_MAG);
 }
 
-void get_MPU_scaled()
+void MPU9250Sensor::getMPUScaled()
 {
     float temp[3];
     int i;
-    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+    imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
 
-    Gxyz[0] = ((float)gx - calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
-    Gxyz[1] = ((float)gy - calibration.G_off[1]) * gscale;
-    Gxyz[2] = ((float)gz - calibration.G_off[2]) * gscale;
+    Gxyz[0] = ((float)gx - calibration->G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
+    Gxyz[1] = ((float)gy - calibration->G_off[1]) * gscale;
+    Gxyz[2] = ((float)gz - calibration->G_off[2]) * gscale;
 
     Axyz[0] = (float)ax;
     Axyz[1] = (float)ay;
@@ -64,13 +56,13 @@ void get_MPU_scaled()
     //apply offsets (bias) and scale factors from Magneto
     if(useFullCalibrationMatrix) {
         for (i = 0; i < 3; i++)
-            temp[i] = (Axyz[i] - calibration.A_B[i]);
-        Axyz[0] = calibration.A_Ainv[0][0] * temp[0] + calibration.A_Ainv[0][1] * temp[1] + calibration.A_Ainv[0][2] * temp[2];
-        Axyz[1] = calibration.A_Ainv[1][0] * temp[0] + calibration.A_Ainv[1][1] * temp[1] + calibration.A_Ainv[1][2] * temp[2];
-        Axyz[2] = calibration.A_Ainv[2][0] * temp[0] + calibration.A_Ainv[2][1] * temp[1] + calibration.A_Ainv[2][2] * temp[2];
+            temp[i] = (Axyz[i] - calibration->A_B[i]);
+        Axyz[0] = calibration->A_Ainv[0][0] * temp[0] + calibration->A_Ainv[0][1] * temp[1] + calibration->A_Ainv[0][2] * temp[2];
+        Axyz[1] = calibration->A_Ainv[1][0] * temp[0] + calibration->A_Ainv[1][1] * temp[1] + calibration->A_Ainv[1][2] * temp[2];
+        Axyz[2] = calibration->A_Ainv[2][0] * temp[0] + calibration->A_Ainv[2][1] * temp[1] + calibration->A_Ainv[2][2] * temp[2];
     } else {
         for (i = 0; i < 3; i++)
-            Axyz[i] = (Axyz[i] - calibration.A_B[i]);
+            Axyz[i] = (Axyz[i] - calibration->A_B[i]);
     }
     vector_normalize(Axyz);
 
@@ -80,13 +72,13 @@ void get_MPU_scaled()
     //apply offsets and scale factors from Magneto
     if(useFullCalibrationMatrix) {
         for (i = 0; i < 3; i++)
-            temp[i] = (Mxyz[i] - calibration.M_B[i]);
-        Mxyz[0] = calibration.M_Ainv[0][0] * temp[0] + calibration.M_Ainv[0][1] * temp[1] + calibration.M_Ainv[0][2] * temp[2];
-        Mxyz[1] = calibration.M_Ainv[1][0] * temp[0] + calibration.M_Ainv[1][1] * temp[1] + calibration.M_Ainv[1][2] * temp[2];
-        Mxyz[2] = calibration.M_Ainv[2][0] * temp[0] + calibration.M_Ainv[2][1] * temp[1] + calibration.M_Ainv[2][2] * temp[2];
+            temp[i] = (Mxyz[i] - calibration->M_B[i]);
+        Mxyz[0] = calibration->M_Ainv[0][0] * temp[0] + calibration->M_Ainv[0][1] * temp[1] + calibration->M_Ainv[0][2] * temp[2];
+        Mxyz[1] = calibration->M_Ainv[1][0] * temp[0] + calibration->M_Ainv[1][1] * temp[1] + calibration->M_Ainv[1][2] * temp[2];
+        Mxyz[2] = calibration->M_Ainv[2][0] * temp[0] + calibration->M_Ainv[2][1] * temp[1] + calibration->M_Ainv[2][2] * temp[2];
     } else {
         for (i = 0; i < 3; i++)
-            Mxyz[i] = (Mxyz[i] - calibration.M_B[i]);
+            Mxyz[i] = (Mxyz[i] - calibration->M_B[i]);
     }
     rawMag[0] = Mxyz[0];
     rawMag[1] = Mxyz[1];
@@ -101,7 +93,7 @@ void get_MPU_scaled()
 // input vectors ax, ay, az and mx, my, mz MUST be normalized!
 // gx, gy, gz must be in units of radians/second
 //
-void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
+void MPU9250Sensor::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
 {
     // Vector to hold integral error for Mahony method
     static float eInt[3] = {0.0, 0.0, 0.0};
@@ -188,7 +180,7 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl
     q[3] = q4 * norm;
 }
 
-void performCalibration() {
+void MPU9250Sensor::startCalibration(int calibrationType) {
     digitalWrite(CALIBRATING_LED, LOW);
     Serial.println("Gathering raw data for device calibration...");
     int calibrationSamples = 300;
@@ -202,7 +194,7 @@ void performCalibration() {
     delay(2000);
     for (int i = 0; i < calibrationSamples; i++)
     {
-        accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+        imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
         Gxyz[0] += float(gx);
         Gxyz[1] += float(gy);
         Gxyz[2] += float(gz);
@@ -226,7 +218,7 @@ void performCalibration() {
     for (int i = 0; i < calibrationSamples; i++)
     {
         digitalWrite(CALIBRATING_LED, LOW);
-        accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+        imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
         calibrationData[0] = ax;
         calibrationData[1] = ay;
         calibrationData[2] = az;
diff --git a/src/sensor.h b/src/sensor.h
new file mode 100644
index 0000000..674012d
--- /dev/null
+++ b/src/sensor.h
@@ -0,0 +1,89 @@
+#ifndef _SENSOR_H_
+#define _SENSOR_H_ 1
+
+#include <BNO080.h>
+#include <MPU9250.h>
+#include <quat.h>
+#include <vector3.h>
+#include "configuration.h"
+
+class Sensor {
+    public:
+        Sensor() = default;
+        virtual ~Sensor() = default;
+        virtual void motionSetup(DeviceConfig * config) = 0;
+        virtual void motionLoop() = 0;
+        virtual void sendData() = 0;
+        virtual void startCalibration(int calibrationType) = 0;
+    protected:
+        Quat quaternion {};
+        Quat sensorOffset {Quat(Vector3(0, 0, 1), PI / 2.0)};
+};
+
+class BNO080Sensor : public Sensor {
+    public:
+        BNO080Sensor() = default;
+        ~BNO080Sensor() = default;
+        void motionSetup(DeviceConfig * config) final;
+        void motionLoop() final;
+        void sendData() final;
+        void startCalibration(int calibrationType) final;
+    private:
+        BNO080 imu {};
+        bool newData {false};
+};
+
+class MPUSensor : public Sensor {
+    public:
+        MPUSensor() = default;
+        ~MPUSensor() = default;
+    protected:
+        MPU9250 imu {};
+        float q[4] {1.0, 0.0, 0.0, 0.0};
+};
+
+class MPU6050Sensor : public MPUSensor {
+    public:
+        MPU6050Sensor() = default;
+        ~MPU6050Sensor() = default;
+        void motionSetup(DeviceConfig * config) final;
+        void motionLoop() final;
+        void sendData() final;
+        void startCalibration(int calibrationType) final;
+    private:
+        Quaternion rawQuat {};
+        // MPU dmp control/status vars
+        bool dmpReady{false};  // set true if DMP init was successful
+        uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+        uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+        uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+        uint16_t fifoCount;     // count of all bytes currently in FIFO
+        uint8_t fifoBuffer[64] {}; // FIFO storage buffer
+};
+
+class MPU9250Sensor : public MPUSensor {
+    public:
+        MPU9250Sensor() = default;
+        ~MPU9250Sensor() = default;
+        void motionSetup(DeviceConfig * config) final;
+        void motionLoop() final;
+        void sendData() final;
+        void startCalibration(int calibrationType) final;
+        void getMPUScaled();
+        void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
+    private:
+        MPU9250 imu {};
+        //raw data and scaled as vector
+        int16_t ax, ay, az;
+        int16_t gx, gy, gz;
+        int16_t mx, my, mz;
+        float Axyz[3] {};
+        float Gxyz[3] {};
+        float Mxyz[3] {};
+        float rawMag[3] {};
+        // Loop timing globals
+        unsigned long now = 0, last = 0;   //micros() timers
+        float deltat = 0;                  //loop time in seconds
+};
+
+#endif /* _SENSOR_H_ */
diff --git a/include/udpclient.cpp b/src/udpclient.cpp
similarity index 96%
rename from include/udpclient.cpp
rename to src/udpclient.cpp
index ceff982..dfdad1d 100644
--- a/include/udpclient.cpp
+++ b/src/udpclient.cpp
@@ -103,6 +103,26 @@ void sendVector(float *const result, int type)
     }
 }
 
+void sendFloat(float const value, int type)
+{
+    if (Udp.beginPacket(host, port) > 0)
+    {
+        sendType(type);
+        sendPacketNumber();
+        Udp.write(convert_to_chars(value, buf), sizeof(value));
+        if (Udp.endPacket() == 0)
+        {
+            //Serial.print("Write error: ");
+            //Serial.println(Udp.getWriteError());
+        }
+    }
+    else
+    {
+        //Serial.print("Write error: ");
+        //Serial.println(Udp.getWriteError());
+    }
+}
+
 void sendQuat(Quat *const quaternion, int type)
 {
     if (Udp.beginPacket(host, port) > 0)
@@ -381,6 +401,10 @@ void setUpWiFi(DeviceConfig * const config) {
     Udp.begin(port);
 }
 
+bool isConnected() {
+    return connected;
+}
+
 void connectClient()
 {
     unsigned long now = millis();
diff --git a/include/udpclient.h b/src/udpclient.h
similarity index 91%
rename from include/udpclient.h
rename to src/udpclient.h
index 5197ebb..fce0105 100644
--- a/include/udpclient.h
+++ b/src/udpclient.h
@@ -18,6 +18,7 @@
 #define PACKET_RAW_MAGENTOMETER 9
 #define PACKET_PING_PONG 10
 #define PACKET_SERIAL 11
+#define PACKET_BATTERY_LEVEL 12
 
 #define PACKET_RECIEVE_HEARTBEAT 1
 #define PACKET_RECIEVE_VIBRATE 2
@@ -37,9 +38,12 @@ void sendQuat(float * const quaternion, int type);
 void sendQuat(Quat * const quaternion, int type);
 void sendVector(float * const result, int type);
 void sendConfig(DeviceConfig * const config, int type);
+void sendFloat(float const value, int type);
 void sendRawCalibrationData(int * const data, int type);
 void setConfigRecievedCallback(configRecievedCallback);
 void setCommandRecievedCallback(commandRecievedCallback);
+void setUpWiFi(DeviceConfig * const config);
+bool isConnected();
 
 template<typename T> T convert_chars(unsigned char* src);
 template<typename T> unsigned char* convert_to_chars(T src, unsigned char* target);
diff --git a/include/wificredentials.h b/src/wificredentials.h
similarity index 100%
rename from include/wificredentials.h
rename to src/wificredentials.h