Revision 0:56a3009221d3, committed 2014-08-12

Comitter:

MBM

Date:

Tue Aug 12 11:01:31 2014 +0000

Commit message:

The PixyLibrary was created from the Arduino libraries for the CMUcam5 Pixy found on http://cmucam.org/projects/cmucam5/files.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy1.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,126 @@
+//--------------------------------------------------------------------------------------------
+//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
+//
+
+/*
+  Pixy.h - Library for interfacing with Pixy.
+  Created by Scott Robinson, October 22, 2013.
+  Released into the public domain.
+
+  06.04.2014 v0.1.3 John Leimon 
+    + LinkSPI.init() should be called from the setup() 
+      function instead of being called automatically from
+      the TPixy<LinkSPI> constructor in global scope. This
+      is a workaround for a bug (?) in the Arduino DUE in which
+      calling SPI.begin() from global scope (via a constructor)
+      inhibits the operation of the Serial peripheral in the
+      DUE. [As of: Arduino 1.5.6-r2]
+*/
+
+#ifndef PIXY_H1
+#define PIXY_H1
+
+#include "TPixy1.h"
+#include "SPI1.h"
+
+
+#define PIXY_SYNC_BYTE1              0x5a
+#define PIXY_SYNC_BYTE_DATA1         0x5b
+#define PIXY_OUTBUF_SIZE1            6
+
+SPI spi1(p5, p6, p7); // mosi, miso, sclk
+
+class LinkSPI1
+{
+  public:
+    void init1()
+    {
+      outLen1 = 0;
+      #ifdef __SAM3X8E__
+      // DUE clock divider //
+      SPI1.setClockDivider1(84);
+      #else
+      // Default clock divider //
+      //SPI.setClockDivider(SPI_CLOCK_DIV16);
+      #endif
+    }
+    
+    uint16_t getWord1()
+    {
+      // ordering is different because Pixy is sending 16 bits through SPI 
+      // instead of 2 bytes in a 16-bit word as with I2C
+      uint16_t w1;
+      uint8_t c1, count1 = 0;
+
+      if (outLen1)
+      {
+        w1 = spi1.write(PIXY_SYNC_BYTE_DATA1);
+        count1 = outBuf1[outIndex1++];
+        if (outIndex1==outLen1)
+          outLen1 = 0; 
+      }
+      else
+        w1 = spi1.write(PIXY_SYNC_BYTE1);
+      w1 <<= 8;
+      c1 = spi1.write(count1);
+      w1 |= c1;
+
+      return w1;
+    }
+
+    uint8_t getByte1()
+    {
+      return spi1.write(0x00);
+    }
+    
+    int8_t send(uint8_t *data1, uint8_t len1)
+    {
+      if (len1>PIXY_OUTBUF_SIZE1 || outLen1!=0)
+        return -1;
+      memcpy(outBuf1, data1, len1);
+      outLen1 = len1;
+      outIndex1 = 0;
+      return len1;
+    }
+
+    void setAddress1(uint8_t addr1)
+    {
+      addr_1 = addr1;
+    }
+
+  private:
+    uint8_t outBuf1[PIXY_OUTBUF_SIZE1];
+    uint8_t outLen1;
+    uint8_t outIndex1;
+    uint8_t addr_1;
+};
+
+
+typedef TPixy1<LinkSPI1> Pixy1;
+
+#endif
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy2.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,125 @@
+//--------------------------------------------------------------------------------------------
+//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
+//
+
+/*
+  Pixy.h - Library for interfacing with Pixy.
+  Created by Scott Robinson, October 22, 2013.
+  Released into the public domain.
+
+  06.04.2014 v0.1.3 John Leimon 
+    + LinkSPI.init() should be called from the setup() 
+      function instead of being called automatically from
+      the TPixy<LinkSPI> constructor in global scope. This
+      is a workaround for a bug (?) in the Arduino DUE in which
+      calling SPI.begin() from global scope (via a constructor)
+      inhibits the operation of the Serial peripheral in the
+      DUE. [As of: Arduino 1.5.6-r2]
+*/
+
+#ifndef PIXY_H2
+#define PIXY_H2
+
+#include "TPixy2.h"
+#include "SPI2.h"
+
+
+#define PIXY_SYNC_BYTE2              0x5a
+#define PIXY_SYNC_BYTE_DATA2         0x5b
+#define PIXY_OUTBUF_SIZE2            6
+
+SPI spi2(p11, p12, p13); // mosi, miso, sclk
+
+class LinkSPI2
+{
+  public:
+    void init2()
+    {
+      outLen2 = 0;
+      #ifdef __SAM3X8E__
+      // DUE clock divider //
+      SPI.setClockDivider2(84);
+      #else
+      // Default clock divider //
+      //SPI.setClockDivider(SPI_CLOCK_DIV16);
+      #endif
+    }
+    
+    uint16_t getWord2()
+    {
+      // ordering is different because Pixy is sending 16 bits through SPI 
+      // instead of 2 bytes in a 16-bit word as with I2C
+      uint16_t w2;
+      uint8_t c2, count2 = 0;
+
+      if (outLen2)
+      {
+        w2 = spi2.write(PIXY_SYNC_BYTE_DATA2);
+        count2 = outBuf2[outIndex2++];
+        if (outIndex2==outLen2)
+          outLen2 = 0; 
+      }
+      else
+        w2 = spi2.write(PIXY_SYNC_BYTE2);
+      w2 <<= 8;
+      c2 = spi2.write(count2);
+      w2 |= c2;
+
+      return w2;
+    }
+
+    uint8_t getByte2()
+    {
+      return spi2.write(0x00);
+    }
+    
+    int8_t send(uint8_t *data2, uint8_t len2)
+    {
+      if (len2>PIXY_OUTBUF_SIZE2 || outLen2!=0)
+        return -1;
+      memcpy(outBuf2, data2, len2);
+      outLen2 = len2;
+      outIndex2 = 0;
+      return len2;
+    }
+
+    void setAddress2(uint8_t addr2)
+    {
+      addr_2 = addr2;
+    }
+
+  private:
+    uint8_t outBuf2[PIXY_OUTBUF_SIZE2];
+    uint8_t outLen2;
+    uint8_t outIndex2;
+    uint8_t addr_2;
+};
+
+
+typedef TPixy2<LinkSPI2> Pixy2;
+
+#endif
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SPI1.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,72 @@
+//--------------------------------------------------------------------------------------------
+//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
+//
+
+#ifndef _SPI_H1
+#define _SPI_H1
+#include "iserial1.h"
+
+#define SPI_RECEIVEBUF_SIZE1     16
+#define SPI_TRANSMITBUF_SIZE1    16
+
+#define SS_ASSERT1()             LPC_SGPIO1->GPIO_OUTREG1 = 0;
+#define SS_NEGATE1()             LPC_SGPIO1->GPIO_OUTREG1 = 1<<14;
+
+#define SPI_SYNC_MASK1           0xff00
+#define SPI_SYNC_WORD1           0x5a00
+#define SPI_SYNC_WORD_DATA1      0x5b00
+#define SPI_MIN_SYNC_COUNT1      5
+
+class Spi1 : public Iserial1
+{
+public:
+    Spi1(SerialCallback1 callback1);
+
+    // Iserial methods
+    virtual int open1();
+    virtual int close1();
+    virtual int receive1(uint8_t *buf1, uint32_t len1);
+    virtual int receiveLen1();
+    virtual int update1();
+
+    void slaveHandler1();
+
+private:
+    int checkIdle1();
+    int sync1();
+    ReceiveQ1<uint16_t> m_rq1;
+    TransmitQ1<uint16_t> m_tq1;
+
+    bool m_sync1;
+    uint32_t m_recvCounter1;
+    uint32_t m_lastRecvCounter1; 
+    uint8_t m_syncCounter1;
+};
+
+void SPIinit1(SerialCallback1 callback1);
+
+extern Spi1 *g_spi1;
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SPI2.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,72 @@
+//--------------------------------------------------------------------------------------------
+//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
+//
+
+#ifndef _SPI_H2
+#define _SPI_H2
+#include "iserial2.h"
+
+#define SPI_RECEIVEBUF_SIZE2     16
+#define SPI_TRANSMITBUF_SIZE2    16
+
+#define SS_ASSERT2()             LPC_SGPIO->GPIO_OUTREG2 = 0;
+#define SS_NEGATE2()             LPC_SGPIO->GPIO_OUTREG2 = 1<<14;
+
+#define SPI_SYNC_MASK2           0xff00
+#define SPI_SYNC_WORD2           0x5a00
+#define SPI_SYNC_WORD_DATA2      0x5b00
+#define SPI_MIN_SYNC_COUNT2      5
+
+class Spi2 : public Iserial2
+{
+public:
+    Spi2(SerialCallback2 callback2);
+
+    // Iserial methods
+    virtual int open2();
+    virtual int close2();
+    virtual int receive2(uint8_t *buf2, uint32_t len2);
+    virtual int receiveLen2();
+    virtual int update2();
+
+    void slaveHandler2();
+
+private:
+    int checkIdle2();
+    int sync2();
+    ReceiveQ2<uint16_t> m_rq2;
+    TransmitQ2<uint16_t> m_tq2;
+
+    bool m_sync2;
+    uint32_t m_recvCounter2;
+    uint32_t m_lastRecvCounter2; 
+    uint8_t m_syncCounter2;
+};
+
+void SPIinit2(SerialCallback2 callback2);
+
+extern Spi2 *g_spi2;
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TPixy1.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,207 @@
+//--------------------------------------------------------------------------------------------
+//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
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TPixy2.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,202 @@
+//--------------------------------------------------------------------------------------------
+//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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iserial1.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,163 @@
+//--------------------------------------------------------------------------------------------
+//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
+//
+
+#ifndef _ISERIAL_H1
+#define _ISERIAL_H1
+
+typedef uint32_t (*SerialCallback1)(uint8_t *data1, uint32_t len1); 
+
+// circular queue, for receiving data
+template <class BufType1> class ReceiveQ1
+{
+public:
+    ReceiveQ1(uint32_t size1)
+    {
+        m_size1 = size1;
+        m_buf1 = new BufType1[m_size1];
+        m_read1 = 0;
+        m_write1 = 0;
+        m_produced1 = 0;
+        m_consumed1 = 0;
+    }
+
+    ~ReceiveQ1()
+    {
+        delete [] m_buf1;
+    }
+
+    inline int32_t receiveLen1()
+    {
+        return m_produced1 - m_consumed1;
+    }
+
+    inline int32_t freeLen1()
+    {
+        return m_size1 - receiveLen1();
+    }
+
+    inline int read(BufType1 *data1)
+    {
+        if (receiveLen1()<=0)
+            return 0;
+        *data1 = m_buf1[m_read1++];
+        m_consumed1++;
+
+        if (m_read1==m_size1)
+            m_read1 = 0;
+
+        return 1;
+    }
+
+    inline int write(BufType1 data1)
+    {
+        if (freeLen1()<=0)
+            return 0; 
+
+        m_buf1[m_write1++] = data1;
+        m_produced1++;
+
+        if (m_write1==m_size1)
+            m_write1 = 0;
+
+        return 1;
+    }
+
+    uint32_t m_size1;
+    BufType1 *m_buf1;
+    uint32_t m_read1;
+    uint32_t m_write1;
+    uint32_t m_produced1;
+    uint32_t m_consumed1;
+};
+
+
+// linear queue, to buffer a chunk and dispense it out
+template <class BufType1> class TransmitQ1
+{
+public:
+    TransmitQ1(uint32_t size1, SerialCallback1 callback1)
+    {
+        m_size1 = size1;
+        m_buf1 = new BufType1[m_size1];
+        m_read1 = 0;
+        m_len1 = 0;
+        m_callback1 = callback1;
+    }
+
+    ~TransmitQ1()
+    {
+        delete [] m_buf1;
+    }
+
+    int read(BufType1 *data1)
+    {
+        if (m_len1==0)
+        {
+            m_len1 = (*m_callback1)((uint8_t *)m_buf1, m_size1*sizeof(BufType1))/sizeof(BufType1);
+            if (m_len1==0)
+                return 0;
+            m_read1 = 0;
+        }
+        *data1 = m_buf1[m_read1++];
+        m_len1--;
+
+        return 1;
+    }
+
+    uint32_t m_size1;
+    BufType1 *m_buf1;
+    uint32_t m_read1;
+    uint32_t m_len1;
+    SerialCallback1 m_callback1;
+};
+
+// virtual interface to a serial device
+class Iserial1
+{
+public:
+    virtual int open()
+    {
+        return 0;
+    }
+    virtual int close()
+    {
+        return 0;
+    }
+    virtual int receive(uint8_t *buf1, uint32_t len1)
+    {
+        return 0;
+    }
+    virtual int receiveLen1()
+    {
+        return 0;
+    }
+    virtual int update1()
+    {
+        return 0;
+    }
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iserial2.h	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,163 @@
+//--------------------------------------------------------------------------------------------
+//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
+//
+
+#ifndef _ISERIAL_H2
+#define _ISERIAL_H2
+
+typedef uint32_t (*SerialCallback2)(uint8_t *data2, uint32_t len2); 
+
+// circular queue, for receiving data
+template <class BufType2> class ReceiveQ2
+{
+public:
+    ReceiveQ2(uint32_t size2)
+    {
+        m_size2 = size2;
+        m_buf2 = new BufType2[m_size2];
+        m_read2 = 0;
+        m_write2 = 0;
+        m_produced2 = 0;
+        m_consumed2 = 0;
+    }
+
+    ~ReceiveQ2()
+    {
+        delete [] m_buf2;
+    }
+
+    inline int32_t receiveLen2()
+    {
+        return m_produced2 - m_consumed2;
+    }
+
+    inline int32_t freeLen2()
+    {
+        return m_size2 - receiveLen2();
+    }
+
+    inline int read(BufType2 *data2)
+    {
+        if (receiveLen2()<=0)
+            return 0;
+        *data2 = m_buf2[m_read2++];
+        m_consumed2++;
+
+        if (m_read2==m_size2)
+            m_read2 = 0;
+
+        return 1;
+    }
+
+    inline int write(BufType2 data2)
+    {
+        if (freeLen2()<=0)
+            return 0; 
+
+        m_buf2[m_write2++] = data2;
+        m_produced2++;
+
+        if (m_write2==m_size2)
+            m_write2 = 0;
+
+        return 1;
+    }
+
+    uint32_t m_size2;
+    BufType2 *m_buf2;
+    uint32_t m_read2;
+    uint32_t m_write2;
+    uint32_t m_produced2;
+    uint32_t m_consumed2;
+};
+
+
+// linear queue, to buffer a chunk and dispense it out
+template <class BufType2> class TransmitQ2
+{
+public:
+    TransmitQ2(uint32_t size2, SerialCallback2 callback2)
+    {
+        m_size2 = size2;
+        m_buf2 = new BufType1[m_size1];
+        m_read2 = 0;
+        m_len2 = 0;
+        m_callback2= callback2;
+    }
+
+    ~TransmitQ2()
+    {
+        delete [] m_buf2;
+    }
+
+    int read(BufType2 *data2)
+    {
+        if (m_len2==0)
+        {
+            m_len2 = (*m_callback2)((uint8_t *)m_buf2, m_size2*sizeof(BufType2))/sizeof(BufType2);
+            if (m_len2==0)
+                return 0;
+            m_read2 = 0;
+        }
+        *data2 = m_buf2[m_read2++];
+        m_len2--;
+
+        return 1;
+    }
+
+    uint32_t m_size2;
+    BufType2 *m_buf2;
+    uint32_t m_read2;
+    uint32_t m_len2;
+    SerialCallback2 m_callback2;
+};
+
+// virtual interface to a serial device
+class Iserial2
+{
+public:
+    virtual int open()
+    {
+        return 0;
+    }
+    virtual int close()
+    {
+        return 0;
+    }
+    virtual int receive(uint8_t *buf2, uint32_t len2)
+    {
+        return 0;
+    }
+    virtual int receiveLen2()
+    {
+        return 0;
+    }
+    virtual int update2()
+    {
+        return 0;
+    }
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Aug 12 11:01:31 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/078e4b97a13e