1 files changed, 22 insertions, 73 deletions

diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index 9b1aa24..b876193 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,107 +1,56 @@
-#include "common/memory/shared_ptr.hpp"
-#include "sensor/calibration.hpp"
-#include "sensor/sensor.hpp"
-#include "serial.hpp"
 #include "status.hpp"
-#include "utils.hpp"
 
+#include <MPU6050_light.h>
 #include <Wire.h>
 #include <XInput.h>
 
-constexpr uint8_t SENSOR_ADDRESS = 0x68U;
-constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds
-constexpr unsigned int SENSOR_RETRY_TIME = 2000U;  // milliseconds
+constexpr unsigned int SENSOR_RETRY_TIME = 2000U; // milliseconds
 
 constexpr unsigned int BAUD_RATE = 9600U;
 
+constexpr int32_t JOY_MAX = 180;
+constexpr int32_t JOY_MIN = -180;
+
+#define LEONARDO_XINPUT_VID 0x045E // NOLINT(cppcoreguidelines-macro-usage)
+#define LEONARDO_XINPUT_PID 0x028E // NOLINT(cppcoreguidelines-macro-usage)
+
 void setup()
 {
 	initialize_status_leds();
 
-	auto sout = common::make_shared<SerialStream>(Serial, BAUD_RATE);
+	auto sensor = MPU6050(Wire);
 
-	auto sensor = common::make_shared<Sensor>(SENSOR_ADDRESS, Wire, SENSOR_THROTTLE_TIME);
+	Wire.begin();
 
-	sout->waitReady();
+	set_led_positive(HIGH);
 
-	while (!sensor->begin())
+	while (sensor.begin() != 0)
 	{
-		*sout << "Error: Could not connect to the _sensor-> Retrying after 2000 "
-				 "milliseconds..."
-			  << endl;
+		set_led_positive(LOW);
 
 		delay(SENSOR_RETRY_TIME);
 	}
 
-	if (!sensor->setAccelSensitivity(2)) // 8g
-	{
-		*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
-	{
-		*sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
-			  << static_cast<int>(sensor->getStatus()) << endl;
-
-		stop();
-	}
-
-	*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;
-
-		stop();
-	}
-
 	set_led_positive(HIGH);
 
-	*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 << "Starting..." << endl;
+	sensor.calcOffsets();
 
 	XInput.begin();
 
+	XInput.setRange(JOY_LEFT, JOY_MIN, JOY_MAX);
+
 	while (true)
 	{
-		delay(SENSOR_THROTTLE_TIME);
-
-		if (!sensor->read())
-		{
-			SensorStatus status = sensor->getStatus();
+		sensor.update();
 
-			if (status == SensorStatus::THROTTLED)
-			{
-				*sout << "Warning: The sensor was read too frequently and throttled"
-					  << endl;
-				continue;
-			}
+		auto pitch = static_cast<int32_t>(sensor.getAngleX());
 
-			*sout << "Error: Failed to read _sensor-> Status: "
-				  << static_cast<int>(status) << endl;
-			stop();
-		}
+		auto roll = static_cast<int32_t>(sensor.getAngleY());
 
-		*sout << "Pitch: " << sensor->getPitch() << "    Roll: " << sensor->getRoll()
-			  << endl;
+		XInput.setJoystick(JOY_LEFT, -roll, pitch);
 
-		XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor->getRoll()),
-						   static_cast<int32_t>(sensor->getPitch()));
+#if USB_VID != LEONARDO_XINPUT_VID && USB_PID != LEONARDO_XINPUT_PID
 		XInput.printDebug(Serial);
+#endif
 	}
 }