1 | var Board = require("./board");
|
2 | var events = require("events");
|
3 | var util = require("util");
|
4 | var Fn = require("./fn");
|
5 |
|
6 | var sum = Fn.sum;
|
7 | var toFixed = Fn.toFixed;
|
8 |
|
9 | var priv = new Map();
|
10 | var axes = ["x", "y", "z"];
|
11 |
|
12 | var Controllers = {
|
13 | ANALOG: {
|
14 | initialize: {
|
15 | value: function(opts, dataHandler) {
|
16 | var pins = opts.pins || [],
|
17 | sensitivity, resolution,
|
18 | state = priv.get(this),
|
19 | dataPoints = {};
|
20 |
|
21 | if (opts.sensitivity === undefined) {
|
22 | throw new Error("Expected a Sensitivity");
|
23 | }
|
24 |
|
25 |
|
26 |
|
27 |
|
28 | sensitivity = opts.sensitivity;
|
29 | resolution = opts.resolution || 4.88;
|
30 | state.K = resolution / sensitivity;
|
31 |
|
32 | pins.forEach(function(pin, index) {
|
33 | this.io.pinMode(pin, this.io.MODES.ANALOG);
|
34 | this.io.analogRead(pin, function(data) {
|
35 | var axis = axes[index];
|
36 | dataPoints[axis] = data;
|
37 | dataHandler(dataPoints);
|
38 | }.bind(this));
|
39 | }, this);
|
40 | }
|
41 | },
|
42 | toNormal: {
|
43 | value: function(raw) {
|
44 | return raw >> 2;
|
45 | }
|
46 | },
|
47 | toDegreesPerSecond: {
|
48 | value: function(raw, rawCenter) {
|
49 | var normal = this.toNormal(raw);
|
50 | var center = this.toNormal(rawCenter);
|
51 | var state = priv.get(this);
|
52 |
|
53 | return ((normal - center) * state.K) | 0;
|
54 | }
|
55 | }
|
56 | },
|
57 |
|
58 |
|
59 | MPU6050: {
|
60 | initialize: {
|
61 | value: function(opts, dataHandler) {
|
62 | var IMU = require("./imu");
|
63 | var state = priv.get(this),
|
64 | driver = IMU.Drivers.get(this.board, "MPU6050", opts);
|
65 |
|
66 | state.sensitivity = opts.sensitivity || 131;
|
67 |
|
68 | driver.on("data", function(data) {
|
69 | dataHandler(data.gyro);
|
70 | });
|
71 | }
|
72 | },
|
73 | toNormal: {
|
74 | value: function(raw) {
|
75 | return (raw >> 11) + 127;
|
76 | }
|
77 | },
|
78 | toDegreesPerSecond: {
|
79 |
|
80 |
|
81 |
|
82 |
|
83 |
|
84 |
|
85 |
|
86 | value: function(raw, rawCenter) {
|
87 | var state = priv.get(this);
|
88 |
|
89 | return toFixed((raw - rawCenter) / state.sensitivity, 4);
|
90 | }
|
91 | }
|
92 | },
|
93 | BNO055: {
|
94 | initialize: {
|
95 | value: function(opts, dataHandler) {
|
96 | var IMU = require("./imu");
|
97 | var state = priv.get(this),
|
98 | driver = IMU.Drivers.get(this.board, "BNO055", opts);
|
99 |
|
100 |
|
101 |
|
102 |
|
103 | state.sensitivity = 16;
|
104 |
|
105 | driver.on("data", function(data) {
|
106 | dataHandler(data.gyro);
|
107 | });
|
108 | }
|
109 | },
|
110 | toNormal: {
|
111 | value: function(raw) {
|
112 | return raw;
|
113 | }
|
114 | },
|
115 | toDegreesPerSecond: {
|
116 |
|
117 |
|
118 | value: function(raw) {
|
119 | var state = priv.get(this);
|
120 | return toFixed(raw / state.sensitivity, 4);
|
121 | }
|
122 | }
|
123 | },
|
124 | };
|
125 |
|
126 | function Gyro(opts) {
|
127 | if (!(this instanceof Gyro)) {
|
128 | return new Gyro(opts);
|
129 | }
|
130 |
|
131 | var controller = null;
|
132 | var isCalibrated = false;
|
133 | var sampleSize = 100;
|
134 |
|
135 | var state = {
|
136 | x: {
|
137 | angle: 0,
|
138 | value: 0,
|
139 | previous: 0,
|
140 | calibration: [],
|
141 | stash: [0, 0, 0, 0, 0],
|
142 | center: 0,
|
143 | hasValue: false
|
144 | },
|
145 | y: {
|
146 | angle: 0,
|
147 | value: 0,
|
148 | previous: 0,
|
149 | calibration: [],
|
150 | stash: [0, 0, 0, 0, 0],
|
151 | center: 0,
|
152 | hasValue: false
|
153 | },
|
154 | z: {
|
155 | angle: 0,
|
156 | value: 0,
|
157 | previous: 0,
|
158 | calibration: [],
|
159 | stash: [0, 0, 0, 0, 0],
|
160 | center: 0,
|
161 | hasValue: false
|
162 | }
|
163 | };
|
164 |
|
165 | Board.Component.call(
|
166 | this, opts = Board.Options(opts)
|
167 | );
|
168 |
|
169 | if (opts.controller && typeof opts.controller === "string") {
|
170 | controller = Controllers[opts.controller.toUpperCase()];
|
171 | } else {
|
172 | controller = opts.controller;
|
173 | }
|
174 |
|
175 | if (controller == null) {
|
176 | controller = Controllers.ANALOG;
|
177 | }
|
178 |
|
179 | Board.Controller.call(this, controller, opts);
|
180 |
|
181 | if (!this.toNormal) {
|
182 | this.toNormal = opts.toNormal || function(raw) {
|
183 | return raw;
|
184 | };
|
185 | }
|
186 |
|
187 | if (!this.toDegreesPerSecond) {
|
188 | this.toDegreesPerSecond = opts.toDegreesPerSecond || function(raw) {
|
189 | return raw;
|
190 | };
|
191 | }
|
192 |
|
193 | priv.set(this, state);
|
194 |
|
195 | if (typeof this.initialize === "function") {
|
196 | this.initialize(opts, function(data) {
|
197 | var isChange = false;
|
198 |
|
199 | Object.keys(data).forEach(function(axis) {
|
200 | var value = data[axis];
|
201 | var sensor = state[axis];
|
202 |
|
203 | sensor.previous = sensor.value;
|
204 | sensor.stash.shift();
|
205 | sensor.stash.push(value);
|
206 | sensor.hasValue = true;
|
207 | sensor.value = (sum(sensor.stash) / 5) | 0;
|
208 |
|
209 | if (!isCalibrated &&
|
210 | (state.x.calibration.length === sampleSize &&
|
211 | state.y.calibration.length === sampleSize &&
|
212 | (this.z === undefined || state.z.calibration.length === sampleSize))) {
|
213 |
|
214 | isCalibrated = true;
|
215 | state.x.center = (sum(state.x.calibration) / sampleSize) | 0;
|
216 | state.y.center = (sum(state.y.calibration) / sampleSize) | 0;
|
217 | state.z.center = (sum(state.z.calibration) / sampleSize) | 0;
|
218 |
|
219 | state.x.calibration.length = 0;
|
220 | state.y.calibration.length = 0;
|
221 | state.z.calibration.length = 0;
|
222 | } else {
|
223 | if (sensor.calibration.length < sampleSize) {
|
224 | sensor.calibration.push(value);
|
225 | }
|
226 | }
|
227 |
|
228 | if (sensor.previous !== sensor.value) {
|
229 | isChange = true;
|
230 | }
|
231 | }, this);
|
232 |
|
233 | if (isCalibrated) {
|
234 | state.x.angle += this.rate.x / 100;
|
235 | state.y.angle += this.rate.y / 100;
|
236 | state.z.angle += this.rate.z / 100;
|
237 |
|
238 | this.emit("data", {
|
239 | x: this.x,
|
240 | y: this.y,
|
241 | z: this.z
|
242 | });
|
243 |
|
244 | if (isChange) {
|
245 | this.emit("change", {
|
246 | x: this.x,
|
247 | y: this.y,
|
248 | z: this.z
|
249 | });
|
250 | }
|
251 | }
|
252 | }.bind(this));
|
253 | }
|
254 |
|
255 | Object.defineProperties(this, {
|
256 | isCalibrated: {
|
257 | get: function() {
|
258 | return isCalibrated;
|
259 | },
|
260 | set: function(value) {
|
261 | if (typeof value === "boolean") {
|
262 | isCalibrated = value;
|
263 | }
|
264 | }
|
265 | },
|
266 | pitch: {
|
267 | get: function() {
|
268 | return {
|
269 | rate: toFixed(this.rate.y, 2),
|
270 | angle: toFixed(state.y.angle, 2)
|
271 | };
|
272 | }
|
273 | },
|
274 | roll: {
|
275 | get: function() {
|
276 | return {
|
277 | rate: toFixed(this.rate.x, 2),
|
278 | angle: toFixed(state.x.angle, 2)
|
279 | };
|
280 | }
|
281 | },
|
282 | yaw: {
|
283 | get: function() {
|
284 | return {
|
285 | rate: this.z !== undefined ? toFixed(this.rate.z, 2) : 0,
|
286 | angle: this.z !== undefined ? toFixed(state.z.angle, 2) : 0
|
287 | };
|
288 | }
|
289 | },
|
290 | x: {
|
291 | get: function() {
|
292 | return toFixed(this.toNormal(state.x.value), 4);
|
293 | }
|
294 | },
|
295 | y: {
|
296 | get: function() {
|
297 | return toFixed(this.toNormal(state.y.value), 4);
|
298 | }
|
299 | },
|
300 | z: {
|
301 | get: function() {
|
302 | return state.z.hasValue ? toFixed(this.toNormal(state.z.value), 4) : undefined;
|
303 | }
|
304 | },
|
305 | rate: {
|
306 | get: function() {
|
307 | var x = this.toDegreesPerSecond(state.x.value, state.x.center);
|
308 | var y = this.toDegreesPerSecond(state.y.value, state.y.center);
|
309 | var z = state.z.hasValue ?
|
310 | this.toDegreesPerSecond(state.z.value, state.z.center) : 0;
|
311 |
|
312 | return {
|
313 | x: x,
|
314 | y: y,
|
315 | z: z
|
316 | };
|
317 | }
|
318 | }
|
319 | });
|
320 | }
|
321 |
|
322 | Object.defineProperties(Gyro, {
|
323 | TK_4X: {
|
324 | value: 0.67
|
325 | },
|
326 | TK_1X: {
|
327 | value: 0.167
|
328 | }
|
329 | });
|
330 |
|
331 |
|
332 | util.inherits(Gyro, events.EventEmitter);
|
333 |
|
334 | Gyro.prototype.recalibrate = function() {
|
335 | this.isCalibrated = false;
|
336 | };
|
337 |
|
338 |
|
339 | if (!!process.env.IS_TEST_MODE) {
|
340 | Gyro.Controllers = Controllers;
|
341 | Gyro.purge = function() {
|
342 | priv.clear();
|
343 | };
|
344 | }
|
345 | module.exports = Gyro;
|