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