Homologation Cachan
Fork of TPixy-Interface by
TPixy.h
- Committer:
- lenriquez389
- Date:
- 2018-06-09
- Revision:
- 4:ee635fc62b6e
- Parent:
- 3:66df7d295245
File content as of revision 4:ee635fc62b6e:
#ifndef _TPIXY_H #define _TPIXY_H #include "mbed.h" #include "TPixyInterface.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 #define PIXY_CENTER_X ((PIXY_MAX_X-PIXY_MIN_X)/2) // RC-servo values #define PIXY_RCS_MIN_POS 650L #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 { uint16_t signature; uint16_t x; uint16_t y; uint16_t width; uint16_t height; uint16_t angle; }; template <class TPixyInterface> class TPixy { public: Serial* serial; Block *blocks; TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL); ~TPixy(); uint16_t getBlocks(uint16_t maxBlocks = 1000); int8_t setServos(uint16_t s0, uint16_t s1); void init(); private: TPixyInterface* link; BlockType blockType; bool getStart(); void resize(); bool skipStart; uint16_t blockCount; uint16_t blockArraySize; }; template <class TPixyInterface> void TPixy<TPixyInterface>::init() { link->init(); } template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type) { skipStart = false; blockCount = 0; blockArraySize = PIXY_INITIAL_ARRAYSIZE; blocks = (Block *)malloc(sizeof(Block)*blockArraySize); link->setArg(arg); } template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy() { free(blocks); } template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart() { uint16_t w, lastw; lastw = 0xffff; while(true) { w = link->getWord(); if (w == 0 && lastw == 0) { wait_ms(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) { if (serial != NULL) { serial->printf("reorder"); } link->getByte(); // resync } lastw = w; } } template <class TPixyInterface> void TPixy<TPixyInterface>::resize() { blockArraySize += PIXY_INITIAL_ARRAYSIZE; blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize); } template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::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) { skipStart = true; blockType = NORMAL_BLOCK; if (serial != NULL) { serial->printf("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 { w = link->getWord(); if (serial != NULL) { serial->printf("cs error"); } } if (w == PIXY_START_WORD) { blockType = NORMAL_BLOCK; } else if (w == PIXY_START_WORD_CC) { blockType = CC_BLOCK; } else { return blockCount; } } return blockCount; } template <class TPixyInterface> int8_t TPixy<TPixyInterface>::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); } #endif