feat: make basic stuff work

3 files changed, 74 insertions, 57 deletions

diff --git a/gyro.ino b/gyro.ino
index 9037799..0f9aff6 100644
--- a/gyro.ino
+++ b/gyro.ino
@@ -1,28 +1,51 @@
+#include <Wire.h>
+
 #include "gyroscope.hpp"
 
+/*
+	Stops the execution of any more instructions.
+*/
+void stop()
+{
+	while (1)
+		;
+}
+
+Gyroscope gyroscope;
+
 void setup()
 {
 	Serial.begin(9600);
 
-	Serial.println("Hej!");
+	// Wait for Serial because the Arduino Leonardo is weird
+	while (!Serial)
+		;
 
-	auto gyroscope_id = Gyroscope::initialize();
+	int gyroscope_id = Gyroscope::initialize();
 
+	// Verifiy the gyroscope id
 	if (gyroscope_id == -1)
 	{
-		Serial.println("Failed to communicate with gyrosensor!");
-		exit(1);
+		Serial.println("Error: Failed to communicate with gyroscope component");
+		stop();
 	}
 
-	Gyroscope gyroscope(gyroscope_id);
+	gyroscope.setup(gyroscope_id);
+
+	Serial.println("Finished setup");
 }
 
 void loop()
 {
-	//auto gyroscope_data = read();
+	int16_t x, y, z;
+	gyroscope.readCoordinates(&x, &y, &z);
 
-	//char *out;
-	//sprintf(out, "X: %d  Y: %d  Z: %d", gyroscope_data.x, gyroscope_data.y, gyroscope_data.z);
+	Serial.print("X: ");
+	Serial.print((int)x);
+	Serial.print(" Y: ");
+	Serial.print((int)y);
+	Serial.print(" Z: ");
+	Serial.println((int)z);
 
-	// Serial.println("hej!");
+	delay(1000);
 }
\ No newline at end of file
diff --git a/gyroscope.cpp b/gyroscope.cpp
index 78d5ba3..e831c53 100644
--- a/gyroscope.cpp
+++ b/gyroscope.cpp
@@ -3,14 +3,9 @@
 
 #include "gyroscope.hpp"
 
-/*
-	Creates a gyroscope communicator.
-
-	@param _gyroscope_id The who ID of the gyroscope to communicate with
-*/
-Gyroscope::Gyroscope(int _gyroscope_id)
+void Gyroscope::setup(int id)
 {
-	gyroscope_id = _gyroscope_id;
+	_gyroscope_id = id;
 
 	writeRegistry(RegisterAddresses::LOW_ODR, 0x00);
 	writeRegistry(RegisterAddresses::CTRL4, 0x00);
@@ -23,37 +18,26 @@ Gyroscope::Gyroscope(int _gyroscope_id)
 }
 
 /*
-	Reads data from the gyroscope
+	Reads coordinates from the gyroscope
 
-	@returns Gyroscope data
+	@param x A pointer to a variable to store the x coordinate in
+	@param y A pointer to a variable to store the y coordinate in
+	@param z A pointer to a variable to store the z coordinate in
 */
-ReadGyroscopeData Gyroscope::read()
+void Gyroscope::readCoordinates(int16_t *x, int16_t *y, int16_t *z)
 {
-	Wire.beginTransmission(gyroscope_id);
+	Wire.beginTransmission(_gyroscope_id);
 	Wire.write(RegisterAddresses::OUT_X_L | (1 << 7));
 	Wire.endTransmission();
 
-	Wire.requestFrom(gyroscope_id, (uint8_t)6);
+	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;
+	*x = _read_next_coordinate();
+	*y = _read_next_coordinate();
+	*z = _read_next_coordinate();
 }
 
 /*
@@ -62,9 +46,9 @@ ReadGyroscopeData Gyroscope::read()
 	@param registry The address of a registry
 	@param value A value that will be written
 */
-void Gyroscope::writeRegistry(uint8_t registry, uint8_t value)
+void Gyroscope::writeRegistry(int8_t registry, int8_t value)
 {
-	Wire.beginTransmission(gyroscope_id);
+	Wire.beginTransmission(_gyroscope_id);
 
 	Wire.write(registry);
 	Wire.write(value);
@@ -81,18 +65,18 @@ int Gyroscope::initialize()
 {
 	Wire.begin();
 
-	auto high_id = testRegistry(Addresses::ID_HIGH, RegisterAddresses::WHO_AM_I);
+	auto high_id = Gyroscope::testRegistry(Addresses::ID_HIGH, RegisterAddresses::WHO_AM_I);
 
 	if (high_id == Addresses::WHO_ID)
 	{
-		return high_id;
+		return Addresses::ID_HIGH;
 	}
 
-	auto low_id = testRegistry(Addresses::ID_LOW, RegisterAddresses::WHO_AM_I);
+	auto low_id = Gyroscope::testRegistry(Addresses::ID_LOW, RegisterAddresses::WHO_AM_I);
 
 	if (low_id == Addresses::WHO_ID)
 	{
-		return low_id;
+		return Addresses::ID_LOW;
 	}
 
 	return -1;
@@ -105,7 +89,7 @@ int Gyroscope::initialize()
 	@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)
+int Gyroscope::testRegistry(int gyroscope_id, int8_t registry)
 {
 	Wire.beginTransmission(gyroscope_id);
 
@@ -116,7 +100,7 @@ int Gyroscope::testRegistry(int gyroscope_id, uint8_t registry)
 		return -1;
 	}
 
-	Wire.requestFrom(gyroscope_id, (uint8_t)1);
+	Wire.requestFrom(gyroscope_id, (int8_t)1);
 
 	if (!Wire.available())
 	{
@@ -125,3 +109,16 @@ int Gyroscope::testRegistry(int gyroscope_id, uint8_t registry)
 
 	return Wire.read();
 }
+
+/*
+	Reads the next coordinate given by the gyroscope.
+
+	@returns A coordinate (x, y or z)
+*/
+int16_t Gyroscope::_read_next_coordinate()
+{
+	int8_t low = Wire.read();
+	int8_t high = Wire.read();
+
+	return (int16_t)(high << 8 | low);
+}
diff --git a/gyroscope.hpp b/gyroscope.hpp
index 627aa8c..b57eafa 100644
--- a/gyroscope.hpp
+++ b/gyroscope.hpp
@@ -17,21 +17,18 @@ namespace RegisterAddresses
 	const int OUT_X_L = 0x28;
 }
 
-class ReadGyroscopeData
-{
-public:
-	int16_t x, y, z;
-};
-
 class Gyroscope
 {
 public:
-	int gyroscope_id;
-	Gyroscope(int gyroscope_id);
-	ReadGyroscopeData read();
-	void writeRegistry(uint8_t registry, uint8_t value);
+	void setup(int id);
+	void readCoordinates(int16_t *x, int16_t *y, int16_t *z);
+	void writeRegistry(int8_t registry, int8_t value);
 	static int initialize();
-	static int testRegistry(int gyroscope_id, uint8_t registry);
+	static int testRegistry(int gyroscope_id, int8_t registry);
+
+private:
+	int _gyroscope_id;
+	int16_t _read_next_coordinate();
 };
 
 #endif
\ No newline at end of file