refactor: improve whole project

8 files changed, 590 insertions, 439 deletions

diff --git a/src/calibration.cpp b/src/calibration.cpp
new file mode 100644
index 0000000..f76d8f8
--- /dev/null
+++ b/src/calibration.cpp
@@ -0,0 +1,110 @@
+#include "calibration.hpp"
+#include "time_utils.hpp"
+
+SensorCalibrator::SensorCalibrator(Sensor *sensor)
+{
+	_sensor = sensor;
+	_resetValues();
+}
+
+bool SensorCalibrator::calibrate(unsigned int throttle_time)
+{
+	bool done = false;
+	Time start_time = time_now();
+
+	while (!done)
+	{
+		if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT)
+			return false;
+
+		delay(throttle_time);
+
+		_resetValues();
+
+		for (int i = 0; i < SENSOR_READ_CNT; i++)
+		{
+			_updateValues();
+		}
+
+		_adjustValues();
+
+		Serial.print("Accel X: ");
+		Serial.print(_accel_x);
+		Serial.print("	Accel Y: ");
+		Serial.print(_accel_y);
+		Serial.print("	Accel Z: ");
+		Serial.print(_accel_z);
+
+		Serial.print("	Gyro X: ");
+		Serial.print(_gyro_x);
+		Serial.print("	Gyro Y: ");
+		Serial.print(_gyro_y);
+		Serial.print("	Gyro Z: ");
+		Serial.println(_gyro_z);
+
+		if (_isValuesInRange())
+		{
+			done = true;
+		}
+
+		_adjustCalibration();
+	}
+
+	return true;
+}
+
+void SensorCalibrator::_resetValues()
+{
+	_accel_x = 0;
+	_accel_y = 0;
+	_accel_z = 0;
+
+	_gyro_x = 0;
+	_gyro_y = 0;
+	_gyro_z = 0;
+}
+
+void SensorCalibrator::_updateValues()
+{
+	_sensor->read();
+
+	_accel_x -= _sensor->getAccelX();
+	_accel_y -= _sensor->getAccelY();
+	_accel_z -= _sensor->getAccelZ();
+
+	_gyro_x -= _sensor->getGyroX();
+	_gyro_y -= _sensor->getGyroY();
+	_gyro_z -= _sensor->getGyroZ();
+}
+
+void SensorCalibrator::_adjustValues()
+{
+	_accel_x *= SENSOR_VAL_ADJUST;
+	_accel_y *= SENSOR_VAL_ADJUST;
+	_accel_z *= SENSOR_VAL_ADJUST;
+
+	_gyro_x *= SENSOR_VAL_ADJUST;
+	_gyro_y *= SENSOR_VAL_ADJUST;
+	_gyro_z *= SENSOR_VAL_ADJUST;
+}
+
+bool SensorCalibrator::_isValuesInRange()
+{
+	return (_accel_x < ACCEL_CAL_X_MAX && _accel_x > ACCEL_CAL_X_MIN &&
+			_accel_y < ACCEL_CAL_Y_MAX && _accel_y > ACCEL_CAL_Y_MIN &&
+			_accel_z < ACCEL_CAL_Z_MAX && _accel_z > ACCEL_CAL_Z_MIN &&
+			_gyro_x < GYRO_CAL_X_MAX && _gyro_x > GYRO_CAL_X_MIN &&
+			_gyro_y < GYRO_CAL_Y_MAX && _gyro_y > GYRO_CAL_Y_MIN &&
+			_gyro_z < GYRO_CAL_Z_MAX && _gyro_z > GYRO_CAL_Z_MIN);
+}
+
+void SensorCalibrator::_adjustCalibration()
+{
+	_sensor->accel_cal_x += _accel_x;
+	_sensor->accel_cal_y += _accel_y;
+	_sensor->accel_cal_z += _accel_z;
+
+	_sensor->gyro_cal_x += _gyro_x;
+	_sensor->gyro_cal_y += _gyro_y;
+	_sensor->gyro_cal_z += _gyro_z;
+}
diff --git a/src/calibration.hpp b/src/calibration.hpp
new file mode 100644
index 0000000..79b3758
--- /dev/null
+++ b/src/calibration.hpp
@@ -0,0 +1,71 @@
+#ifndef CALIBRATION_HPP
+#define CALIBRATION_HPP
+
+#include "sensor.hpp"
+
+// Calibration precision
+#define ACCEL_CAL_X_MAX 0.006
+#define ACCEL_CAL_X_MIN -0.006
+
+#define ACCEL_CAL_Y_MAX 0.006
+#define ACCEL_CAL_Y_MIN -0.006
+
+#define ACCEL_CAL_Z_MAX 0.006
+#define ACCEL_CAL_Z_MIN -0.006
+
+#define GYRO_CAL_X_MAX 0.06
+#define GYRO_CAL_X_MIN -0.06
+
+#define GYRO_CAL_Y_MAX 0.06
+#define GYRO_CAL_Y_MIN -0.06
+
+#define GYRO_CAL_Z_MAX 0.06
+#define GYRO_CAL_Z_MIN -0.06
+
+#define CALIBRATION_TIMEOUT 120000 // Milliseconds
+
+#define SENSOR_READ_CNT 20
+#define SENSOR_VAL_ADJUST 0.05
+
+/**
+ * Sensor calibrator.
+ */
+class SensorCalibrator
+{
+public:
+	/**
+	 * Sensor calibrator.
+	 *
+	 * @param sensor A sensor to calibrate
+	 */
+	SensorCalibrator(Sensor *sensor);
+
+	/**
+	 * Calibrates the sensor.
+	 *
+	 * @param throttle_time The sensor's throttle time
+	 * @returns Whether or not the calibration succeeded. Will return false on
+	 * timeout.
+	 */
+	bool calibrate(unsigned int throttle_time);
+
+private:
+	void _resetValues();
+	void _updateValues();
+	void _adjustValues();
+	bool _isValuesInRange();
+
+	void _adjustCalibration();
+
+	Sensor *_sensor;
+
+	float _accel_x;
+	float _accel_y;
+	float _accel_z;
+
+	float _gyro_x;
+	float _gyro_y;
+	float _gyro_z;
+};
+
+#endif
diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index 025bb7d..f1e968e 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,8 +1,15 @@
 #include "Arduino.h"
 #include "Wire.h"
