From 991381a67d00f2fa8dcf0475172b0151b4310385 Mon Sep 17 00:00:00 2001 From: HampusM Date: Sun, 19 Dec 2021 20:14:11 +0100 Subject: refactor: use the GY521 sensor instead --- gyro.ino | 51 ------------------------ gyroscope.cpp | 124 ---------------------------------------------------------- gyroscope.hpp | 34 ---------------- src/gyro.cpp | 43 ++++++++++++++++++++ 4 files changed, 43 insertions(+), 209 deletions(-) delete mode 100644 gyro.ino delete mode 100644 gyroscope.cpp delete mode 100644 gyroscope.hpp create mode 100644 src/gyro.cpp 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 - -#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 -#include - -#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 -- cgit v1.2.3-18-g5258