1. Ezra Reese
  2. bagelbots / frc702 / 2016

Source

bagelbots / frc702 / 2016 / 2016 FIRST Stronghold Robot Code / src / org / usfirst / frc / team702 / robot / PixyCmu5.java

  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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
package org.usfirst.frc.team702.robot;

import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
import java.util.TimerTask;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.PIDInterface;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Timer;

/**
 * Class implements the interface to the Pixy Camera via I2C.
 *
 * Creates and manages the interface to the Pixy Camera. Able to create a separate thread to process frames 
 * periodically and return the frame data for any detected objects.
 */
public class PixyCmu5 implements PIDSource
{
	
	/**
     * The frame data that is returned by the Pixy camera over the I2C interface
     */
    public class PixyFrame
    {
        int sync = 0;
        int checksum = 0;
        public double timestamp = 0;
        public int signature;
        public int xCenter;
        public int yCenter;
        public int width;
        public int height;
        public int angle;
        public int area;
    }
    
	/**********************************************************
	 *  Pixy CMU Communication Parameters
	 **********************************************************/
	public static final int PIXY_ARRAYSIZE               = 130;
	public static final int PIXY_START_WORD              = 0xaa55;
	public static final int PIXY_START_WORD_CC           = 0xaa56;
	public static final int PIXY_START_WORDX             = 0x55aa;
	public static final int PIXY_SERVO_SYNC              = 0xff;
	public static final int PIXY_CAM_BRIGHTNESS_SYNC     = 0xfe;
	public static final int PIXY_LED_SYNC                = 0xfd;
	public static final int PIXY_OUTBUF_SIZE             = 64;
	public static final int PIXY_SYNC_BYTE               = 0x5a;
	public static final int PIXY_SYNC_BYTE_DATA          = 0x5b;
	
	/**********************************************************
	 * Define Constants for the image size
	 *********************************************************/
	public static final int PIXY_MIN_X = 0;
	public static final int PIXY_MAX_X = 319;
	public static final double PIXY_X_CENTER = (PIXY_MIN_X + PIXY_MAX_X)/2;
	public static final int PIXY_MIN_Y = 0;
	public static final int PIXY_MAX_Y = 199;
	public static final double PIXY_Y_CENTER = (PIXY_MIN_Y + PIXY_MAX_Y)/2;
	
	/**********************************************************
	 * Stock field of view is 75 degrees horizontal and 47 degrees vertical
	 *********************************************************/
	public static final double PIXY_X_FOV = 75.0;
	public static final double PIXY_Y_FOV = 47.0;
	public static final double PIXY_X_DEG_PER_PIXEL = PIXY_X_FOV/(PIXY_MAX_X+1);
	public static final double PIXY_Y_DEG_PER_PIXEL = PIXY_Y_FOV/(PIXY_MAX_Y+1);

	
	/**********************************************************
	 *  Pixy CMU I2C Interface
	 **********************************************************/
    private static final int DATA_SIZE = 130;
    
    /**********************************************************
	 *  Local Variables
	 **********************************************************/
    private List<PixyFrame> m_currentframes;
    private double m_lastupdate = 0.0;
    private byte m_zeroBuffer[];
    private double m_maxDataAge = 1.5; // second
    private boolean m_isreading = false;

	private double m_centerDelta = 10.0; // Pixels
    
	/**********************************************************
	 *  Threading
	 **********************************************************/
    java.util.Timer m_scheduler;    // The timer object to manage the thread
    private double m_period = 0.1; // 1/4 of a second seems to be a stable update rate
    
    /**********************************************************
	 *  PID Interface Variables
	 **********************************************************/
    PIDSourceType m_pid_source_type = PIDSourceType.kDisplacement;
    
    /**********************************************************
	 *  I2C
	 **********************************************************/
    public static final int PIXY_I2C_DEFAULT_ADDR = 0x54; //0xa8;
    private I2C m_i2cbus;
    private int m_i2caddress = PIXY_I2C_DEFAULT_ADDR;
    