+#include "calibration.hpp"
 #include "sensor.hpp"
 
-Sensor sensor(0x68, &Wire);
+#define LINE_CLEAR_LENGTH 30
+
+#define SENSOR_THROTTLE_TIME 50 // milliseconds
+
+Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME);
+
+char line_clear[LINE_CLEAR_LENGTH + 1];
 
 /**
  * Stops code execution.
@@ -21,47 +28,93 @@ void setup()
 
 	while (!sensor.begin())
 	{
-		Serial.print(millis());
-		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 milliseconds...");
+		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 "
+					   "milliseconds...");
 		delay(2000);
 	}
-	sensor.setAccelSensitivity(2); // 8g
-	sensor.setGyroSensitivity(1);  // 500 degrees/s
 
-	sensor.setThrottleEnabled(true);
+	if (!sensor.setAccelSensitivity(2)) // 8g
+	{
+		Serial.print(
+			"Error: Failed to set the sensor's accelerometer sensitivity. Status: ");
+		Serial.println(static_cast<int>(sensor.getStatus()));
+		stop();
+	}
 
-	Serial.println("Automatically calibrating sensor...");
-	bool cal_status = sensor.autoCalibrate();
+	if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
+	{
+		Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: ");
+		Serial.println(static_cast<int>(sensor.getStatus()));
+		stop();
+	}
 
-	if (!cal_status)
+	Serial.println("Calibrating sensor...");
+	SensorCalibrator sensor_calibrator(&sensor);
+
+	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
 	{
-		Serial.print("Error: Automatic calibration timed out after ");
+		Serial.print("Error: Sensor calibration timed out after ");
 		Serial.print(CALIBRATION_TIMEOUT);
 		Serial.println(" milliseconds");
 
 		stop();
 	}
+	Serial.println("Finished calibrating sensor");
+
+	Serial.println("Calibration values:");
 
-	Serial.println("Finished calibrating");
+	Serial.print("Accelerometer X: ");
+	Serial.println(sensor.accel_cal_x);
 
-	Serial.println("Starting...");
+	Serial.print("Accelerometer Y: ");
+	Serial.println(sensor.accel_cal_y);
+
+	Serial.print("Accelerometer Z: ");
+	Serial.println(sensor.accel_cal_z);
+
+	Serial.print("Gyro X: ");
+	Serial.println(sensor.gyro_cal_x);
+
+	Serial.print("Gyro Y: ");
+	Serial.println(sensor.gyro_cal_y);
+
+	Serial.print("Gyro Z: ");
+	Serial.println(sensor.gyro_cal_z);
+
+	Serial.println("\nStarting...");
+
+	size_t prev_len = strlen(line_clear);
+	memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len);
 }
 
 void loop()
 {
-	if (!sensor.is_calibrated)
+	delay(SENSOR_THROTTLE_TIME);
+
+	if (!sensor.read())
 	{
-		return;
-	}
+		SensorStatus status = sensor.getStatus();
+
+		if (status == SensorStatus::THROTTLED)
+		{
+			Serial.println("Warning: The sensor was read too frequently and throttled");
+			return;
+		}
 
-	sensor.read();
+		Serial.print("Error: Failed to read sensor. Status: ");
+		Serial.println(static_cast<int>(status));
+		stop();
+	}
 
 	float pitch = sensor.getPitch();
 	float roll = sensor.getRoll();
 
+	Serial.print(line_clear);
+	Serial.print("\r");
 	Serial.print("Pitch: ");
 	Serial.print(pitch, 3);
 	Serial.print(" Roll: ");
 	Serial.print(roll, 3);
-	Serial.println();
+	Serial.print("\r");
+	Serial.flush();
 }
diff --git a/src/sensor.cpp b/src/sensor.cpp
index be57609..23edc17 100644
--- a/src/sensor.cpp
+++ b/src/sensor.cpp
@@ -1,8 +1,9 @@
 #include "sensor.hpp"
 #include "math.h"
 #include "sensor_registers.hpp"
+#include "time_utils.hpp"
 
-Sensor::Sensor(uint8_t address, TwoWire *wire)
+Sensor::Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time) : _last_time(0)
 {
 	_address = address;
 	_wire = wire;
@@ -16,15 +17,10 @@ Sensor::Sensor(uint8_t address, TwoWire *wire)
 	gyro_cal_y = 0;
 	gyro_cal_z = 0;
 
-	is_calibrated = false;
-
 	_throttle_enabled = true;
-	_throttleTime = DEFAULT_THROTTLE_TIME;
-
-	_lastTime = 0;
-	_lastMicros = 0;
+	_throttle_time = throttle_time;
 
-	_status = SENSOR_OK;
+	_status = SensorStatus::OK;
 
 	_accel_sensitivity = 0;
 	_accel_to_g_force = 1.0 / 16384.0;
@@ -32,8 +28,6 @@ Sensor::Sensor(uint8_t address, TwoWire *wire)
 	_gyro_sensitivity = 0;
 	_ang_rate_to_dps = 1.0 / 131.0;
 
-	_temperature = 0;
-
 	// Reset the sensor
 	reset();
 }
@@ -45,7 +39,7 @@ bool Sensor::begin()
 	if (isConnected())
 	{
 		_wire->beginTransmission(_address);
-		_wire->write(SENSOR_PWR_MGMT_1);
+		_wire->write(SensorRegisters::PWR_MGMT_1);
 		_wire->write(SENSOR_WAKEUP);
 
 		return (_wire->endTransmission() == 0);
@@ -62,8 +56,6 @@ bool Sensor::isConnected()
 
 void Sensor::reset()
 {
-	setThrottleTime(DEFAULT_THROTTLE_TIME);
-
 	_accel_raw_x = 0;
 	_accel_raw_y = 0;
 	_accel_raw_z = 0;
@@ -81,74 +73,47 @@ void Sensor::reset()
 	_yaw = 0;
 }
 
-void Sensor::setThrottleEnabled(bool throttle = true)
-{
-	_throttle_enabled = throttle;
-};
-
-bool Sensor::getThrottleEnabled()
-{
-	return _throttle_enabled;
-};
-
-void Sensor::setThrottleTime(uint16_t time_ms)
-{
-	_throttleTime = time_ms;
-};
-
-uint16_t Sensor::getThrottleTime()
-{
-	return _throttleTime;
-};
-
-int16_t Sensor::read()
+bool Sensor::read()
 {
-	auto time_now = millis();
+	Time now = time_now();
 
-	if (_throttle_enabled)
+	if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time)
 	{
-		if ((time_now - _lastTime) < _throttleTime)
-		{
-			return SENSOR_THROTTLED;
-		}
+		_status = SensorStatus::THROTTLED;
+		return false;
 	}
 
-	_lastTime = time_now;
-
 	_wire->beginTransmission(_address);
-	_wire->write(SENSOR_ACCEL_XOUT_H);
+	_wire->write(SensorRegisters::ACCEL_XOUT_H);
 
 	if (_wire->endTransmission() != 0)
 	{
-		_status = SENSOR_ERR_WRITE;
-		return _status;
+		_status = SensorStatus::ERR_WRITE;
+		return false;
 	}
 
 	int8_t response_length = _wire->requestFrom(_address, (uint8_t)14);
 
 	if (response_length != 14)
 	{
-		_status = SENSOR_ERR_READ;
-		return _status;
+		_status = SensorStatus::ERR_READ;
+		return false;
 	}
 
-	// ACCELEROMETER
+	// Accelerometer
 	_accel_raw_x = _readTwoBytes(); // ACCEL_XOUT_H  ACCEL_XOUT_L
 	_accel_raw_y = _readTwoBytes(); // ACCEL_YOUT_H  ACCEL_YOUT_L
 	_accel_raw_z = _readTwoBytes(); // ACCEL_ZOUT_H  ACCEL_ZOUT_L
 
-	// TEMPERATURE
-	_temperature = _readTwoBytes(); // TEMP_OUT_H    TEMP_OUT_L
-
-	// GYROSCOPE
+	// Gyroscope
 	_gyro_raw_x = _readTwoBytes(); // GYRO_XOUT_H   GYRO_XOUT_L
 	_gyro_raw_y = _readTwoBytes(); // GYRO_YOUT_H   GYRO_YOUT_L
 	_gyro_raw_z = _readTwoBytes(); // GYRO_ZOUT_H   GYRO_ZOUT_L
 
 	// Duration interval
-	time_now = micros();
-	float duration = (time_now - _lastMicros) * 1e-6; // Duration in seconds.
-	_lastMicros = time_now;
+	now.update();
+	float duration = now.diff(_last_time).secs();
+	_last_time = now;
 
 	// Convert raw acceleration to g:s (g-force)
 	_accel_raw_x *= _accel_to_g_force;
@@ -165,12 +130,13 @@ int16_t Sensor::read()
 	float accel_y_pow_two = pow(_accel_raw_y, 2);
 	float accel_z_pow_two = pow(_accel_raw_z, 2);
 
-	_accel_x = atan(_accel_raw_y / sqrt(accel_x_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES;
-	_accel_y = atan(-1.0 * _accel_raw_x / sqrt(accel_y_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES;
-	_accel_z = atan(_accel_raw_z / sqrt(accel_x_pow_two + accel_y_pow_two)) * RAD_TO_DEGREES;
+	_accel_x =
+		atan(_accel_raw_y / sqrt(accel_x_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES;
 
-	// Convert temperature to celsius
-	_temperature = _temperature * 0.00294117647 + 36.53;
+	_accel_y = atan(-1.0 * _accel_raw_x / sqrt(accel_y_pow_two + accel_z_pow_two)) *
+			   RAD_TO_DEGREES;
+	_accel_z =
+		atan(_accel_raw_z / sqrt(accel_x_pow_two + accel_y_pow_two)) * RAD_TO_DEGREES;
 
 	// Convert raw Gyro to degrees/s
 	_gyro_raw_x *= _ang_rate_to_dps;
@@ -190,7 +156,7 @@ int16_t Sensor::read()
 	_pitch = 0.96 * _gyro_y + 0.04 * _accel_y;
 	_roll = 0.96 * _gyro_x + 0.04 * _accel_x;
 
-	return SENSOR_OK;
+	return true;
 }
 
 bool Sensor::setAccelSensitivity(uint8_t sensitivity)
@@ -200,9 +166,9 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 	if (_accel_sensitivity > 3)
 		_accel_sensitivity = 3;
 
-	uint8_t val = getRegister(SENSOR_ACCEL_CONFIG);
+	uint8_t val = getRegister(SensorRegisters::ACCEL_CONFIG);
 
-	if (_status != SENSOR_OK)
+	if (_status != SensorStatus::OK)
 	{
 		return false;
 	}
@@ -213,7 +179,7 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 		val &= 0xE7;
 		val |= (_accel_sensitivity << 3);
 
-		if (setRegister(SENSOR_ACCEL_CONFIG, val) != SENSOR_OK)
+		if (!setRegister(SensorRegisters::ACCEL_CONFIG, val))
 		{
 			return false;
 		}
@@ -225,20 +191,6 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 	return true;
 }
 
-uint8_t Sensor::getAccelSensitivity()
-{
-	uint8_t val = getRegister(SENSOR_ACCEL_CONFIG);
-
-	if (_status != SENSOR_OK)
-	{
-		return _status; // return and propagate error (best thing to do)
-	}
-
-	_accel_sensitivity = (val >> 3) & 3;
-
-	return _accel_sensitivity;
-}
-
 bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 {
 	_gyro_sensitivity = sensitivity;
@@ -246,9 +198,9 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 	if (_gyro_sensitivity > 3)
 		_gyro_sensitivity = 3;
 
-	uint8_t val = getRegister(SENSOR_GYRO_CONFIG);
+	uint8_t val = getRegister(SensorRegisters::GYRO_CONFIG);
 
-	if (_status != 0)
+	if (_status != SensorStatus::OK)
 	{
 		return false;
 	}
@@ -259,7 +211,7 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 		val &= 0xE7;
 		val |= (_gyro_sensitivity << 3);
 
-		if (setRegister(SENSOR_GYRO_CONFIG, val) != SENSOR_OK)
+		if (!setRegister(SensorRegisters::GYRO_CONFIG, val))
 		{
 			return false;
 		}
@@ -270,39 +222,57 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 	return true;
 }
 
-uint8_t Sensor::getGyroSensitivity()
+float Sensor::getAccelX()
 {
-	uint8_t val = getRegister(SENSOR_GYRO_CONFIG);
+	return _accel_raw_x;
+};
 
-	if (_status != SENSOR_OK)
-	{
-		return _status;
-	}
+float Sensor::getAccelY()
+{
+	return _accel_raw_y;
+};
 
-	_gyro_sensitivity = (val >> 3) & 3;
+float Sensor::getAccelZ()
+{
+	return _accel_raw_z;
+};
 
-	return _gyro_sensitivity;
-}
+float Sensor::getAngleX()
+{
+	return _accel_x;
+};
 
-float Sensor::getAccelX() { return _accel_raw_x; };
-float Sensor::getAccelY() { return _accel_raw_y; };
-float Sensor::getAccelZ() { return _accel_raw_z; };
+float Sensor::getAngleY()
+{
+	return _accel_y;
+};
 
-float Sensor::getAngleX() { return _accel_x; };
-float Sensor::getAngleY() { return _accel_y; };
+float Sensor::getGyroX()
+{
+	return _gyro_raw_x;
+};
 
-float Sensor::getTemperature() { return _temperature; };
+float Sensor::getGyroY()
+{
+	return _gyro_raw_y;
+};
 
-float Sensor::getGyroX() { return _gyro_raw_x; };
-float Sensor::getGyroY() { return _gyro_raw_y; };
-float Sensor::getGyroZ() { return _gyro_raw_z; };
+float Sensor::getGyroZ()
+{
+	return _gyro_raw_z;
+};
 
-float Sensor::getPitch() { return _pitch; };
-float Sensor::getRoll() { return _roll; };
+float Sensor::getPitch()
+{
+	return _pitch;
+};
 
-uint32_t Sensor::lastTime() { return _lastTime; };
+float Sensor::getRoll()
+{
+	return _roll;
+};
 
-uint8_t Sensor::setRegister(uint8_t reg, uint8_t value)
+bool Sensor::setRegister(uint8_t reg, uint8_t value)
 {
 	_wire->beginTransmission(_address);
 	_wire->write(reg);
@@ -310,11 +280,11 @@ uint8_t Sensor::setRegister(uint8_t reg, uint8_t value)
 
 	if (_wire->endTransmission() != 0)
 	{
-		_status = SENSOR_ERR_WRITE;
-		return _status;
+		_status = SensorStatus::ERR_WRITE;
+		return false;
 	}
 
-	return SENSOR_OK;
+	return true;
 }
 
 uint8_t Sensor::getRegister(uint8_t reg)
@@ -324,15 +294,15 @@ uint8_t Sensor::getRegister(uint8_t reg)
 
 	if (_wire->endTransmission() != 0)
 	{
-		_status = SENSOR_ERR_WRITE;
-		return _status;
+		_status = SensorStatus::ERR_WRITE;
+		return 0;
 	}
 
 	uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1);
 	if (response_length != 1)
 	{
-		_status = SENSOR_ERR_READ;
-		return _status;
+		_status = SensorStatus::ERR_READ;
+		return 0;
 	}
 
 	uint8_t val = _wire->read();
@@ -340,12 +310,12 @@ uint8_t Sensor::getRegister(uint8_t reg)
 	return val;
 }
 
-int16_t Sensor::getStatus()
+SensorStatus Sensor::getStatus()
 {
-	auto error = _status;
-	_status = SENSOR_OK;
+	SensorStatus status = _status;
+	_status = SensorStatus::OK;
 
-	return error;
+	return status;
 };
 
 int16_t Sensor::_readTwoBytes()
@@ -356,78 +326,3 @@ int16_t Sensor::_readTwoBytes()
 
 	return response;
 }
-
-bool Sensor::autoCalibrate()
-{
-	bool done = false;
-
-	float accel_x, accel_y, accel_z;
-	float gyro_x, gyro_y, gyro_z;
-
-	auto cal_start_time = millis();
-
-	while (!done)
-	{
-		if (millis() - cal_start_time >= CALIBRATION_TIMEOUT)
-		{
-			return false;
-		}
-
-		delay(100);
-
-		accel_x = 0;
-		accel_y = 0;
-		accel_z = 0;
-
-		gyro_x = 0;
-		gyro_y = 0;
-		gyro_z = 0;
-
-		for (int i = 0; i < 20; i++)
-		{
-			read();
-
-			accel_x -= getAccelX();
-			accel_y -= getAccelY();
-			accel_z -= getAccelZ();
-
-			gyro_x -= getGyroX();
-			gyro_y -= getGyroY();
-			gyro_z -= getGyroZ();
-		}
-
-		const float adjust_val = 0.05;
-
-		accel_x *= adjust_val;
-		accel_y *= adjust_val;
-		accel_z *= adjust_val;
-
-		gyro_x *= adjust_val;
-		gyro_y *= adjust_val;
-		gyro_z *= adjust_val;
-
-		if (
-			accel_x < ACCEL_CAL_X_MAX && accel_x > ACCEL_CAL_X_MIN &&
-			accel_y < ACCEL_CAL_Y_MAX && accel_y > ACCEL_CAL_Y_MIN &&
-			accel_z < ACCEL_CAL_Z_MAX && accel_z > ACCEL_CAL_Z_MIN &&
-			gyro_x < GYRO_CAL_X_MAX && gyro_x > GYRO_CAL_X_MIN &&
-			gyro_y < GYRO_CAL_Y_MAX && gyro_y > GYRO_CAL_Y_MIN &&
-			gyro_z < GYRO_CAL_Z_MAX && gyro_z > GYRO_CAL_Z_MIN)
-		{
-			done = true;
-		}
-
-		// Adjust calibration values
-		accel_cal_x += accel_x;
-		accel_cal_y += accel_y;
-		accel_cal_z += accel_z;
-
-		gyro_cal_x += gyro_x;
-		gyro_cal_y += gyro_y;
-		gyro_cal_z += gyro_z;
-	}
-
-	is_calibrated = true;
-
-	return true;
-}
diff --git a/src/sensor.hpp b/src/sensor.hpp
index 3fbc544..07b8f95 100644
--- a/src/sensor.hpp
+++ b/src/sensor.hpp
@@ -3,15 +3,7 @@
 
 #include "Arduino.h"
 #include "Wire.h"
-
-#define DEFAULT_THROTTLE_TIME 10 // milliseconds
-
-// Status codes
-#define SENSOR_OK 0
-#define SENSOR_THROTTLED 1
-#define SENSOR_ERR_READ -1
-#define SENSOR_ERR_WRITE -2
-#define SENSOR_ERR_NOT_CONNECTED -3
+#include "time_utils.hpp"
 
 #define SENSOR_WAKEUP 0x00
 
@@ -19,35 +11,33 @@
 #define RAW_TO_DPS_FACTOR (1.0 / 131.0)
 #define RAW_TO_G_FACTOR (1.0 / 16384.0)
 
-// Automatic calibration precision
-#define ACCEL_CAL_X_MAX 0.002
-#define ACCEL_CAL_X_MIN -0.002
-
-#define ACCEL_CAL_Y_MAX 0.002
-#define ACCEL_CAL_Y_MIN -0.002
-
-#define ACCEL_CAL_Z_MAX 0.002
-#define ACCEL_CAL_Z_MIN -0.002
-
-#define GYRO_CAL_X_MAX 0.2
-#define GYRO_CAL_X_MIN -0.2
-
-#define GYRO_CAL_Y_MAX 0.02
-#define GYRO_CAL_Y_MIN -0.02
-
-#define GYRO_CAL_Z_MAX 0.08
-#define GYRO_CAL_Z_MIN -0.08
-
-#define CALIBRATION_TIMEOUT 120000 // Milliseconds
+enum class SensorStatus
+{
+	OK = 0,
+	THROTTLED = 1,
+	ERR_READ = -1,
+	ERR_WRITE = -2,
+	ERR_NOT_CONNECTED = -3
+};
 
+/**
+ * A GY521 gyro and accelerometer sensor.
+ */
 class Sensor
 {
 public:
-	Sensor(uint8_t address, TwoWire *wire);
+	/**
+	 * A GY521 gyro and accelerometer sensor.
+	 *
+	 * @param address The address of the sensor
+	 * @param wire A Wire instance
+	 * @param throttle_time A minumum time between sensor reads for the sensor to throttle
+	 */
+	Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time);
 
 	/**
 	 * Initializes communication with the sensor.
-	 * 
+	 *
 	 * @returns Whether or not the sensor can be communicated with.
 	 */
 	bool begin();
