same
Fork of PixyLibrary by
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