1 files changed, 32 insertions, 9 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