@@ -63,59 +53,21 @@ public:
 	void reset();
 
 	/**
-	 * Returns whether or not throttling is enabled.
-	 */
-	bool getThrottleEnabled();
-
-	/**
-	 * Sets whether or not throttling is enabled.
-	 * 
-	 * @param throttle Throttling enable/disable
-	 */
-	void setThrottleEnabled(bool throttle);
-
-	/**
-	 * Returns the throttle time.
-	 */
-	uint16_t getThrottleTime();
-
-	/**
-	 * Sets the throttle time.
-	 * 
-	 * @param time_ms Time in milliseconds to throttle
-	 */
-	void setThrottleTime(uint16_t time_ms);
-
-	/**
 	 * Reads from the sensor.
-	 * 
-	 * @returns The sensor status.
-	 */
-	int16_t read();
-
-	/**
-	 * Returns the accelerometer sensitivity.
-	 * 
-	 * 0,1,2,3 ==> 2g 4g 8g 16g
+	 *
+	 * @returns Whether or not it succeeded.
 	 */
-	uint8_t getAccelSensitivity();
+	bool read();
 
 	/**
 	 * Sets the accelerometer sensitivity.
-	 * 
+	 *
 	 * @param sensitivity Accelerometer sensitivity. 0, 1, 2, 3 => 2g 4g 8g 16g
 	 * @returns Whether or not it succeeded.
 	 */
 	bool setAccelSensitivity(uint8_t sensitivity);
 
 	/**
-	 * Returns the gyro sensitivity.
-	 * 
-	 * 0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s
-	 */
-	uint8_t getGyroSensitivity();
-
-	/**
 	 * Sets the gyro sensitivity.
 	 *
 	 * @param sensitivity Gyro sensitivity.
@@ -150,11 +102,6 @@ public:
 	float getAngleY();
 
 	/**
-	 * Returns the current temperature.
-	 */
-	float getTemperature();
-
-	/**
 	 * Returns the current X axis in degrees/s.
 	 */
 	float getGyroX();
