File content as of revision 0:56a3009221d3:

//--------------------------------------------------------------------------------------------
//Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip 
//
//Modifications made by: Mathieu Malone
//Modifications: Modified Arduino code to function with mbed development platform
//Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication
//               between the mbed platform and putty terminal through USB
//
//Latest update by: Mathieu Malone
//Date of last update: July 24th, 2014
//--------------------------------------------------------------------------------------------
//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

/*
  06.04.2014 v0.1.3 John Leimon 
    + Added init() for initializing Pixy, which should
      be called from the setup() function. See comment
      in Pixy.h for details.
*/

#ifndef _TPIXY_H2
#define _TPIXY_H2

#define PIXY_INITIAL_ARRAYSIZE2      30
#define PIXY_MAXIMUM_ARRAYSIZE2      130
#define PIXY_START_WORD2             0xaa55
#define PIXY_START_WORDX2            0x55aa
#define PIXY_DEFAULT_ADDR2           0x54  // I2C

Serial pc2(USBTX, USBRX);

struct Block2 
{
  void print2()
  {
    char buf2[64];
    sprintf(buf2, "sig: %d x2: %d y2: %d width2: %d height2: %d\n", signature2, x2, y2, width2, height2);
    printf(buf2);
    Xp2 = x2;
    Yp2= y2;
    sig2 = signature2;
  }
  uint16_t signature2;
  uint16_t x2;
  uint16_t y2;
  uint16_t width2;
  uint16_t height2;
};

template <class LinkType2> class TPixy2
{
public:
  TPixy2(uint8_t addr2=PIXY_DEFAULT_ADDR2);
  ~TPixy2();
    
  uint16_t getBlocks2(uint16_t maxBlocks2=1000);
  int8_t setServos2(uint16_t s02, uint16_t s12);
  void init2();
  
  Block2 *blocks2;
    
private:
  bool getStart2();
  void resize2();

  LinkType2 link2;
  bool skipStart2;
  uint16_t blockCount2;
  uint16_t blockArraySize2;
};


template <class LinkType2> TPixy2<LinkType2>::TPixy2(uint8_t addr2)
{
  skipStart2 = false;
  blockCount2 = 0;
  blockArraySize2 = PIXY_INITIAL_ARRAYSIZE2;
  blocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2);
  link2.setAddress2(addr2);
}

template <class LinkType2> void TPixy2<LinkType2>::init2()
{
  link2.init2();
}

template <class LinkType2> TPixy2<LinkType2>::~TPixy2()
{
  free(blocks2);
}

template <class LinkType2> bool TPixy2<LinkType2>::getStart2()
{
  uint16_t w2, lastw2;
 
  lastw2 = 0xffff;
  
  while(true)
  {
    w2 = link2.getWord2();
    if (w2==0 && lastw2==0)
    {
      wait(0.00001);
      return false;
    }       
    else if (w2==PIXY_START_WORD2 && lastw2==PIXY_START_WORD2)
      return true;
    else if (w2==PIXY_START_WORDX2)
    {
      pc2.printf("reorder2");
      link2.getByte2(); // resync
    }
    lastw2 = w2; 
  }
}

template <class LinkType2> void TPixy2<LinkType2>::resize2()
{
  Block2 *newBlocks2;
  blockArraySize2 += PIXY_INITIAL_ARRAYSIZE2;
  newBlocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2);
  memcpy(newBlocks2, blocks2, sizeof(Block2)*blockCount2);
  free(blocks2);
  blocks2 = newBlocks2;
}  
        
template <class LinkType2> uint16_t TPixy2<LinkType2>::getBlocks2(uint16_t maxBlocks2)
{
  uint8_t i2;
  uint16_t w2, checksum2, sum2;
  Block2 *block2;
  
  if (!skipStart2)
  {
    if (getStart2()==false)
      return 0;
  }
  else
    skipStart2 = false;
    
  for(blockCount2=0; blockCount2<maxBlocks2 && blockCount2<PIXY_MAXIMUM_ARRAYSIZE2;)
  {
    checksum2 = link2.getWord2();
    if (checksum2==PIXY_START_WORD2) // we've reached the beginning of the next frame
    {
      skipStart2 = true;
      //Serial.println("skip");
      return blockCount2;
    }
    else if (checksum2==0)
      return blockCount2;
    
    if (blockCount2>blockArraySize2)
        resize2();
    
    block2 = blocks2 + blockCount2;
    
    for (i2=0, sum2=0; i2<sizeof(Block2)/sizeof(uint16_t); i2++)
    {
      w2 = link2.getWord2();
      sum2 += w2;
      *((uint16_t *)block2 + i2) = w2;
    }

    if (checksum2==sum2)
      blockCount2++;
    else
      pc2.printf("cs error 2");
    
    w2 = link2.getWord2();
    if (w2!=PIXY_START_WORD2)
      return blockCount2;
  }
return maxBlocks2;
}

template <class LinkType2> int8_t TPixy2<LinkType2>::setServos2(uint16_t s02, uint16_t s12)
{
  uint8_t outBuf2[6];
   
  outBuf2[0] = 0x00;
  outBuf2[1] = 0xff; 
  *(uint16_t *)(outBuf2 + 2) = s02;
  *(uint16_t *)(outBuf2 + 4) = s12;
  
  return link2.send2(outBuf2, 6);
}

#endif