1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
|
#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);
}
|
