-
Notifications
You must be signed in to change notification settings - Fork 9
/
extension.cpp
157 lines (129 loc) · 3.15 KB
/
extension.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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include "pxt.h"
#include "smallMPU6050.h"
using namespace pxt;
namespace microbit_GY521 {
MPU6050 mpu;
int16_t ax=0, ay=0, az=0;
int16_t gx=0, gy=0, gz=0;
#define Gyr_Gain 0.00763358
float dt=0.02;
float AccelX;
float AccelY;
float GyroY;
float mixY;
// ================================================================
// === CALIBRATION_ROUTINE ===
// ================================================================
// Simple calibration - just average first few readings to subtract
// from the later data
//%
bool calibrate_Sensors(int *offSets) {
mpu.initialize();
// Validate parameters are passed correctly
/*
if(offSets[0]!=-2694) return false;
if(offSets[1]!=593) return false;
if(offSets[2]!=485) return false;
if(offSets[3]!=75) return false;
if(offSets[4]!=15) return false;
if(offSets[5]!=21) return false;
*/
mpu.setXAccelOffset(offSets[0]);
mpu.setYAccelOffset(offSets[1]);
mpu.setZAccelOffset(offSets[2]);
mpu.setXGyroOffset(offSets[3]);
mpu.setYGyroOffset(offSets[4]);
mpu.setZGyroOffset(offSets[5]);
// Discard the first reading (don't know if this is needed or
// not, however, it won't hurt.)
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
return true;
}
//%
bool testConnection(){
return mpu.testConnection();
}
// Expose the raw accel and gyro data
//%
void getMotion6(){
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
}
//%
int readAccelX(){
return (int)ax;
}
//%
int readAccelY(){
return (int)ay;
}
//%
int readAccelZ(){
return (int)az;
}
//%
int readGyroX(){
return (int)gx;
}
//%
int readGyroY(){
return (int)gy;
}
//%
int readGyroZ(){
return (int)gz;
}
// Complementary filters used by the self balancing robot solution
//%
int computeY()
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
GyroY = Gyr_Gain * (gy)*-1;
AccelY = (atan2(ay, az) * 180 / PI);
AccelX = (atan2(ax, az) * 180 / PI);
float K = 0.98;
float A = K / (K + dt);
mixY = A *(mixY+GyroY*dt) + (1-A)*AccelX;
mpu.resetFIFO();
return mixY;
}
//%
void setXGyroOffset(int value){
mpu.setXGyroOffset(value);
}
//%
int getXGyroOffset(){
return mpu.getXGyroOffset();
}
//%
void setYGyroOffset(int value){
mpu.setYGyroOffset(value);
}
//%
int getYGyroOffset(){
return mpu.getYGyroOffset();
}
//%
void setZGyroOffset(int value){
mpu.setZGyroOffset(value);
}
//%
int getZGyroOffset(){
return mpu.getZGyroOffset();
}
//%
void setZAccelOffset(int value){
mpu.setZAccelOffset(value);
}
//%
int getYAccelOffset(){
return mpu.getYAccelOffset();
}
//%
int getXAccelOffset(){
return mpu.getXAccelOffset();
}
//%
int getZAccelOffset(){
return mpu.getZAccelOffset();
}
}