Luis Enriquez / CACHAN_HOMOG

Homologation Cachan

Fork of TPixy-Interface by Stephen Wilkins

TPixy.h@0:ef0e3c67dc5b, 2016-03-14 (annotated)

Committer:
swilkins8
Date:
Mon Mar 14 00:35:23 2016 +0000
Revision:
0:ef0e3c67dc5b
Child:
2:fa582d9d91b5
Completed rewrite of code to expand interfacing options

Who changed what in which revision?

UserRevisionLine numberNew contents of line
swilkins8 0:ef0e3c67dc5b 1 //
swilkins8 0:ef0e3c67dc5b 2 // begin license header
swilkins8 0:ef0e3c67dc5b 3 //
swilkins8 0:ef0e3c67dc5b 4 // This file is part of Pixy CMUcam5 or "Pixy" for short
swilkins8 0:ef0e3c67dc5b 5 //
swilkins8 0:ef0e3c67dc5b 6 // All Pixy source code is provided under the terms of the
swilkins8 0:ef0e3c67dc5b 7 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
swilkins8 0:ef0e3c67dc5b 8 // Those wishing to use Pixy source code, software and/or
swilkins8 0:ef0e3c67dc5b 9 // technologies under different licensing terms should contact us at
swilkins8 0:ef0e3c67dc5b 10 // cmucam@cs.cmu.edu. Such licensing terms are available for
swilkins8 0:ef0e3c67dc5b 11 // all portions of the Pixy codebase presented here.
swilkins8 0:ef0e3c67dc5b 12 //
swilkins8 0:ef0e3c67dc5b 13 // end license header
swilkins8 0:ef0e3c67dc5b 14 //
swilkins8 0:ef0e3c67dc5b 15 // This file is for defining the Block struct and the Pixy template class.
swilkins8 0:ef0e3c67dc5b 16 // (TPixy). TPixy takes a communication link as a template parameter so that
swilkins8 0:ef0e3c67dc5b 17 // all communication modes (SPI, I2C and UART) can share the same code.
swilkins8 0:ef0e3c67dc5b 18 //
swilkins8 0:ef0e3c67dc5b 19
swilkins8 0:ef0e3c67dc5b 20 #ifndef _TPIXY_H
swilkins8 0:ef0e3c67dc5b 21 #define _TPIXY_H
swilkins8 0:ef0e3c67dc5b 22
swilkins8 0:ef0e3c67dc5b 23 #include "mbed.h"
swilkins8 0:ef0e3c67dc5b 24 #include "TPixyInterface.h"
swilkins8 0:ef0e3c67dc5b 25
swilkins8 0:ef0e3c67dc5b 26 // Communication/misc parameters
swilkins8 0:ef0e3c67dc5b 27 #define PIXY_INITIAL_ARRAYSIZE 30
swilkins8 0:ef0e3c67dc5b 28 #define PIXY_MAXIMUM_ARRAYSIZE 130
swilkins8 0:ef0e3c67dc5b 29 #define PIXY_START_WORD 0xaa55
swilkins8 0:ef0e3c67dc5b 30 #define PIXY_START_WORD_CC 0xaa56
swilkins8 0:ef0e3c67dc5b 31 #define PIXY_START_WORDX 0x55aa
swilkins8 0:ef0e3c67dc5b 32 #define PIXY_MAX_SIGNATURE 7
swilkins8 0:ef0e3c67dc5b 33 #define PIXY_DEFAULT_ARGVAL 0xffff
swilkins8 0:ef0e3c67dc5b 34
swilkins8 0:ef0e3c67dc5b 35 // Pixy x-y position values
swilkins8 0:ef0e3c67dc5b 36 #define PIXY_MIN_X 0L
swilkins8 0:ef0e3c67dc5b 37 #define PIXY_MAX_X 319L
swilkins8 0:ef0e3c67dc5b 38 #define PIXY_MIN_Y 0L
swilkins8 0:ef0e3c67dc5b 39 #define PIXY_MAX_Y 199L
swilkins8 0:ef0e3c67dc5b 40
swilkins8 0:ef0e3c67dc5b 41 // RC-servo values
swilkins8 0:ef0e3c67dc5b 42 #define PIXY_RCS_MIN_POS 0L
swilkins8 0:ef0e3c67dc5b 43 #define PIXY_RCS_MAX_POS 1000L
swilkins8 0:ef0e3c67dc5b 44 #define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2)
swilkins8 0:ef0e3c67dc5b 45
swilkins8 0:ef0e3c67dc5b 46 enum BlockType {
swilkins8 0:ef0e3c67dc5b 47 NORMAL_BLOCK,
swilkins8 0:ef0e3c67dc5b 48 CC_BLOCK
swilkins8 0:ef0e3c67dc5b 49 };
swilkins8 0:ef0e3c67dc5b 50
swilkins8 0:ef0e3c67dc5b 51 struct Block {
swilkins8 0:ef0e3c67dc5b 52 // print block structure!
swilkins8 0:ef0e3c67dc5b 53 uint16_t signature;
swilkins8 0:ef0e3c67dc5b 54 uint16_t x;
swilkins8 0:ef0e3c67dc5b 55 uint16_t y;
swilkins8 0:ef0e3c67dc5b 56 uint16_t width;
swilkins8 0:ef0e3c67dc5b 57 uint16_t height;
swilkins8 0:ef0e3c67dc5b 58 uint16_t angle;
swilkins8 0:ef0e3c67dc5b 59 };
swilkins8 0:ef0e3c67dc5b 60
swilkins8 0:ef0e3c67dc5b 61 template <class TPixyInterface> class TPixy
swilkins8 0:ef0e3c67dc5b 62 {
swilkins8 0:ef0e3c67dc5b 63 public:
swilkins8 0:ef0e3c67dc5b 64 Serial* serial;
swilkins8 0:ef0e3c67dc5b 65 Block *blocks;
swilkins8 0:ef0e3c67dc5b 66
swilkins8 0:ef0e3c67dc5b 67 TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL);
swilkins8 0:ef0e3c67dc5b 68 ~TPixy();
swilkins8 0:ef0e3c67dc5b 69
swilkins8 0:ef0e3c67dc5b 70 void printBlock(Block &);
swilkins8 0:ef0e3c67dc5b 71 uint16_t getBlocks(uint16_t maxBlocks = 1000);
swilkins8 0:ef0e3c67dc5b 72 int8_t setServos(uint16_t s0, uint16_t s1);
swilkins8 0:ef0e3c67dc5b 73 int8_t setBrightness(uint8_t brightness);
swilkins8 0:ef0e3c67dc5b 74 int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
swilkins8 0:ef0e3c67dc5b 75 void init();
swilkins8 0:ef0e3c67dc5b 76
swilkins8 0:ef0e3c67dc5b 77 private:
swilkins8 0:ef0e3c67dc5b 78 TPixyInterface* link;
swilkins8 0:ef0e3c67dc5b 79 BlockType blockType;
swilkins8 0:ef0e3c67dc5b 80 bool getStart();
swilkins8 0:ef0e3c67dc5b 81 void resize();
swilkins8 0:ef0e3c67dc5b 82 bool skipStart;
swilkins8 0:ef0e3c67dc5b 83 uint16_t blockCount;
swilkins8 0:ef0e3c67dc5b 84 uint16_t blockArraySize;
swilkins8 0:ef0e3c67dc5b 85 };
swilkins8 0:ef0e3c67dc5b 86
swilkins8 0:ef0e3c67dc5b 87 template <class TPixyInterface> void TPixy<TPixyInterface>::init()
swilkins8 0:ef0e3c67dc5b 88 {
swilkins8 0:ef0e3c67dc5b 89 link->init();
swilkins8 0:ef0e3c67dc5b 90 }
swilkins8 0:ef0e3c67dc5b 91
swilkins8 0:ef0e3c67dc5b 92
swilkins8 0:ef0e3c67dc5b 93 template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type)
swilkins8 0:ef0e3c67dc5b 94 {
swilkins8 0:ef0e3c67dc5b 95 skipStart = false;
swilkins8 0:ef0e3c67dc5b 96 blockCount = 0;
swilkins8 0:ef0e3c67dc5b 97 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
swilkins8 0:ef0e3c67dc5b 98 blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
swilkins8 0:ef0e3c67dc5b 99 link->setArg(arg);
swilkins8 0:ef0e3c67dc5b 100 }
swilkins8 0:ef0e3c67dc5b 101
swilkins8 0:ef0e3c67dc5b 102
swilkins8 0:ef0e3c67dc5b 103
swilkins8 0:ef0e3c67dc5b 104 template <class TPixyInterface> void TPixy<TPixyInterface>::printBlock(Block &block)
swilkins8 0:ef0e3c67dc5b 105 {
swilkins8 0:ef0e3c67dc5b 106 int i, j;
swilkins8 0:ef0e3c67dc5b 107 char sig[6], d;
swilkins8 0:ef0e3c67dc5b 108 bool flag;
swilkins8 0:ef0e3c67dc5b 109 if (block.signature > PIXY_MAX_SIGNATURE) { // color code! (CC)
swilkins8 0:ef0e3c67dc5b 110 // convert signature number to an octal string
swilkins8 0:ef0e3c67dc5b 111 for (i = 12, j = 0, flag = false; i >= 0; i -= 3) {
swilkins8 0:ef0e3c67dc5b 112 d = (block.signature >> i) & 0x07;
swilkins8 0:ef0e3c67dc5b 113 if (d > 0 && !flag) {
swilkins8 0:ef0e3c67dc5b 114 flag = true;
swilkins8 0:ef0e3c67dc5b 115 }
swilkins8 0:ef0e3c67dc5b 116 if (flag) {
swilkins8 0:ef0e3c67dc5b 117 sig[j++] = d + '0';
swilkins8 0:ef0e3c67dc5b 118 }
swilkins8 0:ef0e3c67dc5b 119 }
swilkins8 0:ef0e3c67dc5b 120 sig[j] = '\0';
swilkins8 0:ef0e3c67dc5b 121 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 122 serial->printf("CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d\n", sig, block.signature, block.x, block.y, block.width, block.height, block.angle);
swilkins8 0:ef0e3c67dc5b 123 }
swilkins8 0:ef0e3c67dc5b 124 } else {}// regular block. Note, angle is always zero, so no need to print
swilkins8 0:ef0e3c67dc5b 125 serial->printf("sig: %d x: %d y: %d width: %d height: %d\n", block.signature, block.x, block.y, block.width, block.height);
swilkins8 0:ef0e3c67dc5b 126 }
swilkins8 0:ef0e3c67dc5b 127
swilkins8 0:ef0e3c67dc5b 128 template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy()
swilkins8 0:ef0e3c67dc5b 129 {
swilkins8 0:ef0e3c67dc5b 130 free(blocks);
swilkins8 0:ef0e3c67dc5b 131 }
swilkins8 0:ef0e3c67dc5b 132
swilkins8 0:ef0e3c67dc5b 133 template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart()
swilkins8 0:ef0e3c67dc5b 134 {
swilkins8 0:ef0e3c67dc5b 135 uint16_t w, lastw;
swilkins8 0:ef0e3c67dc5b 136 lastw = 0xffff;
swilkins8 0:ef0e3c67dc5b 137 while(true) {
swilkins8 0:ef0e3c67dc5b 138 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 139 if (w == 0 && lastw == 0) {
swilkins8 0:ef0e3c67dc5b 140 wait_ms(10);
swilkins8 0:ef0e3c67dc5b 141 return false;
swilkins8 0:ef0e3c67dc5b 142 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 143 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 144 return true;
swilkins8 0:ef0e3c67dc5b 145 } else if (w == PIXY_START_WORD_CC && lastw == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 146 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 147 return true;
swilkins8 0:ef0e3c67dc5b 148 } else if (w==PIXY_START_WORDX) {
swilkins8 0:ef0e3c67dc5b 149 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 150 serial->printf("reorder");
swilkins8 0:ef0e3c67dc5b 151 }
swilkins8 0:ef0e3c67dc5b 152 link->getByte(); // resync
swilkins8 0:ef0e3c67dc5b 153 }
swilkins8 0:ef0e3c67dc5b 154 lastw = w;
swilkins8 0:ef0e3c67dc5b 155 }
swilkins8 0:ef0e3c67dc5b 156 }
swilkins8 0:ef0e3c67dc5b 157
swilkins8 0:ef0e3c67dc5b 158 template <class TPixyInterface> void TPixy<TPixyInterface>::resize()
swilkins8 0:ef0e3c67dc5b 159 {
swilkins8 0:ef0e3c67dc5b 160 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
swilkins8 0:ef0e3c67dc5b 161 blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
swilkins8 0:ef0e3c67dc5b 162 }
swilkins8 0:ef0e3c67dc5b 163
swilkins8 0:ef0e3c67dc5b 164 template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::getBlocks(uint16_t maxBlocks)
swilkins8 0:ef0e3c67dc5b 165 {
swilkins8 0:ef0e3c67dc5b 166 uint8_t i;
swilkins8 0:ef0e3c67dc5b 167 uint16_t w, checksum, sum;
swilkins8 0:ef0e3c67dc5b 168 Block *block;
swilkins8 0:ef0e3c67dc5b 169
swilkins8 0:ef0e3c67dc5b 170 if (!skipStart) {
swilkins8 0:ef0e3c67dc5b 171 if (getStart() == false) {
swilkins8 0:ef0e3c67dc5b 172 return 0;
swilkins8 0:ef0e3c67dc5b 173 }
swilkins8 0:ef0e3c67dc5b 174 } else {
swilkins8 0:ef0e3c67dc5b 175 skipStart = false;
swilkins8 0:ef0e3c67dc5b 176 }
swilkins8 0:ef0e3c67dc5b 177 for(blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
swilkins8 0:ef0e3c67dc5b 178 checksum = link->getWord();
swilkins8 0:ef0e3c67dc5b 179 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
swilkins8 0:ef0e3c67dc5b 180 skipStart = true;
swilkins8 0:ef0e3c67dc5b 181 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 182 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 183 serial->printf("skip");
swilkins8 0:ef0e3c67dc5b 184 }
swilkins8 0:ef0e3c67dc5b 185 return blockCount;
swilkins8 0:ef0e3c67dc5b 186 } else if (checksum == PIXY_START_WORD_CC) {
swilkins8 0:ef0e3c67dc5b 187 skipStart = true;
swilkins8 0:ef0e3c67dc5b 188 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 189 return blockCount;
swilkins8 0:ef0e3c67dc5b 190 } else if (checksum == 0) {
swilkins8 0:ef0e3c67dc5b 191 return blockCount;
swilkins8 0:ef0e3c67dc5b 192 }
swilkins8 0:ef0e3c67dc5b 193 if (blockCount > blockArraySize) {
swilkins8 0:ef0e3c67dc5b 194 resize();
swilkins8 0:ef0e3c67dc5b 195 }
swilkins8 0:ef0e3c67dc5b 196 block = blocks + blockCount;
swilkins8 0:ef0e3c67dc5b 197
swilkins8 0:ef0e3c67dc5b 198 for (i = 0, sum = 0; i < sizeof(Block)/sizeof(uint16_t); i++) {
swilkins8 0:ef0e3c67dc5b 199 if (blockType == NORMAL_BLOCK && i >= 5) { // skip
swilkins8 0:ef0e3c67dc5b 200 block->angle = 0;
swilkins8 0:ef0e3c67dc5b 201 break;
swilkins8 0:ef0e3c67dc5b 202 }
swilkins8 0:ef0e3c67dc5b 203 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 204 sum += w;
swilkins8 0:ef0e3c67dc5b 205 *((uint16_t *)block + i) = w;
swilkins8 0:ef0e3c67dc5b 206 }
swilkins8 0:ef0e3c67dc5b 207
swilkins8 0:ef0e3c67dc5b 208 if (checksum == sum) {
swilkins8 0:ef0e3c67dc5b 209 blockCount++;
swilkins8 0:ef0e3c67dc5b 210 } else {
swilkins8 0:ef0e3c67dc5b 211 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 212 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 213 serial->printf("cs error");
swilkins8 0:ef0e3c67dc5b 214 }
swilkins8 0:ef0e3c67dc5b 215 }
swilkins8 0:ef0e3c67dc5b 216 if (w == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 217 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 218 } else if (w == PIXY_START_WORD_CC) {
swilkins8 0:ef0e3c67dc5b 219 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 220 } else {
swilkins8 0:ef0e3c67dc5b 221 return blockCount;
swilkins8 0:ef0e3c67dc5b 222 }
swilkins8 0:ef0e3c67dc5b 223 }
swilkins8 0:ef0e3c67dc5b 224 return blockCount;
swilkins8 0:ef0e3c67dc5b 225 }
swilkins8 0:ef0e3c67dc5b 226
swilkins8 0:ef0e3c67dc5b 227 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setServos(uint16_t s0, uint16_t s1)
swilkins8 0:ef0e3c67dc5b 228 {
swilkins8 0:ef0e3c67dc5b 229 uint8_t outBuf[6];
swilkins8 0:ef0e3c67dc5b 230 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 231 outBuf[1] = 0xff;
swilkins8 0:ef0e3c67dc5b 232 *(uint16_t *)(outBuf + 2) = s0;
swilkins8 0:ef0e3c67dc5b 233 *(uint16_t *)(outBuf + 4) = s1;
swilkins8 0:ef0e3c67dc5b 234 return link->send(outBuf, 6);
swilkins8 0:ef0e3c67dc5b 235 }
swilkins8 0:ef0e3c67dc5b 236
swilkins8 0:ef0e3c67dc5b 237 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setBrightness(uint8_t brightness)
swilkins8 0:ef0e3c67dc5b 238 {
swilkins8 0:ef0e3c67dc5b 239 uint8_t outBuf[3];
swilkins8 0:ef0e3c67dc5b 240 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 241 outBuf[1] = 0xfe;
swilkins8 0:ef0e3c67dc5b 242 outBuf[2] = brightness;
swilkins8 0:ef0e3c67dc5b 243 return link->send(outBuf, 3);
swilkins8 0:ef0e3c67dc5b 244 }
swilkins8 0:ef0e3c67dc5b 245
swilkins8 0:ef0e3c67dc5b 246 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setLED(uint8_t r, uint8_t g, uint8_t b)
swilkins8 0:ef0e3c67dc5b 247 {
swilkins8 0:ef0e3c67dc5b 248 uint8_t outBuf[5];
swilkins8 0:ef0e3c67dc5b 249 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 250 outBuf[1] = 0xfd;
swilkins8 0:ef0e3c67dc5b 251 outBuf[2] = r;
swilkins8 0:ef0e3c67dc5b 252 outBuf[3] = g;
swilkins8 0:ef0e3c67dc5b 253 outBuf[4] = b;
swilkins8 0:ef0e3c67dc5b 254 return link->send(outBuf, 5);
swilkins8 0:ef0e3c67dc5b 255 }
swilkins8 0:ef0e3c67dc5b 256
swilkins8 0:ef0e3c67dc5b 257 #endif