    /**********************************************************
	 *  Debug variables
	 **********************************************************/
    private static boolean flg_debug = false;
    
	/**
	 * PixyTask is the private scheduler within PixyCMU5 that 
	 * automatically performs I2C reads to get the frame data
	 * from the connected Pixy device.
	 */
	private class PixyTask extends TimerTask {

        private PixyCmu5 m_pixy;

        public PixyTask(PixyCmu5 pixy) {
          if (pixy == null) {
            throw new NullPointerException("Pixy Instance was null");
          }
          this.m_pixy = pixy;
        }

        @Override
        public void run() {
        	m_pixy.getFrames();
        }
      }
    
	/**************************************************************
	 * 
     * CONSTRUCTORS--CONSTRUCTORS--CONSTRUCTORS--CONSTRUCTORS
     * 
     **************************************************************/
	
	/**
     * The constructor for the PixyCMU5 class. Initializes the I2C bus at the default address
     * $
     */
    public PixyCmu5()
    {
    	m_i2cbus = new I2C(I2C.Port.kOnboard, getI2CAddress());
    	m_currentframes = new LinkedList<PixyFrame>();
    	m_zeroBuffer = new byte [DATA_SIZE];
    	Arrays.fill(m_zeroBuffer, (byte)0);    	
    }
    
	/**
     * The constructors for the PixyCMU5 class. Initializes the I2C bus at the specified address
     * $
     * @param i2c_address_in - The I2C address to connect to
     * @param period - The period in seconds to perform I2C reads
     */
    public PixyCmu5(int i2c_address_in)
    {
    	setI2CAddress(i2c_address_in);
    	m_i2cbus = new I2C(I2C.Port.kOnboard, getI2CAddress());
    	m_currentframes = new LinkedList<PixyFrame>();
    	m_zeroBuffer = new byte [DATA_SIZE];
    	Arrays.fill(m_zeroBuffer, (byte)0);
    }
    
