Commit | Line | Data |
---|---|---|
7670b3f4 | 1 | package org.usfirst.frc.team3501.robot.sensors; |
191c43e3 E |
2 | /** |
3 | * Copyright (c) 2015, www.techhounds.com | |
4 | * All rights reserved. | |
5 | * | |
6 | * <p> | |
7 | * Redistribution and use in source and binary forms, with or without | |
8 | * modification, are permitted provided that the following conditions are met: | |
9 | * </p> | |
10 | * <ul> | |
11 | * <li>Redistributions of source code must retain the above copyright | |
12 | * notice, this list of conditions and the following disclaimer.</li> | |
13 | * <li>Redistributions in binary form must reproduce the above copyright | |
14 | * notice, this list of conditions and the following disclaimer in the | |
15 | * documentation and/or other materials provided with the distribution.</li> | |
16 | * <li>Neither the name of the www.techhounds.com nor the | |
17 | * names of its contributors may be used to endorse or promote products | |
18 | * derived from this software without specific prior written permission.</li> | |
19 | * </ul> | |
20 | * | |
21 | * <p> | |
22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND | |
23 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | |
24 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | |
25 | * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY | |
26 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | |
27 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | |
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | |
29 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | |
30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | |
31 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | |
32 | * </p> | |
33 | */ | |
34 | ||
35 | import java.io.File; | |
36 | import java.io.IOException; | |
37 | import java.io.PrintStream; | |
38 | ||
39 | import edu.wpi.first.wpilibj.I2C; | |
40 | import edu.wpi.first.wpilibj.PIDSourceType; | |
41 | import edu.wpi.first.wpilibj.Timer; | |
191c43e3 E |
42 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; |
43 | ||
44 | /** | |
45 | * A Java wrapper around the ITC based ITG-3200 triple axis gryo. | |
46 | * | |
47 | * <p> | |
48 | * Typical usage: | |
49 | * </p> | |
50 | * <ul> | |
51 | * <li>Make sure your ITG-3200 is connected to the I2C bus on the roboRIO and | |
52 | * mounted flat (so Z axis is used to track direction robot is facing).</li> | |
da6933e2 | 53 | * <li>Construct a single instance of the {@link GyroLib} class to be |
191c43e3 E |
54 | * shared throughout your Robot code.</li> |
55 | * <li>Use the {@link #getRotationZ()} method create "trackers" that allow you | |
56 | * to keep track of how much your robot has rotated (direction your robot is | |
57 | * facing).</li> | |
58 | * </ul> | |
59 | * <p> | |
60 | * Be aware of the following: | |
61 | * </p> | |
62 | * <ul> | |
63 | * <li>Angles are in signed degrees (both positive and negative values are | |
64 | * possible) and not necessarily normalized (large values like -1203 degrees are | |
65 | * possible).</li> | |
66 | * <li>The {@link #reset} method is called once initially at construction. | |
67 | * Reseting the gyro should only be done when your robot is stationary and it | |
68 | * may take up to one second. You should not need to do this and should avoid | |
69 | * doing this during the autonomous or teleop periods (unless you know your | |
70 | * robot won't be moving).</li> | |
71 | * <li>There is a background thread that is automatically started that keeps | |
72 | * reading and accumulating values from the ITG-3200. You should not need to use | |
73 | * the {@link #start()} or {@link #stop()} methods during normal matches. | |
74 | * However, if you only use the gyro during the autonomous period, you can use | |
75 | * the {@link #stop()} method at the end of the autonomous period to save some | |
76 | * CPU.</li> | |
77 | * </ul> | |
78 | * | |
79 | * <h2>Suggested Usage</h2> | |
80 | * <p> | |
81 | * You should be able to use this class to aid your robot in making relative | |
82 | * turns. For example, if you want to create a command which rotates your robot | |
83 | * 90 degrees. | |
84 | * </p> | |
85 | * <ul> | |
86 | * <li>In your command's initialize method, use the {@link #getRotationZ()} and | |
87 | * the {@link Rotation#zero()} method on the returned {@link Rotation} object to | |
88 | * track how much you have turned.</li> | |
89 | * <li>Use the {@link Rotation} object as a PID source and/or check the current | |
90 | * angle reported by the {@link Rotation} object in your isFinished() method. | |
91 | * </li> | |
92 | * </ul> | |
93 | */ | |
da6933e2 | 94 | public final class GyroLib { |
191c43e3 E |
95 | |
96 | /** | |
97 | * Object used to monitor robot rotation. | |
98 | * | |
99 | * <ul> | |
100 | * <li>Use this class to track how much your robot has rotated.</li> | |
da6933e2 | 101 | * <li>Use the {@link GyroLib#getRotationZ()} to create an instance |
191c43e3 E |
102 | * to track how much your robot has rotated around the z-axis (direction robot |
103 | * is facing - useful for making turns)..</li> | |
da6933e2 | 104 | * <li>Use the {@link GyroLib#getRotationX()} to create an instance |
191c43e3 E |
105 | * to track how much your robot has rotated around the x-axis (hopefully not |
106 | * much unless you are tipping).</li> | |
da6933e2 | 107 | * <li>Use the {@link GyroLib#getRotationY()} to create an instance |
191c43e3 E |
108 | * to track how much your robot has rotated around the y-axis (hopefully not |
109 | * much unless you are tipping).</li> | |
da6933e2 | 110 | * <li>Use the {@link GyroLib#getRo |
191c43e3 E |
111 | * </ul> |
112 | */ | |
1e039ebd | 113 | public final class Rotation implements RotationTracker { |
191c43e3 E |
114 | /** Raw axis accumulator on gyro associated with this rotation tracker. */ |
115 | private Accumulator m_axis; | |
116 | /** | |
117 | * The degrees reported by the accumulator the last time this tracker was | |
118 | * zeroed. | |
119 | */ | |
120 | private double m_zeroDeg; | |
121 | /** | |
122 | * The number of readings reported by the accumulator the last time this | |
123 | * tracker was zeroed. | |
124 | */ | |
125 | private int m_zeroCnt; | |
126 | ||
127 | /** | |
128 | * Constructor is protected, instances are created through the | |
da6933e2 | 129 | * {@link GyroLib} methods. |
191c43e3 E |
130 | * |
131 | * @param axis | |
132 | * An accumulator from the gyro for the axis to be tracked. | |
133 | */ | |
134 | private Rotation(Accumulator axis) { | |
135 | m_axis = axis; | |
136 | m_zeroDeg = 0; | |
137 | m_zeroCnt = 0; | |
138 | } | |
139 | ||
140 | /** | |
141 | * Zero the tracker (sets the current heading/direction as the zero point). | |
142 | */ | |
143 | public void zero() { | |
144 | m_zeroDeg = m_axis.getDegrees(); | |
145 | m_zeroCnt = m_axis.getReadings(); | |
146 | } | |
147 | ||
148 | /** | |
149 | * Get the number of degrees rotated since last zeroed. | |
150 | * | |
151 | * @return A signed number of degrees [-INF, +INF]. | |
152 | */ | |
153 | public double getAngle() { | |
154 | double angle = m_axis.getDegrees() - m_zeroDeg; | |
155 | return angle; | |
156 | } | |
157 | ||
158 | /** | |
159 | * Get the total number of times the raw values from the gyro have been read | |
160 | * since zeroed. | |
161 | * | |
162 | * @return A diagnostic count that can be used to make sure the angle is | |
163 | * still being updated. | |
164 | */ | |
165 | public int getReadings() { | |
166 | return m_axis.getReadings() - m_zeroCnt; | |
167 | } | |
168 | ||
169 | /** | |
170 | * Returns the last raw (integer) value read from the gyro for the axis. | |
171 | * | |
172 | * @return An integer value from the ITG-3200 for the associated axis. | |
173 | */ | |
174 | public int getAngleRateRaw() { | |
175 | return m_axis.getRaw(); | |
176 | } | |
177 | ||
178 | /** | |
179 | * Returns the current rotation rate in degrees/second from the last | |
180 | * reading. | |
181 | * | |
182 | * @return How quickly the system is rotating about the axis in | |
183 | * degrees/second. | |
184 | */ | |
185 | public double getAngleRate() { | |
186 | return getAngleRateRaw() * COUNT_TO_DEGSEC; | |
187 | } | |
188 | ||
189 | /** | |
190 | * Returns the angle value from {@link #getAngle()} so object can be used as | |
191 | * a source to a PID controller. | |
192 | * | |
193 | * @return See {@link #getAngle()}. | |
194 | * | |
195 | * @see edu.wpi.first.wpilibj.PIDSource#pidGet() | |
196 | */ | |
197 | public double pidGet() { | |
198 | return getAngle(); | |
199 | } | |
200 | ||
201 | public void setPIDSourceType(PIDSourceType pidSource) { | |
202 | // TODO Auto-generated method stub | |
203 | ||
204 | } | |
205 | ||
206 | public PIDSourceType getPIDSourceType() { | |
207 | // TODO Auto-generated method stub | |
208 | return null; | |
209 | } | |
191c43e3 E |
210 | } |
211 | ||
212 | // | |
213 | // List of I2C registers which the ITG-3200 uses from the datasheet | |
214 | // | |
215 | private static final byte WHO_AM_I = 0x00; | |
216 | private static final byte SMPLRT_DIV = 0x15; | |
217 | private static final byte DLPF_FS = 0x16; | |
218 | // private static final byte INT_CFG = 0x17; | |
219 | // private static final byte INT_STATUS = 0x1A; | |
220 | // private static final byte TEMP_OUT_H = 0x1B; | |
221 | // private static final byte TEMP_OUT_L = 0x1C; | |
222 | private static final byte GYRO_XOUT_H = 0x1D; | |
223 | // private static final byte GYRO_XOUT_L = 0x1E; | |
224 | // private static final byte GYRO_YOUT_H = 0x1F; | |
225 | // private static final byte GYRO_YOUT_L = 0x20; | |
226 | // private static final byte GYRO_ZOUT_H = 0x21; | |
227 | // private static final byte GYRO_ZOUT_L = 0x22; | |
228 | ||
229 | // | |
230 | // Bit flags used for interrupt operation | |
231 | // | |
232 | ||
233 | /* | |
234 | * Set this bit in INT_CFG register for logic level of INT output pin to be | |
235 | * low when interrupt is active (leave 0 if you want it high). | |
236 | */ | |
237 | // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7); | |
238 | ||
239 | /* | |
240 | * Set drive type for interrupt to open drain mode, omit if you want push-pull | |
241 | * mode (what does this mean?). | |
242 | */ | |
243 | // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6; | |
244 | ||
245 | /* | |
246 | * Interrupt latch mode (remains set until you clear it), omit this flag to | |
247 | * get a 0-50us interrupt pulse. | |
248 | */ | |
249 | // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5; | |
250 | ||
251 | /* | |
252 | * Allow any read operation of data to clear the interrupt flag (otherwise it | |
253 | * is only cleared after reading status register). | |
254 | */ | |
255 | // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4; | |
256 | ||
257 | /* | |
258 | * Enable interrup when device is ready (PLL ready after changing clock | |
259 | * source). Hmmm? | |
260 | */ | |
261 | // private static final byte INT_CFG_RDY_EN = 1 << 3; | |
262 | ||
263 | /* Enable interrupt when new data is available. */ | |
264 | // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1; | |
265 | ||
266 | /* Guess at mode to use if we want to try enabling interrupts. */ | |
267 | // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN | | |
268 | // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN); | |
269 | ||
270 | // | |
271 | // The bit flags that can be set in the DLPF register on the ITG-3200 | |
272 | // as specified in the ITG-3200 data sheet. | |
273 | // | |
274 | ||
275 | // | |
276 | // The low pass filter bandwidth settings | |
277 | // | |
278 | ||
279 | // private static final byte DLPF_LOWPASS_256HZ = 0 << 0; | |
280 | private static final byte DLPF_LOWPASS_188HZ = 1 << 0; | |
281 | // private static final byte DLPF_LOWPASS_98HZ = 2 << 0; | |
282 | // private static final byte DLPF_LOWPASS_42HZ = 3 << 0; | |
283 | // private static final byte DLPF_LOWPASS_20HZ = 4 << 0; | |
284 | // private static final byte DLPF_LOWPASS_10HZ = 5 << 0; | |
285 | // private static final byte DLPF_LOWPASS_5HZ = 6 << 0; | |
286 | ||
287 | /** Select range of +/-2000 deg/sec. (only range supported). */ | |
288 | private static final byte DLPF_FS_SEL_2000 = 3 << 3; | |
289 | ||
290 | /** | |
291 | * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high. | |
292 | */ | |
293 | private static final byte itgAddressJumper = 0x69; | |
294 | ||
295 | /** | |
296 | * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low. | |
297 | */ | |
298 | private static final byte itgAddressNoJumper = 0x68; | |
299 | ||
300 | /** Multiplier to convert raw integer values returned to degrees/sec. */ | |
301 | private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375); | |
302 | ||
303 | /** Set this to true for lots of diagnostic output. */ | |
304 | private static final boolean DEBUG = false; | |
305 | ||
306 | /** | |
307 | * How many sample readings to make to determine the bias value for each axis. | |
308 | */ | |
309 | private static final int MIN_READINGS_TO_SET_BIAS = 50; | |
310 | ||
311 | /** I2C Address to use to communicate with the ITG-3200. */ | |
312 | private byte m_addr; | |
313 | ||
314 | /** I2C object used to communicate with Gyro. */ | |
315 | private I2C m_i2c; | |
316 | ||
317 | /** | |
318 | * Background thread responsible for accumulating angle data from the sensor. | |
319 | */ | |
320 | private Thread m_thread; | |
321 | ||
322 | /** Flag used to signal background thread that the gyro should be reset. */ | |
323 | private boolean m_need_reset; | |
324 | ||
325 | /** Flag used to signal background thread that it's time to stop. */ | |
326 | private boolean m_run_thread; | |
327 | ||
328 | /** Accumulator for rotation around the x-axis. */ | |
329 | private Accumulator m_x; | |
330 | ||
331 | /** Accumulator for rotation around the y-axis. */ | |
332 | private Accumulator m_y; | |
333 | ||
334 | /** Accumulator for rotation around the z-axis. */ | |
335 | private Accumulator m_z; | |
336 | private int[] m_xBuffer; | |
337 | private int[] m_yBuffer; | |
338 | private int[] m_zBuffer; | |
339 | private int m_cntBuffer; | |
340 | private int m_sizeBuffer; | |
341 | private double[] m_timeBuffer; | |
342 | ||
343 | /** | |
344 | * Construct a new instance of the ITG-3200 gryo class. | |
345 | * | |
346 | * <p> | |
347 | * IMPORTANT | |
348 | * | |
349 | * @param port | |
350 | * This should be {@link I2C.Port#kOnboard} if the ITG-3200 is | |
351 | * connected to the main I2C bus on the roboRIO. This should be | |
352 | * {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP | |
353 | * port on the roboRIO. | |
354 | * @param jumper | |
355 | * This should be true if the ITG-3200 has the AD0 jumpered to logic | |
356 | * level high and false if not. | |
357 | */ | |
da6933e2 | 358 | public GyroLib(I2C.Port port, boolean jumper) { |
191c43e3 E |
359 | m_addr = (jumper ? itgAddressJumper : itgAddressNoJumper); |
360 | m_i2c = new I2C(port, m_addr); | |
361 | m_thread = new Thread(new Runnable() { | |
362 | @Override | |
363 | public void run() { | |
364 | accumulateData(); | |
365 | } | |
366 | }); | |
367 | if (DEBUG) { | |
368 | check(); | |
369 | } | |
370 | m_x = new Accumulator(); | |
371 | m_y = new Accumulator(); | |
372 | m_z = new Accumulator(); | |
373 | m_need_reset = true; | |
374 | ||
375 | start(); | |
376 | } | |
377 | ||
378 | /** | |
379 | * Construct a {@link Rotation} object used to monitor rotation about the | |
380 | * Z-axis. | |
381 | * | |
382 | * @return A rotation object that very useful for checking the direction your | |
383 | * robot is facing. | |
384 | */ | |
385 | public Rotation getRotationZ() { | |
386 | return new Rotation(m_z); | |
387 | } | |
388 | ||
389 | /** | |
390 | * Construct a {@link Rotation} object used to monitor rotation about the | |
391 | * X-axis. | |
392 | * | |
393 | * @return A rotation object that is probably only useful for checking if your | |
394 | * robot is starting to tip over. | |
395 | */ | |
396 | public Rotation getRotationX() { | |
397 | return new Rotation(m_x); | |
398 | } | |
399 | ||
400 | /** | |
401 | * Construct a {@link Rotation} object used to monitor rotation about the | |
402 | * Y-axis. | |
403 | * | |
404 | * @return A rotation object that is probably only useful for checking if your | |
405 | * robot is starting to tip over. | |
406 | */ | |
407 | public Rotation getRotationY() { | |
408 | return new Rotation(m_y); | |
409 | } | |
410 | ||
411 | /** | |
412 | * Returns string representation of the object for debug purposes. | |
413 | */ | |
414 | public String toString() { | |
415 | return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]"; | |
416 | } | |
417 | ||
418 | /** | |
419 | * Dumps information about the state of the Gyro to the smart dashboard. | |
420 | * | |
421 | * @param tag | |
422 | * Short name like "Gyro" to prefix each label with on the dashboard. | |
423 | * @param debug | |
424 | * Pass true if you want a whole lot of details dumped onto the | |
425 | * dashboard, pass false if you just want the direction of each axis | |
426 | * and the temperature. | |
427 | */ | |
428 | public void updateDashboard(String tag, boolean debug) { | |
429 | SmartDashboard.putNumber(tag + " x-axis degrees", m_x.getDegrees()); | |
430 | SmartDashboard.putNumber(tag + " y-axis degrees", m_y.getDegrees()); | |
431 | SmartDashboard.putNumber(tag + " z-axis degrees", m_z.getDegrees()); | |
432 | ||
433 | if (debug) { | |
434 | SmartDashboard.putNumber(tag + " x-axis raw", m_x.getRaw()); | |
435 | SmartDashboard.putNumber(tag + " y-axis raw", m_y.getRaw()); | |
436 | SmartDashboard.putNumber(tag + " z-axis raw", m_z.getRaw()); | |
437 | ||
438 | SmartDashboard.putNumber(tag + " x-axis count", m_x.getReadings()); | |
439 | SmartDashboard.putNumber(tag + " y-axis count", m_y.getReadings()); | |
440 | SmartDashboard.putNumber(tag + " z-axis count", m_z.getReadings()); | |
441 | ||
442 | SmartDashboard.putString(tag + " I2C Address", | |
443 | "0x" + Integer.toHexString(m_addr)); | |
444 | } | |
445 | } | |
446 | ||
447 | /** | |
448 | * Internal method that runs in the background thread to accumulate data from | |
449 | * the Gyro. | |
450 | */ | |
451 | private void accumulateData() { | |
452 | m_run_thread = true; | |
453 | int resetCnt = 0; | |
454 | ||
455 | while (m_run_thread) { | |
456 | if (m_need_reset) { | |
457 | // Set gyro to the proper mode | |
458 | performResetSequence(); | |
459 | ||
460 | // Reset accumulators and set the number of readings to take to | |
461 | // compute bias values | |
462 | resetCnt = MIN_READINGS_TO_SET_BIAS; | |
463 | m_x.reset(); | |
464 | m_y.reset(); | |
465 | m_z.reset(); | |
466 | m_need_reset = false; | |
467 | } else { | |
468 | // Go read raw values from ITG-3200 and update our accumulators | |
469 | readRawAngleBytes(); | |
470 | ||
471 | if (resetCnt > 0) { | |
472 | // If we were recently reset, and have made enough initial | |
473 | // readings, | |
474 | // then go compute and set our new bias (correction) values | |
475 | // for each accumulator | |
476 | resetCnt--; | |
477 | if (resetCnt == 0) { | |
478 | m_x.setBiasByAccumulatedValues(); | |
479 | m_y.setBiasByAccumulatedValues(); | |
480 | m_z.setBiasByAccumulatedValues(); | |
481 | } | |
482 | } | |
483 | } | |
484 | ||
485 | // Short delay between each reading | |
486 | if (m_run_thread) { | |
487 | Timer.delay(.01); | |
488 | } | |
489 | } | |
490 | } | |
491 | ||
492 | /** | |
493 | * Singles the gyro's background thread that we want to reset the gyro. | |
494 | * | |
495 | * <p> | |
496 | * You don't typically need to call this during a match. If you do call it, | |
497 | * you should only do so when the robot is stationary and will remain | |
498 | * stationary for a short time. | |
499 | * </p> | |
500 | */ | |
501 | public void reset() { | |
502 | m_need_reset = true; | |
503 | } | |
504 | ||
505 | /** | |
506 | * Starts up the background thread that accumulates gyro statistics. | |
507 | * | |
508 | * <p> | |
509 | * You never need to call this method unless you have stopped the gyro and now | |
510 | * want to start it up again. If you do call this method, you should probably | |
511 | * also call the {@link #reset} method. | |
512 | * </p> | |
513 | */ | |
514 | public void start() { | |
515 | if (!m_thread.isAlive()) { | |
516 | m_thread.start(); | |
517 | } | |
518 | } | |
519 | ||
520 | /** | |
521 | * Stops the background thread from accumulating angle information (turns OFF | |
522 | * gyro!). | |
523 | * | |
524 | * <p> | |
525 | * This method is not typically called as it stops the gyro from accumulating | |
526 | * statistics essentially turning it off. The only time you might want to do | |
527 | * this is if you are done using the gyro for the rest of the match and want | |
528 | * to save some CPU cyles (for example, if you only needed the gyro during the | |
529 | * autonomous period). | |
530 | * </p> | |
531 | */ | |
532 | public void stop() { | |
533 | m_run_thread = false; | |
534 | } | |
535 | ||
536 | /** | |
537 | * Sends commands to configure the ITG-3200 the way we need it to run. | |
538 | */ | |
539 | private void performResetSequence() { | |
540 | // Configure the gyroscope | |
541 | // Set the gyroscope scale for the outputs to +/-2000 degrees per second | |
542 | m_i2c.write(DLPF_FS, (DLPF_FS_SEL_2000 | DLPF_LOWPASS_188HZ)); | |
543 | // Set the sample rate to 100 hz | |
544 | m_i2c.write(SMPLRT_DIV, 9); | |
545 | } | |
546 | ||
547 | /** | |
548 | * Enables the buffering of the next "n" data samples (which can then be saved | |
549 | * for analysis). | |
550 | * | |
551 | * @param samples | |
552 | * Maximum number of samples to read. | |
553 | */ | |
554 | public void enableBuffer(int samples) { | |
555 | double[] timeBuffer = new double[samples]; | |
556 | int[] xBuffer = new int[samples]; | |
557 | int[] yBuffer = new int[samples]; | |
558 | int[] zBuffer = new int[samples]; | |
559 | synchronized (this) { | |
560 | m_timeBuffer = timeBuffer; | |
561 | m_xBuffer = xBuffer; | |
562 | m_yBuffer = yBuffer; | |
563 | m_zBuffer = zBuffer; | |
564 | m_cntBuffer = 0; | |
565 | m_sizeBuffer = samples; | |
566 | } | |
567 | } | |
568 | ||
569 | /** | |
570 | * Check to see if the buffer is full. | |
571 | * | |
572 | * @return true if buffer is at capacity. | |
573 | */ | |
574 | public boolean isBufferFull() { | |
575 | boolean isFull; | |
576 | synchronized (this) { | |
577 | isFull = (m_cntBuffer == m_sizeBuffer); | |
578 | } | |
579 | return isFull; | |
580 | } | |
581 | ||
582 | /** | |
583 | * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for | |
584 | * inspection via Excel. | |
585 | */ | |
586 | public void saveBuffer() { | |
587 | double[] timeBuffer; | |
588 | int[] xBuffer; | |
589 | int[] yBuffer; | |
590 | int[] zBuffer; | |
591 | int size; | |
592 | ||
593 | // Transfer buffer info to local variables and turn off buffering in a | |
594 | // thread safe way. | |
595 | synchronized (this) { | |
596 | timeBuffer = m_timeBuffer; | |
597 | xBuffer = m_xBuffer; | |
598 | yBuffer = m_yBuffer; | |
599 | zBuffer = m_zBuffer; | |
600 | size = m_cntBuffer; | |
601 | m_sizeBuffer = 0; | |
602 | m_cntBuffer = 0; | |
603 | } | |
604 | ||
605 | if (size > 0) { | |
606 | try { | |
607 | PrintStream out = new PrintStream(new File("/tmp/gryo-data.csv")); | |
608 | out.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\""); | |
609 | for (int i = 0; i < size; i++) { | |
610 | out.println(timeBuffer[i] + "," + xBuffer[i] + "," + yBuffer[i] + "," | |
611 | + zBuffer[i]); | |
612 | } | |
613 | out.close(); | |
614 | SmartDashboard.putBoolean("Gyro Save OK", true); | |
615 | } catch (IOException ignore) { | |
616 | SmartDashboard.putBoolean("Gyro Save OK", false); | |
617 | } | |
618 | } | |
619 | } | |
620 | ||
621 | /** | |
622 | * Internal method run in the background thread that reads values from the | |
623 | * ITG-3200 and updates the accumulators. | |
624 | */ | |
625 | private void readRawAngleBytes() { | |
626 | double now = Timer.getFPGATimestamp(); | |
627 | ||
628 | byte[] buffer = new byte[6]; | |
629 | boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer); | |
630 | ||
631 | if (rc) { | |
632 | // Got a good read, get 16 bit integer values for each axis and | |
633 | // update accumulated values | |
634 | int x = (buffer[0] << 8) | (buffer[1] & 0xff); | |
635 | int y = (buffer[2] << 8) | (buffer[3] & 0xff); | |
636 | int z = (buffer[4] << 8) | (buffer[5] & 0xff); | |
637 | ||
638 | m_x.update(x, now); | |
639 | m_y.update(y, now); | |
640 | m_z.update(z, now); | |
641 | ||
642 | // If buffered enabled, then save values in a thread safe way | |
643 | if (m_sizeBuffer > 0) { | |
644 | synchronized (this) { | |
645 | int i = m_cntBuffer; | |
646 | if (i < m_sizeBuffer) { | |
647 | m_timeBuffer[i] = now; | |
648 | m_xBuffer[i] = x; | |
649 | m_yBuffer[i] = y; | |
650 | m_zBuffer[i] = z; | |
651 | m_cntBuffer++; | |
652 | } | |
653 | } | |
654 | } | |
655 | } | |
656 | ||
657 | if (DEBUG) { | |
658 | String name = toString(); | |
659 | String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H", | |
660 | "ZOUT_L" }; | |
661 | for (int i = 0; i < labels.length; i++) { | |
662 | SmartDashboard.putString(name + " " + labels[i], | |
663 | "0x" + Integer.toHexString(0xff & buffer[i])); | |
664 | } | |
665 | } | |
666 | } | |
667 | ||
668 | /** | |
669 | * Helper method to check that we can communicate with the gyro. | |
670 | */ | |
671 | private void check() { | |
672 | byte[] buffer = new byte[1]; | |
673 | boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer); | |
674 | if (DEBUG) { | |
675 | String name = toString(); | |
676 | SmartDashboard.putBoolean(name + " Check OK?", rc); | |
677 | SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]); | |
678 | } | |
679 | } | |
680 | ||
681 | /** | |
682 | * Private helper class to accumulate values read from the gryo and convert | |
683 | * degs/sec into degrees. | |
684 | */ | |
685 | private class Accumulator { | |
686 | /** Accumulated degrees since zero. */ | |
687 | private double m_accumulatedDegs; | |
688 | /** | |
689 | * 2 times the computed bias value that is used when getting average of | |
690 | * readings. | |
691 | */ | |
692 | private double m_bias2; | |
693 | /** The prior raw value read from the gyro. */ | |
694 | private int m_lastRaw; | |
695 | /** The prior time stamp the last raw value was read. */ | |
696 | private double m_lastTime; | |
697 | /** The total count of time the gyro value has been read. */ | |
698 | private int m_cnt; | |
699 | /** The sum of all of the raw values read. */ | |
700 | private long m_sum; | |
701 | ||
702 | /** Multipler to covert 2*Count to degrees/sec (optimization). */ | |
703 | private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0); | |
704 | ||
705 | /** | |
706 | * Returns the accumulated degrees. | |
707 | * | |
708 | * @return Accumulated signed degrees since last zeroed. | |
709 | */ | |
710 | public synchronized double getDegrees() { | |
711 | return m_accumulatedDegs; | |
712 | } | |
713 | ||
714 | /** | |
715 | * @return The raw integer reading from the ITG-3200 associated with the | |
716 | * axis. | |
717 | */ | |
718 | public int getRaw() { | |
719 | return m_lastRaw; | |
720 | } | |
721 | ||
722 | /** | |
723 | * Returns the number or readings that went into the accumulated degrees. | |
724 | * | |
725 | * @return Count of readings since last zeroed. | |
726 | */ | |
727 | public synchronized int getReadings() { | |
728 | return m_cnt; | |
729 | } | |
730 | ||
731 | /** | |
732 | * Constructs a new instance. | |
733 | */ | |
734 | private Accumulator() { | |
735 | m_bias2 = 0; | |
736 | zero(); | |
737 | } | |
738 | ||
739 | /** | |
740 | * Zeros out accumulated information. | |
741 | */ | |
742 | private void zero() { | |
743 | m_lastRaw = 0; | |
744 | m_lastTime = 0; | |
745 | m_sum = 0; | |
746 | synchronized (this) { | |
747 | m_cnt = 0; | |
748 | m_accumulatedDegs = 0; | |
749 | } | |
750 | } | |
751 | ||
752 | /** | |
753 | * Zeros out accumulated information and clears (zeros) the internal bias | |
754 | * value. | |
755 | */ | |
756 | private void reset() { | |
757 | zero(); | |
758 | m_bias2 = 0; | |
759 | } | |
760 | ||
761 | /** | |
762 | * Computes new bias value from accumulated values and then zeros. | |
763 | */ | |
764 | private void setBiasByAccumulatedValues() { | |
765 | m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt); | |
766 | zero(); | |
767 | } | |
768 | ||
769 | /** | |
770 | * Updates (accumulates) new value read from axis. | |
771 | * | |
772 | * @param raw | |
773 | * Raw signed 16 bit value read from gyro for axis. | |
774 | * @param time | |
775 | * The time stamp when the value was read. | |
776 | */ | |
777 | private void update(int raw, double time) { | |
778 | double degs = 0; | |
779 | ||
780 | if (m_cnt != 0) { | |
781 | // Get average of degrees per second over the time span | |
782 | double degPerSec = (m_lastRaw + raw - m_bias2) * COUNT2_TO_DEGSEC; | |
783 | // Get time span this rate occurred for | |
784 | double secs = (m_lastTime - time); | |
785 | // Get number of degrees rotated for time period | |
786 | degs = degPerSec * secs; | |
787 | } | |
788 | ||
789 | // Update our thread shared values | |
790 | synchronized (this) { | |
791 | m_accumulatedDegs += degs; | |
792 | m_sum += raw; | |
793 | m_cnt++; | |
794 | m_lastRaw = raw; | |
795 | m_lastTime = time; | |
796 | } | |
797 | } | |
798 | } | |
799 | } |