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