    /**
     * The constructors for the class. Initialize any data and the bus
     * $
     * @param i2c_address_in - The I2C address to connect to
     * @param period - The period in seconds to perform I2C reads
     */
    public PixyCmu5(int i2c_address_in, double period)
    {
    	// Set I2C address and period
    	this.setI2CAddress(i2c_address_in);
    	this.setPeriod(period);
    	m_i2cbus = new I2C(I2C.Port.kOnboard, getI2CAddress());
    	m_currentframes = new LinkedList<PixyFrame>();
    	m_zeroBuffer = new byte [DATA_SIZE];
    	Arrays.fill(m_zeroBuffer, (byte)0);
    	
    	// Call the start method to schedule reading the data periodically
    	this.start(getPeriod());
    }
    
    
    /**
     * Start a thread to read data from the Pixy periodically
     * 
     * @param period - period in seconds to schedule update
     */
    public void start(double period)
    {
   	
    	// Schedule the Pixy task to execute every <period> seconds
    	if(m_scheduler == null && period > 0)
    	{
    		this.setPeriod(period);
    		System.out.println("Attempting to enable Pixy at a " + Double.toString(period) + " second rate.");
    		m_scheduler = new java.util.Timer();
    		m_scheduler.schedule(new PixyTask(this), 0L, (long) (this.getPeriod() * 1000));
    	} else {
    		System.out.println("Pixy Thread already scheduled. Stop before starting a new thread.");
    	}
    }
    
    
    /**
     * Cancel a running timer and attempt to null the variable
     */
    public void stop()
    {
    	// If the timer object is not null, cancel the scheduler
    	if(m_scheduler != null)
    	{
    		System.out.println("Attempting to disable Pixy auto polling.");
    		m_scheduler.cancel();
    		
    		// Set the timer to NULL to allow it to be reallocated if necessary
    		synchronized(this) {
    			m_scheduler = null;
    		}
    		
    	} else {
    		// nothing to do
    	}
    }
    
    
    /**
     * Performs an I2C read at the specified address and decodes any data received.
     * $
     * @return linked list of PixyFrame objects for any detected objects
     */
    public List<PixyFrame> getFrames()
    {
    	// Initialize the local linked list for the results
    	List<PixyFrame> frames = new LinkedList<PixyFrame>();
    	
    	// Read lock code
    	if(this.getIsReading())
    	{
    		return frames;
    	}
    	this.setIsReading(true);
    	
    	// readBuffer is the raw data output from the i2c bus
    	byte readBuffer[] = new byte [DATA_SIZE];

    	// Read data from the i2c bus 
    	m_i2cbus.readOnly(readBuffer, DATA_SIZE);

    	if(Arrays.equals(m_zeroBuffer, readBuffer))
    	{
    		if(flg_debug)
    		{
    			System.out.println("Pixy - Read Failed! All elements returned were 0!");
    		}

    		// Add synchronization block to assign frame output to class
        	synchronized (this)
        	{
        		if(m_currentframes != null)
        		{	
        			m_lastupdate = Timer.getFPGATimestamp();
    	    		m_currentframes.clear();
        		}
        	}
        	this.setIsReading(false);
    		return frames;
    	}
    	
    	if(flg_debug)
    	{
    		// Allocate debug string
    		String readBuffer_char = "";
        	
    		// Build the string for debug printouts        	
    		for(int idx = 0; idx < readBuffer.length; idx++)
    		{
    			readBuffer_char += Integer.toString(readBuffer[idx] & 0xFF) + " ";
    		}
    		// Print the raw buffer to the console
    		System.out.println(readBuffer_char);
    	}
    	
    	/* 
    	 * Move through the array and look for the pattern that indicates the start of a frame.
    	 * Frame data is encoded as follows:
    	 * 
         * Bytes    16-bit word    Description
	     * ----------------------------------------------------------------
	     * 0, 1     y              sync: 0xaa55=normal object, 0xaa56=color code object
	     * 2, 3     y              checksum (sum of all 16-bit words 2-6)
	     * 4, 5     y              signature number
	     * 6, 7     y              x center of object
	     * 8, 9     y              y center of object
	     * 10, 11   y              width of object
	     * 12, 13   y              height of object
    	 */
    	for(int idx = 0; idx < readBuffer.length - 18; idx++ ) 
    	{
    		/*
    		 * The Pixy sends the data in Little Endian format [https://en.wikipedia.org/wiki/Endianness]
	    	 *  with the data packed in 16 bit integers (4 hex digits each) [0xFF]. Little Endian format 
	    	 *  means that the bytes (one hex digit) are sent with the least-significant byte first, so they
	    	 *  need to be reversed before combining them into a 16 bit word.
    		 * */
    		
    		// Two Pixy start words 0xAA55 indicate that 
    		if( !( (((readBuffer[idx+1] << 8) | readBuffer[idx] ) & 0xFFFF) == PIXY_START_WORD  ))
    		{
    			// If this word wasn't found, cancel executing the rest of the loop iteration and move to the next byte
    			continue; 
    		}
    		
    		if( !( (((readBuffer[idx+3] << 8) | readBuffer[idx+2] ) & 0xFFFF)  == PIXY_START_WORD ))
    		{
    			// If this word wasn't found, cancel executing the rest of the loop iteration and move to the next byte
    			continue;
    		}
    		
    		/* If we make it this far, we found two instances of 0xaa55 back-to-back which means that the next 14 bytes
    		 * make up the data for the frame. Create a new instance of the Frame class (which acts as a structure) and start 
    		 * pulling out byte pairs and switching them before packing them into integers. Since some of the data 
    		 * on the i2c bus is encoded as unsigned 16 bit integers and Java doesn't have unsigned types, we need to manually
    		 * re-encode the data into 32 bit integers which can correctly represent the value.
    		 * [https://en.wikipedia.org/wiki/Integer_(computer_science)]
    		 * 
    		 * To do this we treat the 16 bit unsigned value as a 32 bit signed value (how Java stores integers)
    		 * so the real-world value that we can work with represents the correct data.
    		 */
        	PixyFrame tempFrame = new PixyFrame();
    		
    		int startIdx = idx+4;
    		tempFrame.checksum = convertBytesToInt(readBuffer[startIdx+1], readBuffer[startIdx]); startIdx+=2;
    		tempFrame.signature = convertBytesToInt(readBuffer[startIdx+1], readBuffer[startIdx]); startIdx+=2;
    		tempFrame.xCenter = convertBytesToInt(readBuffer[startIdx+1], readBuffer[startIdx]); startIdx+=2;
    		tempFrame.yCenter = convertBytesToInt(readBuffer[startIdx+1], readBuffer[startIdx]); startIdx+=2;
    		tempFrame.width = convertBytesToInt(readBuffer[startIdx+1], readBuffer[startIdx]); startIdx+=2;
    		tempFrame.height = convertBytesToInt(readBuffer[startIdx+1], readBuffer[startIdx]); startIdx+=2;
    		tempFrame.area = tempFrame.height * tempFrame.width;
    		tempFrame.timestamp = m_lastupdate;
    		idx = startIdx;
    		
    		// Concatenate the data in Frame into a string and print to the console
    		if(flg_debug)
    		{
	    		System.out.println("Checksum: "+ Integer.toString(tempFrame.checksum) + 
	    				" Signature: "+ Integer.toString(tempFrame.signature) +
	    				" xCenter: "+ Integer.toString(tempFrame.xCenter) + 
	    				" yCenter: "+ Integer.toString(tempFrame.yCenter) +
	    				" width: "+ Integer.toString(tempFrame.width) +
	    				" height: "+ Integer.toString(tempFrame.height));
    		}
    		
    		// Append the constructed frame to the Linked List which will be returned to the caller
    		frames.add(tempFrame);
    	}
    	
    	// Add synchronization block to assign frame output to class
    	synchronized (this)
    	{
    		if(m_currentframes != null)
    		{	
    			m_lastupdate = Timer.getFPGATimestamp();
	    		m_currentframes.clear();
	    		m_currentframes = frames;
    		}
    	}

    	this.setIsReading(false);
    	return frames;
    }
    
    
    /**************************************************************
     * 
     * HELPER METHODS--HELPER METHODS--HELPER METHODS
     * 
     **************************************************************/
    
    
    
