TPixy2.h

00001 //--------------------------------------------------------------------------------------------
00002 //Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip 
00003 //
00004 //Modifications made by: Mathieu Malone
00005 //Modifications: Modified Arduino code to function with mbed development platform
00006 //Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication
00007 //               between the mbed platform and putty terminal through USB
00008 //
00009 //Latest update by: Mathieu Malone
00010 //Date of last update: July 24th, 2014
00011 //--------------------------------------------------------------------------------------------
00012 //
00013 // begin license header
00014 //
00015 // This file is part of Pixy CMUcam5 or "Pixy" for short
00016 //
00017 // All Pixy source code is provided under the terms of the
00018 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
00019 // Those wishing to use Pixy source code, software and/or
00020 // technologies under different licensing terms should contact us at
00021 // cmucam@cs.cmu.edu. Such licensing terms are available for
00022 // all portions of the Pixy codebase presented here.
00023 //
00024 // end license header
00025 //
00026 
00027 /*
00028   06.04.2014 v0.1.3 John Leimon 
00029     + Added init() for initializing Pixy, which should
00030       be called from the setup() function. See comment
00031       in Pixy.h for details.
00032 */
00033 
00034 #ifndef _TPIXY_H2
00035 #define _TPIXY_H2
00036 
00037 #define PIXY_INITIAL_ARRAYSIZE2      30
00038 #define PIXY_MAXIMUM_ARRAYSIZE2      130
00039 #define PIXY_START_WORD2             0xaa55
00040 #define PIXY_START_WORDX2            0x55aa
00041 #define PIXY_DEFAULT_ADDR2           0x54  // I2C
00042 
00043 Serial pc2(USBTX, USBRX);
00044 
00045 struct Block2 
00046 {
00047   void print2()
00048   {
00049     char buf2[64];
00050     sprintf(buf2, "sig: %d x2: %d y2: %d width2: %d height2: %d\n", signature2, x2, y2, width2, height2);
00051     printf(buf2);
00052     Xp2 = x2;
00053     Yp2= y2;
00054     sig2 = signature2;
00055   }
00056   uint16_t signature2;
00057   uint16_t x2;
00058   uint16_t y2;
00059   uint16_t width2;
00060   uint16_t height2;
00061 };
00062 
00063 template <class LinkType2> class TPixy2
00064 {
00065 public:
00066   TPixy2(uint8_t addr2=PIXY_DEFAULT_ADDR2);
00067   ~TPixy2();
00068     
00069   uint16_t getBlocks2(uint16_t maxBlocks2=1000);
00070   int8_t setServos2(uint16_t s02, uint16_t s12);
00071   void init2();
00072   
00073   Block2 *blocks2;
00074     
00075 private:
00076   bool getStart2();
00077   void resize2();
00078 
00079   LinkType2 link2;
00080   bool skipStart2;
00081   uint16_t blockCount2;
00082   uint16_t blockArraySize2;
00083 };
00084 
00085 
00086 template <class LinkType2> TPixy2<LinkType2>::TPixy2(uint8_t addr2)
00087 {
00088   skipStart2 = false;
00089   blockCount2 = 0;
00090   blockArraySize2 = PIXY_INITIAL_ARRAYSIZE2;
00091   blocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2);
00092   link2.setAddress2(addr2);
00093 }
00094 
00095 template <class LinkType2> void TPixy2<LinkType2>::init2()
00096 {
00097   link2.init2();
00098 }
00099 
00100 template <class LinkType2> TPixy2<LinkType2>::~TPixy2()
00101 {
00102   free(blocks2);
00103 }
00104 
00105 template <class LinkType2> bool TPixy2<LinkType2>::getStart2()
00106 {
00107   uint16_t w2, lastw2;
00108  
00109   lastw2 = 0xffff;
00110   
00111   while(true)
00112   {
00113     w2 = link2.getWord2();
00114     if (w2==0 && lastw2==0)
00115     {
00116       wait(0.00001);
00117       return false;
00118     }       
00119     else if (w2==PIXY_START_WORD2 && lastw2==PIXY_START_WORD2)
00120       return true;
00121     else if (w2==PIXY_START_WORDX2)
00122     {
00123       pc2.printf("reorder2");
00124       link2.getByte2(); // resync
00125     }
00126     lastw2 = w2; 
00127   }
00128 }
00129 
00130 template <class LinkType2> void TPixy2<LinkType2>::resize2()
00131 {
00132   Block2 *newBlocks2;
00133   blockArraySize2 += PIXY_INITIAL_ARRAYSIZE2;
00134   newBlocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2);
00135   memcpy(newBlocks2, blocks2, sizeof(Block2)*blockCount2);
00136   free(blocks2);
00137   blocks2 = newBlocks2;
00138 }  
00139         
00140 template <class LinkType2> uint16_t TPixy2<LinkType2>::getBlocks2(uint16_t maxBlocks2)
00141 {
00142   uint8_t i2;
00143   uint16_t w2, checksum2, sum2;
00144   Block2 *block2;
00145   
00146   if (!skipStart2)
00147   {
00148     if (getStart2()==false)
00149       return 0;
00150   }
00151   else
00152     skipStart2 = false;
00153     
00154   for(blockCount2=0; blockCount2<maxBlocks2 && blockCount2<PIXY_MAXIMUM_ARRAYSIZE2;)
00155   {
00156     checksum2 = link2.getWord2();
00157     if (checksum2==PIXY_START_WORD2) // we've reached the beginning of the next frame
00158     {
00159       skipStart2 = true;
00160       //Serial.println("skip");
00161       return blockCount2;
00162     }
00163     else if (checksum2==0)
00164       return blockCount2;
00165     
00166     if (blockCount2>blockArraySize2)
00167         resize2();
00168     
00169     block2 = blocks2 + blockCount2;
00170     
00171     for (i2=0, sum2=0; i2<sizeof(Block2)/sizeof(uint16_t); i2++)
00172     {
00173       w2 = link2.getWord2();
00174       sum2 += w2;
00175       *((uint16_t *)block2 + i2) = w2;
00176     }
00177 
00178     if (checksum2==sum2)
00179       blockCount2++;
00180     else
00181       pc2.printf("cs error 2");
00182     
00183     w2 = link2.getWord2();
00184     if (w2!=PIXY_START_WORD2)
00185       return blockCount2;
00186   }
00187 return maxBlocks2;
00188 }
00189 
00190 template <class LinkType2> int8_t TPixy2<LinkType2>::setServos2(uint16_t s02, uint16_t s12)
00191 {
00192   uint8_t outBuf2[6];
00193    
00194   outBuf2[0] = 0x00;
00195   outBuf2[1] = 0xff; 
00196   *(uint16_t *)(outBuf2 + 2) = s02;
00197   *(uint16_t *)(outBuf2 + 4) = s12;
00198   
00199   return link2.send2(outBuf2, 6);
00200 }
00201 
00202 #endif