same
Fork of PixyLibrary by
TPixy2.h
- Committer:
- johnylafleur
- Date:
- 2015-04-13
- Revision:
- 1:4b14159ab9bb
- Parent:
- 0:56a3009221d3
File content as of revision 1:4b14159ab9bb:
//-------------------------------------------------------------------------------------------- //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