Fork of pixy by
Revision 1:683b8f2ba78e, committed 2017-07-05
- Comitter:
- Bisca
- Date:
- Wed Jul 05 09:17:06 2017 +0000
- Parent:
- 0:ed8dc4531ac1
- Commit message:
Changed in this revision
T_Pixy.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/T_Pixy.h Wed Jul 05 09:17:06 2017 +0000 @@ -0,0 +1,273 @@ +// +// 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 +// +// This file is for defining the Block struct and the Pixy template class. +// (TPixy). TPixy takes a communication link as a template parameter so that +// all communication modes (SPI, I2C and UART) can share the same code. +// + +#ifndef _TPIXY_H +#define _TPIXY_H + +#include "Arduino.h" + +// Communication/misc parameters +#define PIXY_INITIAL_ARRAYSIZE 30 +#define PIXY_MAXIMUM_ARRAYSIZE 130 +#define PIXY_START_WORD 0xaa55 +#define PIXY_START_WORD_CC 0xaa56 +#define PIXY_START_WORDX 0x55aa +#define PIXY_MAX_SIGNATURE 7 +#define PIXY_DEFAULT_ARGVAL 0xffff + +// Pixy x-y position values +#define PIXY_MIN_X 0L +#define PIXY_MAX_X 319L +#define PIXY_MIN_Y 0L +#define PIXY_MAX_Y 199L + +// RC-servo values +#define PIXY_RCS_MIN_POS 0L +#define PIXY_RCS_MAX_POS 1000L +#define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2) + + +enum BlockType +{ + NORMAL_BLOCK, + CC_BLOCK +}; + +struct Block +{ + // print block structure! + void print() + { + int i, j; + char buf[128], sig[6], d; + bool flag; + if (signature>PIXY_MAX_SIGNATURE) // color code! (CC) + { + // convert signature number to an octal string + for (i=12, j=0, flag=false; i>=0; i-=3) + { + d = (signature>>i)&0x07; + if (d>0 && !flag) + flag = true; + if (flag) + sig[j++] = d + '0'; + } + sig[j] = '\0'; + sprintf(buf, "CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d\n", sig, signature, x, y, width, height, angle); + } + else // regular block. Note, angle is always zero, so no need to print + sprintf(buf, "sig: %d x: %d y: %d width: %d height: %d\n", signature, x, y, width, height); + Serial.print(buf); + } + uint16_t signature; + uint16_t x; + uint16_t y; + uint16_t width; + uint16_t height; + uint16_t angle; +}; + + + +template <class LinkType> class TPixy +{ +public: + TPixy(uint16_t arg=PIXY_DEFAULT_ARGVAL); + ~TPixy(); + + uint16_t getBlocks(uint16_t maxBlocks=1000); + int8_t setServos(uint16_t s0, uint16_t s1); + int8_t setBrightness(uint8_t brightness); + int8_t setLED(uint8_t r, uint8_t g, uint8_t b); + void init(); + + Block *blocks; + +private: + boolean getStart(); + void resize(); + + LinkType link; + boolean skipStart; + BlockType blockType; + uint16_t blockCount; + uint16_t blockArraySize; +}; + + +template <class LinkType> TPixy<LinkType>::TPixy(uint16_t arg) +{ + skipStart = false; + blockCount = 0; + blockArraySize = PIXY_INITIAL_ARRAYSIZE; + blocks = (Block *)malloc(sizeof(Block)*blockArraySize); + link.setArg(arg); +} + +template <class LinkType> void TPixy<LinkType>::init() +{ + link.init(); +} + +template <class LinkType> TPixy<LinkType>::~TPixy() +{ + free(blocks); +} + +template <class LinkType> boolean TPixy<LinkType>::getStart() +{ + uint16_t w, lastw; + + lastw = 0xffff; + + while(true) + { + w = link.getWord(); + if (w==0 && lastw==0) + { + delayMicroseconds(10); + return false; + } + else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD) + { + blockType = NORMAL_BLOCK; + return true; + } + else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD) + { + blockType = CC_BLOCK; + return true; + } + else if (w==PIXY_START_WORDX) + { + Serial.println("reorder"); + link.getByte(); // resync + } + lastw = w; + } +} + +template <class LinkType> void TPixy<LinkType>::resize() +{ + blockArraySize += PIXY_INITIAL_ARRAYSIZE; + blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize); +} + +template <class LinkType> uint16_t TPixy<LinkType>::getBlocks(uint16_t maxBlocks) +{ + uint8_t i; + uint16_t w, checksum, sum; + Block *block; + + if (!skipStart) + { + if (getStart()==false) + return 0; + } + else + skipStart = false; + + for(blockCount=0; blockCount<maxBlocks && blockCount<PIXY_MAXIMUM_ARRAYSIZE;) + { + checksum = link.getWord(); + if (checksum==PIXY_START_WORD) // we've reached the beginning of the next frame + { + skipStart = true; + blockType = NORMAL_BLOCK; + //Serial.println("skip"); + return blockCount; + } + else if (checksum==PIXY_START_WORD_CC) + { + skipStart = true; + blockType = CC_BLOCK; + return blockCount; + } + else if (checksum==0) + return blockCount; + + if (blockCount>blockArraySize) + resize(); + + block = blocks + blockCount; + + for (i=0, sum=0; i<sizeof(Block)/sizeof(uint16_t); i++) + { + if (blockType==NORMAL_BLOCK && i>=5) // skip + { + block->angle = 0; + break; + } + w = link.getWord(); + sum += w; + *((uint16_t *)block + i) = w; + } + + if (checksum==sum) + blockCount++; + else + Serial.println("cs error"); + + w = link.getWord(); + if (w==PIXY_START_WORD) + blockType = NORMAL_BLOCK; + else if (w==PIXY_START_WORD_CC) + blockType = CC_BLOCK; + else + return blockCount; + } +} + +template <class LinkType> int8_t TPixy<LinkType>::setServos(uint16_t s0, uint16_t s1) +{ + uint8_t outBuf[6]; + + outBuf[0] = 0x00; + outBuf[1] = 0xff; + *(uint16_t *)(outBuf + 2) = s0; + *(uint16_t *)(outBuf + 4) = s1; + + return link.send(outBuf, 6); +} + +template <class LinkType> int8_t TPixy<LinkType>::setBrightness(uint8_t brightness) +{ + uint8_t outBuf[3]; + + outBuf[0] = 0x00; + outBuf[1] = 0xfe; + outBuf[2] = brightness; + + return link.send(outBuf, 3); +} + +template <class LinkType> int8_t TPixy<LinkType>::setLED(uint8_t r, uint8_t g, uint8_t b) +{ + uint8_t outBuf[5]; + + outBuf[0] = 0x00; + outBuf[1] = 0xfd; + outBuf[2] = r; + outBuf[3] = g; + outBuf[4] = b; + + return link.send(outBuf, 5); +} + +#endif