TPixy.h

00001 //
00002 // begin license header
00003 //
00004 // This file is part of Pixy CMUcam5 or "Pixy" for short
00005 //
00006 // All Pixy source code is provided under the terms of the
00007 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
00008 // Those wishing to use Pixy source code, software and/or
00009 // technologies under different licensing terms should contact us at
00010 // cmucam@cs.cmu.edu. Such licensing terms are available for
00011 // all portions of the Pixy codebase presented here.
00012 //
00013 // end license header
00014 //
00015 // This file is for defining the Block struct and the Pixy template class.
00016 // (TPixy).  TPixy takes a communication link as a template parameter so that
00017 // all communication modes (SPI, I2C and UART) can share the same code.
00018 //
00019 
00020 #ifndef _TPIXY_H
00021 #define _TPIXY_H
00022 
00023 #include "mbed.h"
00024 #include "TPixyInterface.h"
00025 
00026 // Communication/misc parameters
00027 #define PIXY_INITIAL_ARRAYSIZE      30
00028 #define PIXY_MAXIMUM_ARRAYSIZE      130
00029 #define PIXY_START_WORD             0xaa55
00030 #define PIXY_START_WORD_CC          0xaa56
00031 #define PIXY_START_WORDX            0x55aa
00032 #define PIXY_MAX_SIGNATURE          7
00033 #define PIXY_DEFAULT_ARGVAL         0xffff
00034 
00035 // Pixy x-y position values
00036 #define PIXY_MIN_X                  0L
00037 #define PIXY_MAX_X                  319L
00038 #define PIXY_MIN_Y                  0L
00039 #define PIXY_MAX_Y                  199L
00040 
00041 // RC-servo values
00042 #define PIXY_RCS_MIN_POS            0L
00043 #define PIXY_RCS_MAX_POS            1000L
00044 #define PIXY_RCS_CENTER_POS         ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2)
00045 
00046 enum BlockType {
00047     NORMAL_BLOCK,
00048     CC_BLOCK
00049 };
00050 
00051 /** A structure for containing Block data
00052   * signature the signature of the block
00053   * x the x position of the block on the screen
00054   * y the y position of the block on the screen
00055   * width the width of the block on the screen
00056   * height the height of the block on the screen
00057   * angle the angle of the block
00058   */
00059 struct Block {
00060     uint16_t signature;
00061     uint16_t x;
00062     uint16_t y;
00063     uint16_t width;
00064     uint16_t height;
00065     uint16_t angle;
00066 };
00067 
00068 /** The superclass handler for the Pixy camera from a given connection handler (SPI, I2C, etc)
00069   * Used for instantiating a Pixy object that interfaces via an abtritrary connection handler
00070   */
00071 template <class TPixyInterface> class TPixy
00072 {
00073 public:
00074     Serial* serial;
00075     Block *blocks;
00076     
00077     /** Creates a TPixy object with a given connection handler
00078       * @param type the pointer to the interface connection handler
00079       * @param serialOut the optional serial output pointer to print out Pixy camera data
00080       * @param arg an optional integer argument used for custom implementations of the Pixy library
00081       */
00082     TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL);
00083     ~TPixy();
00084 
00085     /** Prints the given Block out to the given serial port
00086       * @param &block the Block to print
00087       */
00088     void printBlock(Block &block);
00089     /** Returns the number of Blocks found in the current view and updates all Blocks' info
00090       * @param maxBlocks the maximum number of Blocks to search for
00091       * @return the number of Blocks found
00092       */
00093     uint16_t getBlocks(uint16_t maxBlocks = 1000);
00094     /** Sets the PWM value of the Pixy servos
00095       * @param s0 the value of the left servo
00096       * @param s1 the value of the right servo
00097       * @return the interface return value for sending the servo command
00098       */
00099     int8_t setServos(uint16_t s0, uint16_t s1);
00100     /** Sets the brightness of the Pixy RGB LED
00101       * @param brightness the brightness of the LED
00102       * @return the interface return value for sending the brightness command
00103       */
00104     int8_t setBrightness(uint8_t brightness);
00105     /** Sets the color of the Pixy RGB LED
00106       * @param r the red color value
00107       * @param g the green color value
00108       * @param b the blue color value
00109       */
00110     int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
00111     /** Initializes the Pixy
00112       */
00113     void init();
00114 
00115 private:
00116     TPixyInterface* link;
00117     BlockType blockType;
00118     /** Returns if the Pixy is ready to receive/send a message
00119       */
00120     bool getStart();
00121     /** Resizes the block array to add an additional block
00122       */
00123     void resize();
00124     bool skipStart;
00125     uint16_t blockCount;
00126     uint16_t blockArraySize;
00127 };
00128 
00129 template <class TPixyInterface> void TPixy<TPixyInterface>::init()
00130 {
00131     link->init();
00132 }
00133 
00134 
00135 template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type)
00136 {
00137     skipStart = false;
00138     blockCount = 0;
00139     blockArraySize = PIXY_INITIAL_ARRAYSIZE;
00140     blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
00141     link->setArg(arg);
00142 }
00143 
00144 template <class TPixyInterface> void TPixy<TPixyInterface>::printBlock(Block &block)
00145 {
00146     int i, j;
00147     char sig[6], d;
00148     bool flag;
00149     if (block.signature > PIXY_MAX_SIGNATURE) { // color code! (CC)
00150         // convert signature number to an octal string
00151         for (i = 12, j = 0, flag = false; i >= 0; i -= 3) {
00152             d = (block.signature >> i) & 0x07;
00153             if (d > 0 && !flag) {
00154                 flag = true;
00155             }
00156             if (flag) {
00157                 sig[j++] = d + '0';
00158             }
00159         }
00160         sig[j] = '\0';
00161         if (serial != NULL) {
00162             serial->printf("CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d\n", sig, block.signature, block.x, block.y, block.width, block.height, block.angle);
00163         }
00164     } else {}// regular block.  Note, angle is always zero, so no need to print
00165     serial->printf("sig: %d x: %d y: %d width: %d height: %d\n", block.signature, block.x, block.y, block.width, block.height);
00166 }
00167 
00168 template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy()
00169 {
00170     free(blocks);
00171 }
00172 
00173 template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart()
00174 {
00175     uint16_t w, lastw;
00176     lastw = 0xffff;
00177     while(true) {
00178         w = link->getWord();
00179         if (w == 0 && lastw == 0) {
00180             wait_ms(10);
00181             return false;
00182         } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) {
00183             blockType = NORMAL_BLOCK;
00184             return true;
00185         } else if (w == PIXY_START_WORD_CC && lastw == PIXY_START_WORD) {
00186             blockType = CC_BLOCK;
00187             return true;
00188         } else if (w == PIXY_START_WORDX) {
00189             if (serial != NULL) {
00190                 serial->printf("reorder");
00191             }
00192             link->getByte(); // resync
00193         }
00194         lastw = w;
00195     }
00196 }
00197 
00198 template <class TPixyInterface> void TPixy<TPixyInterface>::resize()
00199 {
00200     blockArraySize += PIXY_INITIAL_ARRAYSIZE;
00201     blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
00202 }
00203 
00204 template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::getBlocks(uint16_t maxBlocks)
00205 {
00206     uint8_t i;
00207     uint16_t w, checksum, sum;
00208     Block *block;
00209 
00210     if (!skipStart) {
00211         if (getStart() == false) {
00212             return 0;
00213         }
00214     } else {
00215         skipStart = false;
00216     }
00217     for(blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
00218         checksum = link->getWord();
00219         if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
00220             skipStart = true;
00221             blockType = NORMAL_BLOCK;
00222             if (serial != NULL) {
00223                 serial->printf("skip");
00224             }
00225             return blockCount;
00226         } else if (checksum == PIXY_START_WORD_CC) {
00227             skipStart = true;
00228             blockType = CC_BLOCK;
00229             return blockCount;
00230         } else if (checksum == 0) {
00231             return blockCount;
00232         }
00233         if (blockCount > blockArraySize) {
00234             resize();
00235         }
00236         block = blocks + blockCount;
00237 
00238         for (i = 0, sum = 0; i < sizeof(Block)/sizeof(uint16_t); i++) {
00239             if (blockType == NORMAL_BLOCK && i >= 5) { // skip
00240                 block->angle = 0;
00241                 break;
00242             }
00243             w = link->getWord();
00244             sum += w;
00245             *((uint16_t *)block + i) = w;
00246         }
00247 
00248         if (checksum == sum) {
00249             blockCount++;
00250         } else {
00251             w = link->getWord();
00252             if (serial != NULL) {
00253                 serial->printf("cs error");
00254             }
00255         }
00256         if (w == PIXY_START_WORD) {
00257             blockType = NORMAL_BLOCK;
00258         } else if (w == PIXY_START_WORD_CC) {
00259             blockType = CC_BLOCK;
00260         } else {
00261             return blockCount;
00262         }
00263     }
00264     return blockCount;
00265 }
00266 
00267 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setServos(uint16_t s0, uint16_t s1)
00268 {
00269     uint8_t outBuf[6];
00270     outBuf[0] = 0x00;
00271     outBuf[1] = 0xff;
00272     *(uint16_t *)(outBuf + 2) = s0;
00273     *(uint16_t *)(outBuf + 4) = s1;
00274     return link->send(outBuf, 6);
00275 }
00276 
00277 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setBrightness(uint8_t brightness)
00278 {
00279     uint8_t outBuf[3];
00280     outBuf[0] = 0x00;
00281     outBuf[1] = 0xfe;
00282     outBuf[2] = brightness;
00283     return link->send(outBuf, 3);
00284 }
00285 
00286 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setLED(uint8_t r, uint8_t g, uint8_t b)
00287 {
00288     uint8_t outBuf[5];
00289     outBuf[0] = 0x00;
00290     outBuf[1] = 0xfd;
00291     outBuf[2] = r;
00292     outBuf[3] = g;
00293     outBuf[4] = b;
00294     return link->send(outBuf, 5);
00295 }
00296 
00297 #endif