Diff: TPixy2.h

Revision:

0:56a3009221d3

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