blob: 78d5ba337b6a81b0862b40748f08517eb6182b45 (
plain)
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
125
126
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();
}
|
