Mathieu Malone
/
PixyLibrary
Library used to initialize and to communicate with the CMUcam5 Pixy
Diff: TPixy1.h
- Revision:
- 0:56a3009221d3
diff -r 000000000000 -r 56a3009221d3 TPixy1.h --- /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 + + + + +