1 files changed, 127 insertions, 0 deletions
diff --git a/gyroscope.cpp b/gyroscope.cpp
new file mode 100644
index 0000000..78d5ba3
--- /dev/null
+++ b/gyroscope.cpp
@@ -0,0 +1,127 @@
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "gyroscope.hpp"
+
+/*
+	Creates a gyroscope communicator.
+
+	@param _gyroscope_id The who ID of the gyroscope to communicate with
+*/
+Gyroscope::Gyroscope(int _gyroscope_id)
+{
+	gyroscope_id = _gyroscope_id;
+
+	writeRegistry(RegisterAddresses::LOW_ODR, 0x00);
+	writeRegistry(RegisterAddresses::CTRL4, 0x00);
+
+	// 0x6F = 0b01101111
+	//
+	// DR1 DR0 BW1 BW0 PD Zen Yen Xen
+	//  0   1   1   0   1  1   1   1
+	writeRegistry(RegisterAddresses::CTRL1, 0x6F);
+}
+
+/*
+	Reads data from the gyroscope
+
+	@returns Gyroscope data
+*/
+ReadGyroscopeData Gyroscope::read()
+{
+	Wire.beginTransmission(gyroscope_id);
+	Wire.write(RegisterAddresses::OUT_X_L | (1 << 7));
+	Wire.endTransmission();
+
+	Wire.requestFrom(gyroscope_id, (uint8_t)6);
+
+	while (Wire.available() < 6)
+	{
+		Serial.println("Hej!");
+	}
+
+	uint8_t x_low = Wire.read();
+	uint8_t x_high = Wire.read();
+	uint8_t y_low = Wire.read();
+	uint8_t y_high = Wire.read();
+	uint8_t z_low = Wire.read();
+	uint8_t z_high = Wire.read();
+
+	ReadGyroscopeData data;
+
+	data.x = (int16_t)(x_high << 8 | x_low);
+	data.y = (int16_t)(y_high << 8 | y_low);
+	data.z = (int16_t)(z_high << 8 | z_low);
+
+	return data;
+}
+
+/*
+	Writes a value to a gyroscope registry.
+
+	@param registry The address of a registry
+	@param value A value that will be written
+*/
+void Gyroscope::writeRegistry(uint8_t registry, uint8_t value)
+{
+	Wire.beginTransmission(gyroscope_id);
+
+	Wire.write(registry);
+	Wire.write(value);
+
+	Wire.endTransmission();
+}
+
+/*
+	Initializes the gyroscope.
+
+	@returns The gyroscope's who id or -1 if it's unidentifiable
+*/
+int Gyroscope::initialize()
+{
+	Wire.begin();
+
+	auto high_id = testRegistry(Addresses::ID_HIGH, RegisterAddresses::WHO_AM_I);
+
+	if (high_id == Addresses::WHO_ID)
+	{
+		return high_id;
+	}
+
+	auto low_id = testRegistry(Addresses::ID_LOW, RegisterAddresses::WHO_AM_I);
+
+	if (low_id == Addresses::WHO_ID)
+	{
+		return low_id;
+	}
+
+	return -1;
+}
+
+/*
+	Reads from a gyroscope registry safely.
+
+	@param gyroscope_id The who ID of the gyroscope to read from
+	@param registry The address of a registry
+	@returns Registry data or -1 if there's no response.
+*/
+int Gyroscope::testRegistry(int gyroscope_id, uint8_t registry)
+{
+	Wire.beginTransmission(gyroscope_id);
+
+	Wire.write(registry);
+
+	if (Wire.endTransmission() != 0)
+	{
+		return -1;
+	}
+
+	Wire.requestFrom(gyroscope_id, (uint8_t)1);
+
+	if (!Wire.available())
+	{
+		return -1;
+	}
+
+	return Wire.read();
+}