    /**
     * Returns true if a signature is detected and data is still fresh
     * 
     * @return true if object is detected, false if object is not detected
     */
    public boolean isObjectDetected()
    {
    	// If the data is fresh and objects have been detected
    	if(getDataAge() < m_maxDataAge && !getCurrentframes().isEmpty())
    	{
    		return true;
    	}
    	
    	return false;
    }
    
    /**
     * Calculates if the object is in the center of the frame with
     * a margin of error specified
     * $
     * @param X - X location in pixels
     * @return true if centered
     */
    public boolean isInXCenter(double X)
    {
    	// See if the X center is in the middle of the frame
    	if(Math.abs(X - PIXY_X_CENTER) < m_centerDelta)
    	{
    		return true;
    	}
    	
    	return false;
    }
    
    /**
     * Calculates if the object is in the center of the frame with
     * a margin of error specified
     * $
     * @param X - X location in pixels
     * @return true if centered
     */
    public boolean isInXCenter(PixyFrame frame)
    {
    	// See if the X center is in the middle of the frame
    	if(Math.abs(frame.xCenter - PIXY_X_CENTER) < m_centerDelta)
    	{
    		return true;
    	}
    	
    	return false;
    }
    
    /**
     * Calculates if the object is in the center of the frame with
     * a margin of error specified
     * $
     * @param X - X location in pixels
     * @return true if centered
     */
    public boolean isInYCenter(double Y)
    {
    	// See if the Y center is in the middle of the frame
    	if(Math.abs(Y - PIXY_Y_CENTER) < m_centerDelta)
    	{
    		return true;
    	}
    	
    	return false;
    }
    
