1 files changed, 52 insertions, 50 deletions

diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index d05b3ce..bee13b6 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,100 +1,102 @@
+#include "common/memory/shared_ptr.hpp"
 #include "sensor/calibration.hpp"
 #include "sensor/sensor.hpp"
 #include "serial.hpp"
 #include "utils.hpp"
 
-#include <Arduino.h>
 #include <Wire.h>
 #include <XInput.h>
 
 constexpr uint8_t SENSOR_ADDRESS = 0x68U;
-constexpr uint32_t SENSOR_THROTTLE_TIME = 50U; // milliseconds
-constexpr uint32_t SENSOR_RETRY_TIME = 2000U;  // milliseconds
+constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds
+constexpr unsigned int SENSOR_RETRY_TIME = 2000U;  // milliseconds
 
-constexpr uint32_t BAUD_RATE = 9600U;
-
-auto sout = SerialStream(Serial_(), BAUD_RATE);
-
-auto sensor = Sensor(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME);
+constexpr unsigned int BAUD_RATE = 9600U;
 
 void setup()
 {
-	sout.waitReady();
+	auto sout = common::make_shared<SerialStream>(Serial, BAUD_RATE);
+
+	auto sensor = common::make_shared<Sensor>(SENSOR_ADDRESS, Wire, SENSOR_THROTTLE_TIME);
 
-	while (!sensor.begin())
+	sout->waitReady();
+
+	while (!sensor->begin())
 	{
-		sout << "Error: Could not connect to the sensor. Retrying after 2000 "
-				"milliseconds..."
-			 << endl;
+		*sout << "Error: Could not connect to the _sensor-> Retrying after 2000 "
+				 "milliseconds..."
+			  << endl;
 
 		delay(SENSOR_RETRY_TIME);
 	}
 
-	if (!sensor.setAccelSensitivity(2)) // 8g
+	if (!sensor->setAccelSensitivity(2)) // 8g
 	{
-		sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
-			 << static_cast<int>(sensor.getStatus()) << endl;
+		*sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
+			  << static_cast<int>(sensor->getStatus()) << endl;
 
 		stop();
 	}
 
-	if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
+	if (!sensor->setGyroSensitivity(1)) // 500 degrees/s
 	{
-		sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
-			 << static_cast<int>(sensor.getStatus()) << endl;
+		*sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
+			  << static_cast<int>(sensor->getStatus()) << endl;
 
 		stop();
 	}
 
-	sout << "Calibrating sensor..." << endl;
+	*sout << "Calibrating _sensor->.." << endl;
 
 	SensorCalibrator sensor_calibrator(sensor, sout);
 
 	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
 	{
-		sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
-			 << " milliseconds" << endl;
+		*sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
+			  << " milliseconds" << endl;
 
 		stop();
 	}
 
-	sout << "Finished calibrating sensor\n";
+	*sout << "Finished calibrating sensor\n";
 
-	sout << "Calibration values:\n"
-		 << "Accelerometer X: " << sensor.accel_cal_x << "\n"
-		 << "Accelerometer Y: " << sensor.accel_cal_y << "\n"
-		 << "Accelerometer Z: " << sensor.accel_cal_z << "\n"
-		 << "Gyro X: " << sensor.gyro_cal_x << "\n"
-		 << "Gyro Y: " << sensor.gyro_cal_y << "\n"
-		 << "Gyro Z: " << sensor.gyro_cal_z << "\n";
+	*sout << "Calibration values:\n"
+		  << "Accelerometer X: " << sensor->accel_cal_x << "\n"
+		  << "Accelerometer Y: " << sensor->accel_cal_y << "\n"
+		  << "Accelerometer Z: " << sensor->accel_cal_z << "\n"
+		  << "Gyro X: " << sensor->gyro_cal_x << "\n"
+		  << "Gyro Y: " << sensor->gyro_cal_y << "\n"
+		  << "Gyro Z: " << sensor->gyro_cal_z << "\n";
 
-	sout << "Starting..." << endl;
+	*sout << "Starting..." << endl;
 
 	XInput.begin();
-}
 
-void loop()
-{
-	delay(SENSOR_THROTTLE_TIME);
-
-	if (!sensor.read())
+	while (true)
 	{
-		SensorStatus status = sensor.getStatus();
+		delay(SENSOR_THROTTLE_TIME);
 
-		if (status == SensorStatus::THROTTLED)
+		if (!sensor->read())
 		{
-			sout << "Warning: The sensor was read too frequently and throttled" << endl;
-			return;
+			SensorStatus status = sensor->getStatus();
+
+			if (status == SensorStatus::THROTTLED)
+			{
+				*sout << "Warning: The sensor was read too frequently and throttled"
+					  << endl;
+				continue;
+			}
+
+			*sout << "Error: Failed to read _sensor-> Status: "
+				  << static_cast<int>(status) << endl;
+			stop();
 		}
 
-		sout << "Error: Failed to read sensor. Status: " << static_cast<int>(status)
-			 << endl;
-		stop();
-	}
+		*sout << "Pitch: " << sensor->getPitch() << "    Roll: " << sensor->getRoll()
+			  << endl;
 
-	sout << "Pitch: " << sensor.getPitch() << "    Roll: " << sensor.getRoll() << endl;
-
-	XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor.getRoll()),
-					   static_cast<int32_t>(sensor.getPitch()));
-	XInput.printDebug(Serial);
+		XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor->getRoll()),
+						   static_cast<int32_t>(sensor->getPitch()));
+		XInput.printDebug(Serial);
+	}
 }