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