TPixy1.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_H1
00035 #define _TPIXY_H1
00036 
00037 #define PIXY_INITIAL_ARRAYSIZE1      30
00038 #define PIXY_MAXIMUM_ARRAYSIZE1      130
00039 #define PIXY_START_WORD1             0xaa55
00040 #define PIXY_START_WORDX1            0x55aa
00041 #define PIXY_DEFAULT_ADDR1           0x54  // I2C
00042 
00043 Serial pc1(USBTX, USBRX);
00044 
00045 struct Block1 
00046 {
00047   void print1()
00048   {
00049     char buf1[64];
00050     sprintf(buf1, "sig: %d x1: %d y1: %d width1: %d height1: %d\n", signature1, x1, y1, width1, height1);
00051     printf(buf1);
00052     Xp1 = x1;
00053     Yp1 = y1;
00054     sig1 = signature1;
00055   }
00056   uint16_t signature1;
00057   uint16_t x1;
00058   uint16_t y1;
00059   uint16_t width1;
00060   uint16_t height1;
00061 };
00062 
00063 template <class LinkType1> class TPixy1
00064 {
00065 public:
00066   TPixy1(uint8_t addr1=PIXY_DEFAULT_ADDR1);
00067   ~TPixy1();
00068     
00069   uint16_t getBlocks1(uint16_t maxBlocks1=1000);
00070   int8_t setServos1(uint16_t s01, uint16_t s11);
00071   void init1();
00072   
00073   Block1 *blocks1;
00074     
00075 private:
00076   bool getStart1();
00077   void resize1();
00078 
00079   LinkType1 link1;
00080   bool skipStart1;
00081   uint16_t blockCount1;
00082   uint16_t blockArraySize1;
00083 };
00084 
00085 
00086 template <class LinkType1> TPixy1<LinkType1>::TPixy1(uint8_t addr1)
00087 {
00088   skipStart1 = false;
00089   blockCount1 = 0;
00090   blockArraySize1 = PIXY_INITIAL_ARRAYSIZE1;
00091   blocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1);
00092   link1.setAddress1(addr1);
00093 }
00094 
00095 template <class LinkType1> void TPixy1<LinkType1>::init1()
00096 {
00097   link1.init1();
00098 }
00099 
00100 template <class LinkType1> TPixy1<LinkType1>::~TPixy1()
00101 {
00102   free(blocks1);
00103 }
00104 
00105 template <class LinkType1> bool TPixy1<LinkType1>::getStart1()
00106 {
00107   uint16_t w1, lastw1;
00108  
00109   lastw1 = 0xffff;
00110   
00111   while(true)
00112   {
00113     w1 = link1.getWord1();
00114     if (w1==0 && lastw1==0)
00115     {
00116       wait(0.00001);
00117       return false;
00118     }       
00119     else if (w1==PIXY_START_WORD1 && lastw1==PIXY_START_WORD1)
00120       return true;
00121     else if (w1==PIXY_START_WORDX1)
00122     {
00123       pc1.printf("reorder1");
00124       link1.getByte1(); // resync
00125     }
00126     lastw1 = w1; 
00127   }
00128 }
00129 
00130 template <class LinkType1> void TPixy1<LinkType1>::resize1()
00131 {
00132   Block1 *newBlocks1;
00133   blockArraySize1 += PIXY_INITIAL_ARRAYSIZE1;
00134   newBlocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1);
00135   memcpy(newBlocks1, blocks1, sizeof(Block1)*blockCount1);
00136   free(blocks1);
00137   blocks1 = newBlocks1;
00138 }  
00139         
00140 template <class LinkType1> uint16_t TPixy1<LinkType1>::getBlocks1(uint16_t maxBlocks1)
00141 {
00142   uint8_t i1;
00143   uint16_t w1, checksum1, sum1;
00144   Block1 *block1;
00145   
00146   if (!skipStart1)
00147   {
00148     if (getStart1()==false)
00149       return 0;
00150   }
00151   else
00152     skipStart1 = false;
00153     
00154   for(blockCount1=0; blockCount1<maxBlocks1 && blockCount1<PIXY_MAXIMUM_ARRAYSIZE1;)
00155   {
00156     checksum1 = link1.getWord1();
00157     if (checksum1==PIXY_START_WORD1) // we've reached the beginning of the next frame
00158     {
00159       skipStart1 = true;
00160       //Serial.println("skip");
00161       return blockCount1;
00162     }
00163     else if (checksum1==0)
00164       return blockCount1;
00165     
00166     if (blockCount1>blockArraySize1)
00167         resize1();
00168     
00169     block1 = blocks1 + blockCount1;
00170     
00171     for (i1=0, sum1=0; i1<sizeof(Block1)/sizeof(uint16_t); i1++)
00172     {
00173       w1 = link1.getWord1();
00174       sum1 += w1;
00175       *((uint16_t *)block1 + i1) = w1;
00176     }
00177 
00178     if (checksum1==sum1)
00179       blockCount1++;
00180     else
00181       pc1.printf("cs error 1");
00182     
00183     w1 = link1.getWord1();
00184     if (w1!=PIXY_START_WORD1)
00185       return blockCount1;
00186   }
00187 return maxBlocks1;
00188 }
00189 
00190 template <class LinkType1> int8_t TPixy1<LinkType1>::setServos1(uint16_t s01, uint16_t s11)
00191 {
00192   uint8_t outBuf1[6];
00193    
00194   outBuf1[0] = 0x00;
00195   outBuf1[1] = 0xff; 
00196   *(uint16_t *)(outBuf1 + 2) = s01;
00197   *(uint16_t *)(outBuf1 + 4) = s11;
00198   
00199   return link1.send1(outBuf1, 6);
00200 }
00201 
00202 #endif
00203 
00204 
00205 
00206 
00207