File content as of revision 1:4b14159ab9bb:

//--------------------------------------------------------------------------------------------
//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_H1
#define _TPIXY_H1

#define PIXY_INITIAL_ARRAYSIZE1      30
#define PIXY_MAXIMUM_ARRAYSIZE1      130
#define PIXY_START_WORD1             0xaa55
#define PIXY_START_WORDX1            0x55aa
#define PIXY_DEFAULT_ADDR1           0x54  // I2C

Serial pc1(USBTX, USBRX);

struct Block1 
{
  void print1()
  {
    char buf1[64];
    sprintf(buf1, "sig: %d x1: %d y1: %d width1: %d height1: %d\n", signature1, x1, y1, width1, height1);
    printf(buf1);
    Xp1 = x1;
    Yp1 = y1;
    sig1 = signature1;
  }
  uint16_t signature1;
  uint16_t x1;
  uint16_t y1;
  uint16_t width1;
  uint16_t height1;
};

template <class LinkType1> class TPixy1
{
public:
  TPixy1(uint8_t addr1=PIXY_DEFAULT_ADDR1);
  ~TPixy1();
	
  uint16_t getBlocks1(uint16_t maxBlocks1=1000);
  int8_t setServos1(uint16_t s01, uint16_t s11);
  void init1();
  
  Block1 *blocks1;
	
private:
  bool getStart1();
  void resize1();

  LinkType1 link1;
  bool skipStart1;
  uint16_t blockCount1;
  uint16_t blockArraySize1;
};


template <class LinkType1> TPixy1<LinkType1>::TPixy1(uint8_t addr1)
{
  skipStart1 = false;
  blockCount1 = 0;
  blockArraySize1 = PIXY_INITIAL_ARRAYSIZE1;
  blocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1);
  link1.setAddress1(addr1);
}

template <class LinkType1> void TPixy1<LinkType1>::init1()
{
  link1.init1();
}

template <class LinkType1> TPixy1<LinkType1>::~TPixy1()
{
  free(blocks1);
}

template <class LinkType1> bool TPixy1<LinkType1>::getStart1()
{
  uint16_t w1, lastw1;
 
  lastw1 = 0xffff;
  
  while(true)
  {
    w1 = link1.getWord1();
    if (w1==0 && lastw1==0)
	{
      wait(0.00001);
	  return false;
	}		
    else if (w1==PIXY_START_WORD1 && lastw1==PIXY_START_WORD1)
      return true;
	else if (w1==PIXY_START_WORDX1)
	{
	  pc1.printf("reorder1");
	  link1.getByte1(); // resync
	}
	lastw1 = w1; 
  }
}

template <class LinkType1> void TPixy1<LinkType1>::resize1()
{
  Block1 *newBlocks1;
  blockArraySize1 += PIXY_INITIAL_ARRAYSIZE1;
  newBlocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1);
  memcpy(newBlocks1, blocks1, sizeof(Block1)*blockCount1);
  free(blocks1);
  blocks1 = newBlocks1;
}  
		
template <class LinkType1> uint16_t TPixy1<LinkType1>::getBlocks1(uint16_t maxBlocks1)
{
  uint8_t i1;
  uint16_t w1, checksum1, sum1;
  Block1 *block1;
  
  if (!skipStart1)
  {
    if (getStart1()==false)
      return 0;
  }
  else
	skipStart1 = false;
	
  for(blockCount1=0; blockCount1<maxBlocks1 && blockCount1<PIXY_MAXIMUM_ARRAYSIZE1;)
  {
    checksum1 = link1.getWord1();
    if (checksum1==PIXY_START_WORD1) // we've reached the beginning of the next frame
    {
      skipStart1 = true;
	  //Serial.println("skip");
      return blockCount1;
    }
    else if (checksum1==0)
      return blockCount1;
    
	if (blockCount1>blockArraySize1)
		resize1();
	
	block1 = blocks1 + blockCount1;
	
    for (i1=0, sum1=0; i1<sizeof(Block1)/sizeof(uint16_t); i1++)
    {
      w1 = link1.getWord1();
      sum1 += w1;
      *((uint16_t *)block1 + i1) = w1;
    }

    if (checksum1==sum1)
      blockCount1++;
    else
      pc1.printf("cs error 1");
	
	w1 = link1.getWord1();
    if (w1!=PIXY_START_WORD1)
      return blockCount1;
  }
return maxBlocks1;
}

template <class LinkType1> int8_t TPixy1<LinkType1>::setServos1(uint16_t s01, uint16_t s11)
{
  uint8_t outBuf1[6];
   
  outBuf1[0] = 0x00;
  outBuf1[1] = 0xff; 
  *(uint16_t *)(outBuf1 + 2) = s01;
  *(uint16_t *)(outBuf1 + 4) = s11;
  
  return link1.send1(outBuf1, 6);
}

#endif