@@ -180,22 +127,17 @@ public:
 	float getRoll();
 
 	/**
-	 * Returns the last time the sensor was read.
-	 */
-	uint32_t lastTime();
-
-	/**
 	 * Sets the value of a key in the sensor's register.
-	 * 
+	 *
 	 * @param reg The address of a registry key
 	 * @param value A new value
-	 * @returns The sensor status
+	 * @returns Whether or not it succeeded.
 	 */
-	uint8_t setRegister(uint8_t reg, uint8_t value);
+	bool setRegister(uint8_t reg, uint8_t value);
 
 	/**
 	 * Returns the value of a key in the sensor's registry.
-	 * 
+	 *
 	 * @param reg The address of a registry key
 	 */
 	uint8_t getRegister(uint8_t reg);
@@ -203,17 +145,7 @@ public:
 	/**
 	 * Returns the last sensor status.
 	 */
-	int16_t getStatus();
-
-	/**
-	 * Automatically calibrates the sensor.
-	 *
-	 * @returns Whether or not the calibration succeeded.
-	 * 	Will return false if it times out.
-	 */
-	bool autoCalibrate();
-
-	bool is_calibrated;
+	SensorStatus getStatus();
 
 	// Accelerometer calibration values
 	float accel_cal_x;
@@ -229,12 +161,11 @@ private:
 	uint8_t _address;
 
 	bool _throttle_enabled;