    /**
     * Calculates if the object is in the center of the frame with
     * a margin of error specified
     * $
     * @param X - X location in pixels
     * @return true if centered
     */
    public boolean isInYCenter(PixyFrame frame)
    {
    	// See if the Y center is in the middle of the frame
    	if(Math.abs(frame.yCenter - PIXY_Y_CENTER) < m_centerDelta)
    	{
    		return true;
    	}
    	
    	return false;
    }
    
    
    /**
     * Checks to see if the most recent data is still valid or if the time has expired
     * $
     * @return true if valid, false if invalid
     */
    public boolean isDataFresh()
    {
    	return this.getDataAge() <= this.getMaxDataAge();
    }
    
    /**
     * Returns true if a signature is detected and ANY of them are in the center of the frame
     * 
     * @return
     */
    public boolean isDetectedAndCentered()
    {
    	// If the data is fresh and objects have been detected
    	if(getDataAge() < m_maxDataAge && !getCurrentframes().isEmpty())
    	{
    		for(PixyFrame idx : getCurrentframes())
    		{
    			if(isInXCenter(idx.xCenter))
    			{
    				return true;
    			}
    		}	
    	}
    	return false;
    }
    
    /**
     * Sets the brightness value of the Pixy camera
     * $
     * @param brightness - 0-255
     * @return false if success, true if abort
     */
    public boolean setBrightness(byte brightness)
    {
    	byte[] outBuf = new byte[3];
    	
    	outBuf[0] = 0x00;
    	outBuf[1] = (byte)PIXY_CAM_BRIGHTNESS_SYNC;
    	outBuf[2] = brightness;
    	
    	// Perform the I2C Transaction
    	return m_i2cbus.transaction(outBuf, 3, null, 0);
    }
    
    /**
     * Sets the RGB LED value of the Pixy camera
     * $
     * @param R
     * @param G
     * @param B
     * @return false if success, true if abort
     */
    public boolean setLED(byte R, byte G, byte B)
    {
    	byte[] outBuf = new byte[5];
    	
    	outBuf[0] = 0x00;
    	outBuf[1] = (byte)PIXY_LED_SYNC;
    	outBuf[2] = R;
    	outBuf[3] = G;
    	outBuf[4] = B;
    	
    	// Perform the I2C Transaction
    	return m_i2cbus.transaction(outBuf, 5, null, 0);
    }
    
    /**************************************************************
     * 
     * STATICS--STATICS--STATICS--STATICS--STATICS--STATICS
     * 
     **************************************************************/
    
    /**
     * Converts two unsigned bytes to a signed integer
     * $
     * @param msb - Most significant byte
     * @param lsb - Least significant byte
     * @return - Integer value
     */
    public static int convertBytesToInt(int msb, int lsb)
    {
        if (msb < 0)
            msb += 256;
        int value = msb * 256;

        if (lsb < 0)
        {
            // lsb should be unsigned
            value += 256;
        }
        value += lsb;
        return value;
    }
    
    
    /**
     * Returns the offset from the X center, negative indicates center is to the left, positive indicates it is to the right.
     * $
     * @param frame PixyFrame representing the detected object
     * @return number of pixels from center
     */
    public static double xCenterDelta(PixyFrame frame)
    {
    	return frame.xCenter - PixyCmu5.PIXY_X_CENTER;
    }
    
    /**
     * Returns the number of degrees along the horizontal axis the detected object is away from the center
     * $
     * @param frame PixyFrame representing the detected object
     * @return degrees along x axis from center of field of view
     */
    public static double degreesXFromCenter(PixyFrame frame)
    {
    	return xCenterDelta(frame)*PIXY_X_DEG_PER_PIXEL;
    }
    
    /**************************************************************
     * 
     * GETTERS/SETTERS--GETTERS/SETTERS--GETTERS/SETTERS
     * 
     **************************************************************/
    
