1 files changed, 57 insertions, 63 deletions

diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index f1e968e..53a1912 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,94 +1,87 @@
 #include "Arduino.h"
+
 #include "Wire.h"
 #include "calibration.hpp"
-#include "sensor.hpp"
+#include "sensor/sensor.hpp"
+#include "utils/memory.hpp"
+
+#include "utils/serial.hpp"
 
 #define LINE_CLEAR_LENGTH 30
 
+#define SENSOR_ADDRESS 0x68
 #define SENSOR_THROTTLE_TIME 50 // milliseconds
 
-Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME);
-
-char line_clear[LINE_CLEAR_LENGTH + 1];
+unique_ptr<SerialStream> sout_ptr = nullptr;
 
-/**
- * Stops code execution.
- */
-void stop()
-{
-	while (true) {}
-}
+unique_ptr<Sensor> sensor = nullptr;
 
 void setup()
 {
-	Serial.begin(9600);
+	sout_ptr = make_unique<SerialStream>(Serial, 9600);
 
-	// Wait for Serial because the Arduino Leonardo is weird
-	while (!Serial) {}
+	auto sout = *sout_ptr;
 
-	while (!sensor.begin())
+	sensor = make_unique<Sensor>(SENSOR_ADDRESS, &Wire, sout, SENSOR_THROTTLE_TIME);
+
+	while (!sensor->begin())
 	{
-		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 "
-					   "milliseconds...");
+		sout << "Error: Could not connect to the sensor. Retrying after 2000 "
+				"milliseconds..."
+			 << endl;
+
 		delay(2000);
 	}
 
-	if (!sensor.setAccelSensitivity(2)) // 8g
+	sout << "Skit!" << endl;
+
+	if (!sensor->setAccelSensitivity(2)) // 8g
 	{
-		Serial.print(
-			"Error: Failed to set the sensor's accelerometer sensitivity. Status: ");
-		Serial.println(static_cast<int>(sensor.getStatus()));
+		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
 	{
-		Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: ");
-		Serial.println(static_cast<int>(sensor.getStatus()));
+		sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
+			 << static_cast<int>(sensor->getStatus()) << endl;
+
 		stop();
 	}
 
-	Serial.println("Calibrating sensor...");
-	SensorCalibrator sensor_calibrator(&sensor);
+	sout << "Calibrating sensor..." << endl;
+
+	SensorCalibrator sensor_calibrator(sensor, sout);
 
 	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
 	{
-		Serial.print("Error: Sensor calibration timed out after ");
-		Serial.print(CALIBRATION_TIMEOUT);
-		Serial.println(" milliseconds");
+		sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
+			 << " milliseconds" << endl;
 
 		stop();
 	}
-	Serial.println("Finished calibrating sensor");
-
-	Serial.println("Calibration values:");
-
-	Serial.print("Accelerometer X: ");
-	Serial.println(sensor.accel_cal_x);
-
-	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);
+	sout << "Finished calibrating sensor\n";
 
-	Serial.print("Gyro Y: ");
-	Serial.println(sensor.gyro_cal_y);
+	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";
 
-	Serial.print("Gyro Z: ");
-	Serial.println(sensor.gyro_cal_z);
+	sout << "Starting..." << endl;
 
-	Serial.println("\nStarting...");
-
-	size_t prev_len = strlen(line_clear);
-	memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len);
+	sout << "Pitch: " << sensor->getPitch() << "  "
+		 << "Roll: " << sensor->getRoll() << endl;
 }
 
 void loop()
 {
+	/*
 	delay(SENSOR_THROTTLE_TIME);
 
 	if (!sensor.read())
@@ -97,24 +90,25 @@ void loop()
 
 		if (status == SensorStatus::THROTTLED)
 		{
-			Serial.println("Warning: The sensor was read too frequently and throttled");
+			printSerial("Warning: The sensor was read too frequently and throttled");
+			endl();
+
 			return;
 		}
 
-		Serial.print("Error: Failed to read sensor. Status: ");
-		Serial.println(static_cast<int>(status));
+		printSerial("Error: Failed to read sensor. Status: %d", static_cast<int>(status));
+		endl();
+
 		stop();
 	}
 
-	float pitch = sensor.getPitch();
-	float roll = sensor.getRoll();
+	char *pitch = floatToStr(sensor.getPitch(), FLOAT_WIDTH, FLOAT_PRECISION);
+	char *roll = floatToStr(sensor.getRoll(), FLOAT_WIDTH, FLOAT_PRECISION);
 
-	Serial.print(line_clear);
-	Serial.print("\r");
-	Serial.print("Pitch: ");
-	Serial.print(pitch, 3);
-	Serial.print(" Roll: ");
-	Serial.print(roll, 3);
-	Serial.print("\r");
+	printSerial("\rPitch: %s	Roll: %s", pitch, roll);
 	Serial.flush();
+
+	free(pitch);
+	free(roll);
+	*/
 }