-	uint16_t _throttleTime;
+	unsigned int _throttle_time;
 
-	uint32_t _lastTime;
-	uint32_t _lastMicros;
+	Time _last_time;
 
-	int16_t _status;
+	SensorStatus _status;
 
 	uint8_t _accel_sensitivity;
 	float _accel_to_g_force;
@@ -250,8 +181,6 @@ private:
 
 	float _pitch, _roll, _yaw;
 
-	float _temperature;
-
 	int16_t _readTwoBytes();
 
 	TwoWire *_wire;
diff --git a/src/sensor_registers.hpp b/src/sensor_registers.hpp
index 34b47c6..c1b19b4 100644
--- a/src/sensor_registers.hpp
+++ b/src/sensor_registers.hpp
@@ -1,134 +1,137 @@
 #ifndef SENSOR_REGISTERS_HPP
 #define SENSOR_REGISTERS_HPP
 
-#define SENSOR_XG_OFFS_TC 0x00
-#define SENSOR_YG_OFFS_TC 0x01
-#define SENSOR_ZG_OFFS_TC 0x02
-
-#define SENSOR_X_FINE_GAIN 0x03
-#define SENSOR_Y_FINE_GAIN 0x04
-#define SENSOR_Z_FINE_GAIN 0x05
-
-#define SENSOR_XA_OFFS_H 0x06
-#define SENSOR_XA_OFFS_L_TC 0x07
-#define SENSOR_YA_OFFS_H 0x08
-#define SENSOR_YA_OFFS_L_TC 0x09
-#define SENSOR_ZA_OFFS_H 0x0A
-#define SENSOR_ZA_OFFS_L_TC 0x0B
-
-#define SENSOR_SELF_TEST_X 0x0D
-#define SENSOR_SELF_TEST_Y 0x0E
-#define SENSOR_SELF_TEST_Z 0x0F
-#define SENSOR_SELF_TEST_A 0x10
-
-#define SENSOR_XG_OFFS_USRH 0x13
-#define SENSOR_XG_OFFS_USRL 0x14
-#define SENSOR_YG_OFFS_USRH 0x15
-#define SENSOR_YG_OFFS_USRL 0x16
-#define SENSOR_ZG_OFFS_USRH 0x17
-#define SENSOR_ZG_OFFS_USRL 0x18
-
-#define SENSOR_SMPLRT_DIV 0x19
-#define SENSOR_CONFIG 0x1A
-#define SENSOR_GYRO_CONFIG 0x1B
-#define SENSOR_ACCEL_CONFIG 0x1C
-
-#define SENSOR_FF_THR 0x1D
-#define SENSOR_FF_DUR 0x1E
-#define SENSOR_MOT_THR 0x1F
-#define SENSOR_MOT_DUR 0x20
-#define SENSOR_ZRMOT_THR 0x21
-#define SENSOR_ZRMOT_DUR 0x22
-#define SENSOR_FIFO_EN 0x23
-
-#define SENSOR_I2C_MST_CTRL 0x24
-#define SENSOR_I2C_SLV0_ADDR 0x25
-#define SENSOR_I2C_SLV0_REG 0x26
-#define SENSOR_I2C_SLV0_CTRL 0x27
-#define SENSOR_I2C_SLV1_ADDR 0x28
-#define SENSOR_I2C_SLV1_REG 0x29
-#define SENSOR_I2C_SLV1_CTRL 0x2A
-#define SENSOR_I2C_SLV2_ADDR 0x2B
-#define SENSOR_I2C_SLV2_REG 0x2C
-#define SENSOR_I2C_SLV2_CTRL 0x2D
-#define SENSOR_I2C_SLV3_ADDR 0x2E
-#define SENSOR_I2C_SLV3_REG 0x2F
-#define SENSOR_I2C_SLV3_CTRL 0x30
-#define SENSOR_I2C_SLV4_ADDR 0x31
-#define SENSOR_I2C_SLV4_REG 0x32
-#define SENSOR_I2C_SLV4_DO 0x33
-#define SENSOR_I2C_SLV4_CTRL 0x34
-#define SENSOR_I2C_SLV4_DI 0x35
-#define SENSOR_I2C_MST_STATUS 0x36
-
-#define SENSOR_INT_PIN_CFG 0x37
-#define SENSOR_INT_ENABLE 0x38
-#define SENSOR_DMP_INT_STATUS 0x39
-#define SENSOR_INT_STATUS 0x3A
-
-#define SENSOR_ACCEL_XOUT_H 0x3B
-#define SENSOR_ACCEL_XOUT_L 0x3C
-#define SENSOR_ACCEL_YOUT_H 0x3D
-#define SENSOR_ACCEL_YOUT_L 0x3E
-#define SENSOR_ACCEL_ZOUT_H 0x3F
-#define SENSOR_ACCEL_ZOUT_L 0x40
-#define SENSOR_TEMP_OUT_H 0x41
-#define SENSOR_TEMP_OUT_L 0x42
-#define SENSOR_GYRO_XOUT_H 0x43
-#define SENSOR_GYRO_XOUT_L 0x44
-#define SENSOR_GYRO_YOUT_H 0x45
-#define SENSOR_GYRO_YOUT_L 0x46
-#define SENSOR_GYRO_ZOUT_H 0x47
-#define SENSOR_GYRO_ZOUT_L 0x48
-
-#define SENSOR_EXT_SENS_DATA_00 0x49
-#define SENSOR_EXT_SENS_DATA_01 0x4A
-#define SENSOR_EXT_SENS_DATA_02 0x4B
-#define SENSOR_EXT_SENS_DATA_03 0x4C
-#define SENSOR_EXT_SENS_DATA_04 0x4D
-#define SENSOR_EXT_SENS_DATA_05 0x4E
-#define SENSOR_EXT_SENS_DATA_06 0x4F
-#define SENSOR_EXT_SENS_DATA_07 0x50
-#define SENSOR_EXT_SENS_DATA_08 0x51
-#define SENSOR_EXT_SENS_DATA_09 0x52
-#define SENSOR_EXT_SENS_DATA_10 0x53
-#define SENSOR_EXT_SENS_DATA_11 0x54
-#define SENSOR_EXT_SENS_DATA_12 0x55
-#define SENSOR_EXT_SENS_DATA_13 0x56
-#define SENSOR_EXT_SENS_DATA_14 0x57
-#define SENSOR_EXT_SENS_DATA_15 0x58
-#define SENSOR_EXT_SENS_DATA_16 0x59
-#define SENSOR_EXT_SENS_DATA_17 0x5A
-#define SENSOR_EXT_SENS_DATA_18 0x5B
-#define SENSOR_EXT_SENS_DATA_19 0x5C
-#define SENSOR_EXT_SENS_DATA_20 0x5D
-#define SENSOR_EXT_SENS_DATA_21 0x5E
-#define SENSOR_EXT_SENS_DATA_22 0x5F
-#define SENSOR_EXT_SENS_DATA_23 0x60
-
-#define SENSOR_MOT_DETECT_STATUS 0x61
-
-#define SENSOR_I2C_SLV0_DO 0x63
-#define SENSOR_I2C_SLV1_DO 0x64
-#define SENSOR_I2C_SLV2_DO 0x65
-#define SENSOR_I2C_SLV3_DO 0x66
-#define SENSOR_I2C_MST_DELAY_CTRL 0x67
-
-#define SENSOR_SIGNAL_PATH_RESET 0x68
-#define SENSOR_MOT_DETECT_CTRL 0x69
-#define SENSOR_USER_CTRL 0x6A
-
-#define SENSOR_PWR_MGMT_1 0x6B
-#define SENSOR_PWR_MGMT_2 0x6C
-#define SENSOR_BANK_SEL 0x6D
-#define SENSOR_MEM_START_ADDR 0x6E
-#define SENSOR_MEM_R_W 0x6F
-
-#define SENSOR_DMP_CFG_1 0x70
-#define SENSOR_DMP_CFG_2 0x71
-#define SENSOR_FIFO_COUNTH 0x72
-#define SENSOR_FIFO_COUNTL 0x73
-#define SENSOR_FIFO_R_W 0x74
-#define SENSOR_WHO_AM_I 0x75
+namespace SensorRegisters
+{
+	const uint8_t XG_OFFS_TC = 0x00;
+	const uint8_t YG_OFFS_TC = 0x01;
+	const uint8_t ZG_OFFS_TC = 0x02;
+
+	const uint8_t X_FINE_GAIN = 0x03;
+	const uint8_t Y_FINE_GAIN = 0x04;
+	const uint8_t Z_FINE_GAIN = 0x05;
+
+	const uint8_t XA_OFFS_H = 0x06;
+	const uint8_t XA_OFFS_L_TC = 0x07;
+	const uint8_t YA_OFFS_H = 0x08;
+	const uint8_t YA_OFFS_L_TC = 0x09;
+	const uint8_t ZA_OFFS_H = 0x0A;
+	const uint8_t ZA_OFFS_L_TC = 0x0B;
+
+	const uint8_t SELF_TEST_X = 0x0D;
+	const uint8_t SELF_TEST_Y = 0x0E;
+	const uint8_t SELF_TEST_Z = 0x0F;
+	const uint8_t SELF_TEST_A = 0x10;
+
+	const uint8_t XG_OFFS_USRH = 0x13;
+	const uint8_t XG_OFFS_USRL = 0x14;
+	const uint8_t YG_OFFS_USRH = 0x15;
+	const uint8_t YG_OFFS_USRL = 0x16;
+	const uint8_t ZG_OFFS_USRH = 0x17;
+	const uint8_t ZG_OFFS_USRL = 0x18;
+
+	const uint8_t SMPLRT_DIV = 0x19;
+	const uint8_t CONFIG = 0x1A;
+	const uint8_t GYRO_CONFIG = 0x1B;
+	const uint8_t ACCEL_CONFIG = 0x1C;
+
+	const uint8_t FF_THR = 0x1D;
+	const uint8_t FF_DUR = 0x1E;
+	const uint8_t MOT_THR = 0x1F;
+	const uint8_t MOT_DUR = 0x20;
+	const uint8_t ZRMOT_THR = 0x21;
+	const uint8_t ZRMOT_DUR = 0x22;
+	const uint8_t FIFO_EN = 0x23;
+
+	const uint8_t I2C_MST_CTRL = 0x24;
+	const uint8_t I2C_SLV0_ADDR = 0x25;
+	const uint8_t I2C_SLV0_REG = 0x26;
+	const uint8_t I2C_SLV0_CTRL = 0x27;
+	const uint8_t I2C_SLV1_ADDR = 0x28;
+	const uint8_t I2C_SLV1_REG = 0x29;
+	const uint8_t I2C_SLV1_CTRL = 0x2A;
+	const uint8_t I2C_SLV2_ADDR = 0x2B;
+	const uint8_t I2C_SLV2_REG = 0x2C;
+	const uint8_t I2C_SLV2_CTRL = 0x2D;
+	const uint8_t I2C_SLV3_ADDR = 0x2E;
+	const uint8_t I2C_SLV3_REG = 0x2F;
+	const uint8_t I2C_SLV3_CTRL = 0x30;
+	const uint8_t I2C_SLV4_ADDR = 0x31;
+	const uint8_t I2C_SLV4_REG = 0x32;
+	const uint8_t I2C_SLV4_DO = 0x33;
+	const uint8_t I2C_SLV4_CTRL = 0x34;
+	const uint8_t I2C_SLV4_DI = 0x35;
+	const uint8_t I2C_MST_STATUS = 0x36;
+
+	const uint8_t INT_PIN_CFG = 0x37;
+	const uint8_t INT_ENABLE = 0x38;
+	const uint8_t DMP_INT_STATUS = 0x39;
+	const uint8_t INT_STATUS = 0x3A;
+
+	const uint8_t ACCEL_XOUT_H = 0x3B;
+	const uint8_t ACCEL_XOUT_L = 0x3C;
+	const uint8_t ACCEL_YOUT_H = 0x3D;
+	const uint8_t ACCEL_YOUT_L = 0x3E;
+	const uint8_t ACCEL_ZOUT_H = 0x3F;
+	const uint8_t ACCEL_ZOUT_L = 0x40;
+	const uint8_t TEMP_OUT_H = 0x41;
+	const uint8_t TEMP_OUT_L = 0x42;
+	const uint8_t GYRO_XOUT_H = 0x43;
+	const uint8_t GYRO_XOUT_L = 0x44;
+	const uint8_t GYRO_YOUT_H = 0x45;
+	const uint8_t GYRO_YOUT_L = 0x46;
+	const uint8_t GYRO_ZOUT_H = 0x47;
+	const uint8_t GYRO_ZOUT_L = 0x48;
+
+	const uint8_t EXT_SENS_DATA_00 = 0x49;
+	const uint8_t EXT_SENS_DATA_01 = 0x4A;
+	const uint8_t EXT_SENS_DATA_02 = 0x4B;
+	const uint8_t EXT_SENS_DATA_03 = 0x4C;
+	const uint8_t EXT_SENS_DATA_04 = 0x4D;
+	const uint8_t EXT_SENS_DATA_05 = 0x4E;
+	const uint8_t EXT_SENS_DATA_06 = 0x4F;
+	const uint8_t EXT_SENS_DATA_07 = 0x50;
+	const uint8_t EXT_SENS_DATA_08 = 0x51;
+	const uint8_t EXT_SENS_DATA_09 = 0x52;
+	const uint8_t EXT_SENS_DATA_10 = 0x53;
+	const uint8_t EXT_SENS_DATA_11 = 0x54;
+	const uint8_t EXT_SENS_DATA_12 = 0x55;
+	const uint8_t EXT_SENS_DATA_13 = 0x56;
+	const uint8_t EXT_SENS_DATA_14 = 0x57;
+	const uint8_t EXT_SENS_DATA_15 = 0x58;
+	const uint8_t EXT_SENS_DATA_16 = 0x59;
+	const uint8_t EXT_SENS_DATA_17 = 0x5A;
+	const uint8_t EXT_SENS_DATA_18 = 0x5B;
+	const uint8_t EXT_SENS_DATA_19 = 0x5C;
+	const uint8_t EXT_SENS_DATA_20 = 0x5D;
+	const uint8_t EXT_SENS_DATA_21 = 0x5E;
+	const uint8_t EXT_SENS_DATA_22 = 0x5F;
+	const uint8_t EXT_SENS_DATA_23 = 0x60;
+
+	const uint8_t MOT_DETECT_STATUS = 0x61;
+
+	const uint8_t I2C_SLV0_DO = 0x63;
+	const uint8_t I2C_SLV1_DO = 0x64;
+	const uint8_t I2C_SLV2_DO = 0x65;
+	const uint8_t I2C_SLV3_DO = 0x66;
+	const uint8_t I2C_MST_DELAY_CTRL = 0x67;
+
+	const uint8_t SIGNAL_PATH_RESET = 0x68;
+	const uint8_t MOT_DETECT_CTRL = 0x69;
+	const uint8_t USER_CTRL = 0x6A;
+
+	const uint8_t PWR_MGMT_1 = 0x6B;
+	const uint8_t PWR_MGMT_2 = 0x6C;
+	const uint8_t BANK_SEL = 0x6D;
+	const uint8_t MEM_START_ADDR = 0x6E;
+	const uint8_t MEM_R_W = 0x6F;
+
+	const uint8_t DMP_CFG_1 = 0x70;
+	const uint8_t DMP_CFG_2 = 0x71;
+	const uint8_t FIFO_COUNTH = 0x72;
+	const uint8_t FIFO_COUNTL = 0x73;
+	const uint8_t FIFO_R_W = 0x74;
+	const uint8_t WHO_AM_I = 0x75;
+}
 
 #endif