    /**
     * @return currentFrames - Returns the thread synchronized list of current frames
     */
    public synchronized List<PixyFrame> getCurrentframes() {
		return m_currentframes;
	}
    
    /**
     * @return m_lastupdate - The FPGA timestamp of the last time the frame was processed.
     */
    public synchronized double getLastupdate()
    {
    	return m_lastupdate;
    }
    
    /**
     * The number of seconds since the last Pixy Frame was processed.
     * $
     * @return dataAge - The number of seconds between the current timestamp and the last time the Pixy Frame was processed
     */
    public synchronized double getDataAge()
    {
    	return Timer.getFPGATimestamp() - this.getLastupdate();
    }
    
    /**
     * Sets the frequency the read process will occur at.
     * $
     * @param period in seconds
     */
    public synchronized void setPeriod(double period)
    {
    	if(period < 0)
    	{
    		throw new IllegalArgumentException("Period must be a positive value");
    	}
    	
    	m_period = period;
    }
    
    /**
     * Gets the frequency the read process will occur at.
     * $
     * @return period in seconds
     */
    public synchronized double getPeriod()
    {
    	return this.m_period;
    }
    
    /**
     * Sets the i2c address of the Pixy to connect to
     * $
     * @param i2c_address
     */
    public synchronized void setI2CAddress(int i2c_address)
    {
    	if(i2c_address <= 0 || i2c_address > 255)
    	{
    		throw new IllegalArgumentException("Invalid I2C address for Pixy Camera");
    	}
    	
    	this.m_i2caddress = i2c_address;
    }
    
    /**
     * Gets the i2c address of the Pixy we are trying to communicate with
     * $
     * @return i2c address
     */
    public synchronized int getI2CAddress()
    {
    	return this.m_i2caddress;
    }
    
    
    /**
	 * Gets the maximum data age for a frame to consider it still valid
	 * $
	 * @return seconds a frame should be considered valid after it's been read
	 */
    public double getMaxDataAge() {
		return m_maxDataAge;
	}

    /**
	 * Sets the maximum data age for a frame to consider it still valid
	 * $
	 * @double maxDataAge - seconds a frame should be considered valid after it's been read
	 */
	public void setMaxDataAge(double maxDataAge) {
		this.m_maxDataAge = maxDataAge;
	}

	/**
	 * Gets the number of pixels an object can be away from the center for it to be considered centered
	 * $
	 * @return centerDelta
	 */
	public double getCenterDelta() {
		return m_centerDelta;
	}
	
	/**
	 * Sets the number of pixels an object can be away from the center for it to be considered centered
	 * $
	 * @param centerDelta
	 */
	public void setCenterDelta(double centerDelta) {
		this.m_centerDelta = centerDelta;
	}
	
	/**
	 * Sets the internal read flag to indicate an I2C transaction is still in process
	 * $
	 * @param centerDelta
	 */
	private synchronized void setIsReading(boolean reading)
	{
		this.m_isreading = reading;
	}
	
	/**
	 * Gets the internal read flag to indicate an I2C transaction is still in process
	 * $
	 * @param centerDelta
	 */
	public synchronized boolean getIsReading()
	{
		return this.m_isreading;
	}

	
	/**************************************************************
     * PID SOURCE INTERFACE
     **************************************************************/
    
    public PIDSourceType getPIDSourceType() {
    	return m_pid_source_type;
    }
    
    public void setPIDSourceType(PIDSourceType type) {
    	m_pid_source_type = type;
    }

	/* (non-Javadoc)
	 * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
	 * Implement the pidGet method
	 */
	@Override
	public double pidGet() {
		List<PixyFrame> pixyFrames;
		
		// Synchronize with the thread and latch the current frames
		pixyFrames = this.getCurrentframes();
		
		// If there are no frames processed or the data is too old
		if(pixyFrames.isEmpty() || !isDataFresh())
		{
			return 0;
		}
		
		return PixyCmu5.degreesXFromCenter(pixyFrames.get(0));
	}

	
    
}