refactor: use the GY521 sensor instead

4 files changed, 43 insertions, 209 deletions

diff --git a/gyro.ino b/gyro.ino
deleted file mode 100644
index 0f9aff6..0000000
--- a/gyro.ino
+++ /dev/null
@@ -1,51 +0,0 @@
-#include <Wire.h>
-
-#include "gyroscope.hpp"
-
-/*
-	Stops the execution of any more instructions.
-*/
-void stop()
-{
-	while (1)
-		;
-}
-
-Gyroscope gyroscope;
-
-void setup()
-{
-	Serial.begin(9600);
-
-	// Wait for Serial because the Arduino Leonardo is weird
-	while (!Serial)
-		;
-
-	int gyroscope_id = Gyroscope::initialize();
-
-	// Verifiy the gyroscope id
-	if (gyroscope_id == -1)
-	{
-		Serial.println("Error: Failed to communicate with gyroscope component");
-		stop();
-	}
-
-	gyroscope.setup(gyroscope_id);
-
-	Serial.println("Finished setup");
-}
-
-void loop()
-{
-	int16_t x, y, z;
-	gyroscope.readCoordinates(&x, &y, &z);
-
-	Serial.print("X: ");
-	Serial.print((int)x);
-	Serial.print(" Y: ");
-	Serial.print((int)y);
-	Serial.print(" Z: ");
-	Serial.println((int)z);
-
-	delay(1000);
-}
\ No newline at end of file
diff --git a/gyroscope.cpp b/gyroscope.cpp
deleted file mode 100644
index e831c53..0000000
--- a/gyroscope.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-#include <Arduino.h>
-#include <Wire.h>
-
-#include "gyroscope.hpp"
-
-void Gyroscope::setup(int id)
-{
-	_gyroscope_id = 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 coordinates from the gyroscope
-
-	@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
-*/
-void Gyroscope::readCoordinates(int16_t *x, int16_t *y, int16_t *z)
-{
-	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)
-		;
-
-	*x = _read_next_coordinate();
-	*y = _read_next_coordinate();
-	*z = _read_next_coordinate();
-}
-
-/*
-	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(int8_t registry, int8_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 = Gyroscope::testRegistry(Addresses::ID_HIGH, RegisterAddresses::WHO_AM_I);
-
-	if (high_id == Addresses::WHO_ID)
-	{
-		return Addresses::ID_HIGH;
-	}
-
-	auto low_id = Gyroscope::testRegistry(Addresses::ID_LOW, RegisterAddresses::WHO_AM_I);
-
-	if (low_id == Addresses::WHO_ID)
-	{
-		return Addresses::ID_LOW;
-	}
-
-	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, int8_t registry)
-{
-	Wire.beginTransmission(gyroscope_id);
-
-	Wire.write(registry);
-
-	if (Wire.endTransmission() != 0)
-	{
-		return -1;
-	}
-
-	Wire.requestFrom(gyroscope_id, (int8_t)1);
-
-	if (!Wire.available())
-	{
-		return -1;
-	}
-
-	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
deleted file mode 100644
index b57eafa..0000000
--- a/gyroscope.hpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef GYROSCOPE_H
-#define GYROSCOPE_H
-
-namespace Addresses
-{
-	const int ID_HIGH = 0b1101011;
-	const int ID_LOW = 0b1101010;
-	const int WHO_ID = 0xD7;
-}
-
-namespace RegisterAddresses
-{
-	const int WHO_AM_I = 0x0F;
-	const int LOW_ODR = 0x39;
-	const int CTRL4 = 0x23;
-	const int CTRL1 = 0x20;
-	const int OUT_X_L = 0x28;
-}
-
-class Gyroscope
-{
-public:
-	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, int8_t registry);
-
-private:
-	int _gyroscope_id;
-	int16_t _read_next_coordinate();
-};
-
-#endif
\ No newline at end of file
diff --git a/src/gyro.cpp b/src/gyro.cpp
new file mode 100644
index 0000000..bdd68e9
--- /dev/null
+++ b/src/gyro.cpp
@@ -0,0 +1,43 @@
+#include "Arduino.h"

+#include "GY521.h"

+

+GY521 sensor(0x68);

+

+void setup() {

+	Serial.begin(9600);

+

+	// Wait for Serial because the Arduino Leonardo is weird

+	while (!Serial) {}

+

+	Wire.begin();

+

+	delay(100);

+	while (!sensor.wakeup()) {

+		Serial.print(millis());

+		Serial.println("Error: Could not connect to the GY521 sensor. Retrying after 2000 milliseconds...");

+		delay(2000);

+	}

+	sensor.setAccelSensitivity(2); // 8g

+	sensor.setGyroSensitivity(1);  // 500 degrees/s

+

+	sensor.setThrottle();

+	Serial.println("start...");

+

+	// Calibration values

+	sensor.axe = 0.198;

+	sensor.aye = -0.018;

+	sensor.gxe = 0.780;

+	sensor.gye = -1.495;

+}

+

+void loop() {

+	sensor.read();

+	float pitch = sensor.getPitch();

+	float roll = sensor.getRoll();

+

+	Serial.print("Pitch: ");

+	Serial.print(pitch, 3);

+	Serial.print(" Roll: ");

+	Serial.print(roll, 3);

+	Serial.println();

+}
\ No newline at end of file