-
Notifications
You must be signed in to change notification settings - Fork 1
/
IMUsManager.cpp
107 lines (87 loc) · 2.37 KB
/
IMUsManager.cpp
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
#include "IMUsManager.h"
#include "Multiplexer.h"
#include <Wire.h>
#include <tuple> // std::tuple, std::get, std::tie, std::ignore
void IMUsManager::operator[](int i) {
lookAt(i);
}
void IMUsManager::lookAt(int i) {
if (i > 7) {
DEBUG_PRINT.println("Channel index '" + String(i) + "' out of bounds.");
}
if (currentIndex != i) {
currentIndex = i;
mp.set(i);
extend = extends[i];
imu1[i];
imu2[i];
}
}
void IMUsManager::calibrateChannel(int channel) {
DEBUG_PRINT.println("Calibrating IMUs on multiplexer channel " + String(channel));
mp.set(channel);
byte error;
Wire.beginTransmission(i2c_addr_default);
error = Wire.endTransmission();
if (error == 0) {
imu1.Calibrate();
imu1.imu.setAccelRange(16);
imu1.imu.setGyroRange(2000);
}
Wire.beginTransmission(i2c_addr_jumper);
error = Wire.endTransmission();
if (error == 0) {
imu2.Calibrate();
imu2.imu.setAccelRange(16);
imu2.imu.setGyroRange(2000);
}
}
void IMUsManager::calibrateAllChannels() {
for (int i = 0; i < 8; i++) {
try {
calibrateChannel(i);
}
catch (String error) {
DEBUG_PRINT.println(error);
}
}
}
std::tuple<bool, bool> IMUsManager::initChannel(int channel) {
DEBUG_PRINT.println("Initializing IMUs on multiplexer channel " + String(channel));
byte error, address;
mp.set(channel);
Wire.beginTransmission(i2c_addr_default);
error = Wire.endTransmission();
if (error == 0) {
imu1.Calibrate();
if (imu1.start(i2c_addr_default)) {
imu1Good = true;
DEBUG_PRINT.println("imu1 good");
} else {
DEBUG_PRINT.println("Error in multiplexer channel " + String(channel) + String(error) + " at address " + String(address, HEX));
}
}
Wire.beginTransmission(i2c_addr_jumper);
error = Wire.endTransmission();
if (error == 0) {
imu2.Calibrate();
if (imu2.start(i2c_addr_jumper)) {
imu2Good = true;
DEBUG_PRINT.println("imu2 good");
} else {
DEBUG_PRINT.println("Error in multiplexer channel " + String(channel) + String(error) + " at address " + String(address, HEX));
}
}
extends[channel] = imu1Good && imu2Good;
return std::tuple<bool, bool>(imu1Good, imu2Good);
}
void IMUsManager::initAllChannels() {
for (int i = 0; i < 8; i++) {
try {
initChannel(i);
}
catch (String error) {
DEBUG_PRINT.println(error);
}
}
}