diff --git a/src/time_utils.cpp b/src/time_utils.cpp
new file mode 100644
index 0000000..84c7d96
--- /dev/null
+++ b/src/time_utils.cpp
@@ -0,0 +1,37 @@
+#include "time_utils.hpp"
+#include "Arduino.h"
+
+Time::Time(unsigned long time_micros)
+{
+	_time_micros = time_micros;
+}
+
+void Time::update()
+{
+	_time_micros = micros();
+}
+
+Time Time::diff(Time prev_time)
+{
+	return Time(_time_micros - prev_time.microsecs());
+}
+
+unsigned long Time::microsecs()
+{
+	return _time_micros;
+}
+
+unsigned long Time::millisecs()
+{
+	return _time_micros * 0.001;
+}
+
+float Time::secs()
+{
+	return _time_micros * 0.000001;
+}
+
+Time time_now()
+{
+	return Time(micros());
+}
diff --git a/src/time_utils.hpp b/src/time_utils.hpp
new file mode 100644
index 0000000..f8f2eb3
--- /dev/null
+++ b/src/time_utils.hpp
@@ -0,0 +1,53 @@
+#ifndef TIME_UTILS_HPP
+#define TIME_UTILS_HPP
+
+/**
+ * A representation of time.
+ */
+class Time
+{
+public:
+	/**
+	 * A representation of time.
+	 *
+	 * @param time_micros Time in microseconds
+	 */
+	Time(unsigned long time_micros);
+
+	/**
+	 * Updates the time to the current time.
+	 */
+	void update();
+
+	/**
+	 * Returns the difference between two points in time.
+	 *
+	 * @param prev_time A previous point in time
+	 */
+	Time diff(Time prev_time);
+
+	/**
+	 * Returns the time in seconds.
+	 */
+	float secs();
+
+	/**
+	 * Returns the time in milliseconds.
+	 */
+	unsigned long millisecs();
+
+	/**
+	 * Returns the time in microseconds.
+	 */
+	unsigned long microsecs();
+
+private:
+	unsigned long _time_micros;
+};
+
+/**
+ * Returns a time object for the time since the program started.
+ */
+Time time_now();
+
+#endif