Diff: TPixy1.h

Revision:

0:56a3009221d3

--- /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
+
+
+
+
+