add core files

4 files changed, 194 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..1899660
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+build
+.vscode
\ No newline at end of file
diff --git a/gyro.ino b/gyro.ino
new file mode 100644
index 0000000..9037799
--- /dev/null
+++ b/gyro.ino
@@ -0,0 +1,28 @@
+#include "gyroscope.hpp"
+
+void setup()
+{
+	Serial.begin(9600);
+
+	Serial.println("Hej!");
+
+	auto gyroscope_id = Gyroscope::initialize();
+
+	if (gyroscope_id == -1)
+	{
+		Serial.println("Failed to communicate with gyrosensor!");
+		exit(1);
+	}
+
+	Gyroscope gyroscope(gyroscope_id);
+}
+
+void loop()
+{
+	//auto gyroscope_data = read();
+
+	//char *out;
+	//sprintf(out, "X: %d  Y: %d  Z: %d", gyroscope_data.x, gyroscope_data.y, gyroscope_data.z);
+
+	// Serial.println("hej!");
+}
\ No newline at end of file
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();
+}
diff --git a/gyroscope.hpp b/gyroscope.hpp
new file mode 100644
index 0000000..627aa8c
--- /dev/null
+++ b/gyroscope.hpp
@@ -0,0 +1,37 @@
+#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 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);
+	static int initialize();
+	static int testRegistry(int gyroscope_id, uint8_t registry);
+};
+
+#endif
\ No newline at end of file