Pixy.h

00001 #ifndef _PIXY_H
00002 #define _PIXY_H
00003 
00004 #include "TPixy.h"
00005 #include "TPixyInterface.h"
00006 
00007 #define X_CENTER        ((PIXY_MAX_X-PIXY_MIN_X)/2)
00008 #define Y_CENTER        ((PIXY_MAX_Y-PIXY_MIN_Y)/2)
00009 
00010 /** The SPI Pixy interface for the Pixy camera
00011   * @code
00012   * #include "Pixy.h"
00013   * Serial serial(USBTX, USBRX);
00014   * SPI spi(p5, p6, p7);
00015   * PixySPI pixy(&spi, &serial);
00016   *
00017   * int main() {
00018   *     runPanTiltDemo();
00019   * }
00020   * @endcode
00021   */
00022   
00023 class PixySPI : public TPixy<PixyInterfaceSPI>
00024 {
00025 public:
00026     /** Constructor for the PixySPI class
00027       * Instantiates a Pixy object that communicates via an SPI connection
00028       * @param SPI* spi the pointer to the SPI connection
00029       * @param Serial* serialOut the optional serial output pointer to print out Pixy camera data
00030       * @param int arg an optional integer argument used for custom implementations of the Pixy library
00031       */
00032     PixySPI(SPI* spi, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL) :
00033         TPixy<PixyInterfaceSPI>((new PixyInterfaceSPI(spi)), serialOut, arg) {}
00034 };
00035 
00036 
00037 /* Not Implemented */
00038 /*
00039 class PixyI2C : TPixy<PixyInterfaceI2C> {
00040     public:
00041         PixyI2C(I2C* i2c, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL) : i2cInterface(i2c), TPixy<PixyInterfaceI2C>(i2cInterface, serialOut, arg);
00042     private:
00043         TPixyInterface<I2C>* i2cInterface;
00044     };
00045     */
00046     
00047 class ServoLoop
00048 {
00049 public:
00050     /** Constructor for a ServoLoop object
00051       * Creates a ServoLoop object for easy control of the Pixy servos
00052       * @param pgain the proportional gain for the control
00053       * @param dgain the derivative gain for the control
00054       */
00055     ServoLoop(int32_t pgain, int32_t dgain);
00056     /** Update method for a ServoLoop object
00057       * Updates the m_pos variable for setting a Pixy servo
00058       * @param error the error between the center of the camera and the position of the tracking color
00059       */
00060     void update(int32_t error);
00061 
00062     int32_t m_pos;
00063     int32_t m_prevError;
00064     int32_t m_pgain;
00065     int32_t m_dgain;
00066 };
00067 
00068 /** Basic Pan/Tilt Demo code
00069   * Runs the Pan/Tilt demo code
00070   * @param pixy the pointer to the pixy object to run the demo on
00071   * @param serial the optional serial pointer to display output to
00072   */
00073 template <class TPixyInterface> void runPanTiltDemo(TPixy<TPixyInterface>* pixy, Serial* serial = NULL)
00074 {
00075     ServoLoop panLoop(-150, -300);
00076     ServoLoop tiltLoop(200, 250);
00077     if (serial != NULL) {
00078         serial->printf("Starting...\n");
00079     }   
00080     static int i = 0;
00081     int j;
00082     uint16_t blocks;
00083     int32_t panError, tiltError;
00084     pixy->init();
00085     while(true) {
00086         blocks = pixy->getBlocks();
00087         if (blocks) {
00088             panError = X_CENTER - pixy->blocks[0].x;
00089             tiltError = pixy->blocks[0].y - Y_CENTER;
00090 
00091             panLoop.update(panError);
00092             tiltLoop.update(tiltError);
00093 
00094             pixy->setServos(panLoop.m_pos, tiltLoop.m_pos);
00095             i++;
00096 
00097             if (i % 50 == 0 && serial != NULL) {
00098                 serial->printf("Detected %d:\n", blocks);
00099                 //toPC.printf(buf);
00100                 for (j = 0; j < blocks; j++) {
00101                     serial->printf("  block %d: ", j);
00102                     pixy->printBlock(pixy->blocks[j]);
00103                 }
00104             }
00105         }
00106     }
00107 }
00108 #endif