Stephen Wilkins / TPixy-Interface

A library for interfacing with the Pixy color recognition camera

Dependents:   PixyCamera MbedOS_Robot_Team ManualControlFinal PlayBack ... more

TPixy.h@3:66df7d295245, 2016-03-14 (annotated)

Committer:
swilkins8
Date:
Mon Mar 14 18:53:43 2016 +0000
Revision:
3:66df7d295245
Parent:
2:fa582d9d91b5 
Completed comments/documentation for all classes

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 3:66df7d295245 51 /** A structure for containing Block data
swilkins8 3:66df7d295245 52 * signature the signature of the block
swilkins8 3:66df7d295245 53 * x the x position of the block on the screen
swilkins8 3:66df7d295245 54 * y the y position of the block on the screen
swilkins8 3:66df7d295245 55 * width the width of the block on the screen
swilkins8 3:66df7d295245 56 * height the height of the block on the screen
swilkins8 3:66df7d295245 57 * angle the angle of the block
swilkins8 3:66df7d295245 58 */
swilkins8 0:ef0e3c67dc5b 59 struct Block {
swilkins8 0:ef0e3c67dc5b 60 uint16_t signature;
swilkins8 0:ef0e3c67dc5b 61 uint16_t x;
swilkins8 0:ef0e3c67dc5b 62 uint16_t y;
swilkins8 0:ef0e3c67dc5b 63 uint16_t width;
swilkins8 0:ef0e3c67dc5b 64 uint16_t height;
swilkins8 0:ef0e3c67dc5b 65 uint16_t angle;
swilkins8 0:ef0e3c67dc5b 66 };
swilkins8 0:ef0e3c67dc5b 67
swilkins8 2:fa582d9d91b5 68 /** The superclass handler for the Pixy camera from a given connection handler (SPI, I2C, etc)
swilkins8 2:fa582d9d91b5 69 * Used for instantiating a Pixy object that interfaces via an abtritrary connection handler
swilkins8 2:fa582d9d91b5 70 */
swilkins8 0:ef0e3c67dc5b 71 template <class TPixyInterface> class TPixy
swilkins8 0:ef0e3c67dc5b 72 {
swilkins8 0:ef0e3c67dc5b 73 public:
swilkins8 0:ef0e3c67dc5b 74 Serial* serial;
swilkins8 0:ef0e3c67dc5b 75 Block *blocks;
swilkins8 0:ef0e3c67dc5b 76
swilkins8 3:66df7d295245 77 /** Creates a TPixy object with a given connection handler
swilkins8 3:66df7d295245 78 * @param type the pointer to the interface connection handler
swilkins8 3:66df7d295245 79 * @param serialOut the optional serial output pointer to print out Pixy camera data
swilkins8 3:66df7d295245 80 * @param arg an optional integer argument used for custom implementations of the Pixy library
swilkins8 2:fa582d9d91b5 81 */
swilkins8 0:ef0e3c67dc5b 82 TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL);
swilkins8 0:ef0e3c67dc5b 83 ~TPixy();
swilkins8 0:ef0e3c67dc5b 84
swilkins8 3:66df7d295245 85 /** Prints the given Block out to the given serial port
swilkins8 3:66df7d295245 86 * @param &block the Block to print
swilkins8 3:66df7d295245 87 */
swilkins8 3:66df7d295245 88 void printBlock(Block &block);
swilkins8 3:66df7d295245 89 /** Returns the number of Blocks found in the current view and updates all Blocks' info
swilkins8 3:66df7d295245 90 * @param maxBlocks the maximum number of Blocks to search for
swilkins8 3:66df7d295245 91 * @return the number of Blocks found
swilkins8 3:66df7d295245 92 */
swilkins8 3:66df7d295245 93 uint16_t getBlocks(uint16_t maxBlocks = 1000);
swilkins8 3:66df7d295245 94 /** Sets the PWM value of the Pixy servos
swilkins8 3:66df7d295245 95 * @param s0 the value of the left servo
swilkins8 3:66df7d295245 96 * @param s1 the value of the right servo
swilkins8 3:66df7d295245 97 * @return the interface return value for sending the servo command
swilkins8 2:fa582d9d91b5 98 */
swilkins8 0:ef0e3c67dc5b 99 int8_t setServos(uint16_t s0, uint16_t s1);
swilkins8 3:66df7d295245 100 /** Sets the brightness of the Pixy RGB LED
swilkins8 3:66df7d295245 101 * @param brightness the brightness of the LED
swilkins8 3:66df7d295245 102 * @return the interface return value for sending the brightness command
swilkins8 3:66df7d295245 103 */
swilkins8 0:ef0e3c67dc5b 104 int8_t setBrightness(uint8_t brightness);
swilkins8 3:66df7d295245 105 /** Sets the color of the Pixy RGB LED
swilkins8 3:66df7d295245 106 * @param r the red color value
swilkins8 3:66df7d295245 107 * @param g the green color value
swilkins8 3:66df7d295245 108 * @param b the blue color value
swilkins8 3:66df7d295245 109 */
swilkins8 0:ef0e3c67dc5b 110 int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
swilkins8 3:66df7d295245 111 /** Initializes the Pixy
swilkins8 3:66df7d295245 112 */
swilkins8 0:ef0e3c67dc5b 113 void init();
swilkins8 0:ef0e3c67dc5b 114
swilkins8 0:ef0e3c67dc5b 115 private:
swilkins8 0:ef0e3c67dc5b 116 TPixyInterface* link;
swilkins8 0:ef0e3c67dc5b 117 BlockType blockType;
swilkins8 3:66df7d295245 118 /** Returns if the Pixy is ready to receive/send a message
swilkins8 3:66df7d295245 119 */
swilkins8 0:ef0e3c67dc5b 120 bool getStart();
swilkins8 3:66df7d295245 121 /** Resizes the block array to add an additional block
swilkins8 3:66df7d295245 122 */
swilkins8 0:ef0e3c67dc5b 123 void resize();
swilkins8 0:ef0e3c67dc5b 124 bool skipStart;
swilkins8 0:ef0e3c67dc5b 125 uint16_t blockCount;
swilkins8 0:ef0e3c67dc5b 126 uint16_t blockArraySize;
swilkins8 0:ef0e3c67dc5b 127 };
swilkins8 0:ef0e3c67dc5b 128
swilkins8 0:ef0e3c67dc5b 129 template <class TPixyInterface> void TPixy<TPixyInterface>::init()
swilkins8 0:ef0e3c67dc5b 130 {
swilkins8 0:ef0e3c67dc5b 131 link->init();
swilkins8 0:ef0e3c67dc5b 132 }
swilkins8 0:ef0e3c67dc5b 133
swilkins8 0:ef0e3c67dc5b 134
swilkins8 0:ef0e3c67dc5b 135 template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type)
swilkins8 0:ef0e3c67dc5b 136 {
swilkins8 0:ef0e3c67dc5b 137 skipStart = false;
swilkins8 0:ef0e3c67dc5b 138 blockCount = 0;
swilkins8 0:ef0e3c67dc5b 139 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
swilkins8 0:ef0e3c67dc5b 140 blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
swilkins8 0:ef0e3c67dc5b 141 link->setArg(arg);
swilkins8 0:ef0e3c67dc5b 142 }
swilkins8 0:ef0e3c67dc5b 143
swilkins8 0:ef0e3c67dc5b 144 template <class TPixyInterface> void TPixy<TPixyInterface>::printBlock(Block &block)
swilkins8 0:ef0e3c67dc5b 145 {
swilkins8 0:ef0e3c67dc5b 146 int i, j;
swilkins8 0:ef0e3c67dc5b 147 char sig[6], d;
swilkins8 0:ef0e3c67dc5b 148 bool flag;
swilkins8 0:ef0e3c67dc5b 149 if (block.signature > PIXY_MAX_SIGNATURE) { // color code! (CC)
swilkins8 0:ef0e3c67dc5b 150 // convert signature number to an octal string
swilkins8 0:ef0e3c67dc5b 151 for (i = 12, j = 0, flag = false; i >= 0; i -= 3) {
swilkins8 0:ef0e3c67dc5b 152 d = (block.signature >> i) & 0x07;
swilkins8 0:ef0e3c67dc5b 153 if (d > 0 && !flag) {
swilkins8 0:ef0e3c67dc5b 154 flag = true;
swilkins8 0:ef0e3c67dc5b 155 }
swilkins8 0:ef0e3c67dc5b 156 if (flag) {
swilkins8 0:ef0e3c67dc5b 157 sig[j++] = d + '0';
swilkins8 0:ef0e3c67dc5b 158 }
swilkins8 0:ef0e3c67dc5b 159 }
swilkins8 0:ef0e3c67dc5b 160 sig[j] = '\0';
swilkins8 0:ef0e3c67dc5b 161 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 162 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 163 }
swilkins8 0:ef0e3c67dc5b 164 } else {}// regular block. Note, angle is always zero, so no need to print
swilkins8 0:ef0e3c67dc5b 165 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 166 }
swilkins8 0:ef0e3c67dc5b 167
swilkins8 0:ef0e3c67dc5b 168 template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy()
swilkins8 0:ef0e3c67dc5b 169 {
swilkins8 0:ef0e3c67dc5b 170 free(blocks);
swilkins8 0:ef0e3c67dc5b 171 }
swilkins8 0:ef0e3c67dc5b 172
swilkins8 0:ef0e3c67dc5b 173 template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart()
swilkins8 0:ef0e3c67dc5b 174 {
swilkins8 0:ef0e3c67dc5b 175 uint16_t w, lastw;
swilkins8 0:ef0e3c67dc5b 176 lastw = 0xffff;
swilkins8 0:ef0e3c67dc5b 177 while(true) {
swilkins8 0:ef0e3c67dc5b 178 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 179 if (w == 0 && lastw == 0) {
swilkins8 0:ef0e3c67dc5b 180 wait_ms(10);
swilkins8 0:ef0e3c67dc5b 181 return false;
swilkins8 0:ef0e3c67dc5b 182 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 183 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 184 return true;
swilkins8 0:ef0e3c67dc5b 185 } else if (w == PIXY_START_WORD_CC && lastw == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 186 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 187 return true;
swilkins8 3:66df7d295245 188 } else if (w == PIXY_START_WORDX) {
swilkins8 0:ef0e3c67dc5b 189 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 190 serial->printf("reorder");
swilkins8 0:ef0e3c67dc5b 191 }
swilkins8 0:ef0e3c67dc5b 192 link->getByte(); // resync
swilkins8 0:ef0e3c67dc5b 193 }
swilkins8 0:ef0e3c67dc5b 194 lastw = w;
swilkins8 0:ef0e3c67dc5b 195 }
swilkins8 0:ef0e3c67dc5b 196 }
swilkins8 0:ef0e3c67dc5b 197
swilkins8 0:ef0e3c67dc5b 198 template <class TPixyInterface> void TPixy<TPixyInterface>::resize()
swilkins8 0:ef0e3c67dc5b 199 {
swilkins8 0:ef0e3c67dc5b 200 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
swilkins8 0:ef0e3c67dc5b 201 blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
swilkins8 0:ef0e3c67dc5b 202 }
swilkins8 0:ef0e3c67dc5b 203
swilkins8 0:ef0e3c67dc5b 204 template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::getBlocks(uint16_t maxBlocks)
swilkins8 0:ef0e3c67dc5b 205 {
swilkins8 0:ef0e3c67dc5b 206 uint8_t i;
swilkins8 0:ef0e3c67dc5b 207 uint16_t w, checksum, sum;
swilkins8 0:ef0e3c67dc5b 208 Block *block;
swilkins8 0:ef0e3c67dc5b 209
swilkins8 0:ef0e3c67dc5b 210 if (!skipStart) {
swilkins8 0:ef0e3c67dc5b 211 if (getStart() == false) {
swilkins8 0:ef0e3c67dc5b 212 return 0;
swilkins8 0:ef0e3c67dc5b 213 }
swilkins8 0:ef0e3c67dc5b 214 } else {
swilkins8 0:ef0e3c67dc5b 215 skipStart = false;
swilkins8 0:ef0e3c67dc5b 216 }
swilkins8 0:ef0e3c67dc5b 217 for(blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
swilkins8 0:ef0e3c67dc5b 218 checksum = link->getWord();
swilkins8 0:ef0e3c67dc5b 219 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
swilkins8 0:ef0e3c67dc5b 220 skipStart = true;
swilkins8 0:ef0e3c67dc5b 221 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 222 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 223 serial->printf("skip");
swilkins8 0:ef0e3c67dc5b 224 }
swilkins8 0:ef0e3c67dc5b 225 return blockCount;
swilkins8 0:ef0e3c67dc5b 226 } else if (checksum == PIXY_START_WORD_CC) {
swilkins8 0:ef0e3c67dc5b 227 skipStart = true;
swilkins8 0:ef0e3c67dc5b 228 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 229 return blockCount;
swilkins8 0:ef0e3c67dc5b 230 } else if (checksum == 0) {
swilkins8 0:ef0e3c67dc5b 231 return blockCount;
swilkins8 0:ef0e3c67dc5b 232 }
swilkins8 0:ef0e3c67dc5b 233 if (blockCount > blockArraySize) {
swilkins8 0:ef0e3c67dc5b 234 resize();
swilkins8 0:ef0e3c67dc5b 235 }
swilkins8 0:ef0e3c67dc5b 236 block = blocks + blockCount;
swilkins8 0:ef0e3c67dc5b 237
swilkins8 0:ef0e3c67dc5b 238 for (i = 0, sum = 0; i < sizeof(Block)/sizeof(uint16_t); i++) {
swilkins8 0:ef0e3c67dc5b 239 if (blockType == NORMAL_BLOCK && i >= 5) { // skip
swilkins8 0:ef0e3c67dc5b 240 block->angle = 0;
swilkins8 0:ef0e3c67dc5b 241 break;
swilkins8 0:ef0e3c67dc5b 242 }
swilkins8 0:ef0e3c67dc5b 243 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 244 sum += w;
swilkins8 0:ef0e3c67dc5b 245 *((uint16_t *)block + i) = w;
swilkins8 0:ef0e3c67dc5b 246 }
swilkins8 0:ef0e3c67dc5b 247
swilkins8 0:ef0e3c67dc5b 248 if (checksum == sum) {
swilkins8 0:ef0e3c67dc5b 249 blockCount++;
swilkins8 0:ef0e3c67dc5b 250 } else {
swilkins8 0:ef0e3c67dc5b 251 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 252 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 253 serial->printf("cs error");
swilkins8 0:ef0e3c67dc5b 254 }
swilkins8 0:ef0e3c67dc5b 255 }
swilkins8 0:ef0e3c67dc5b 256 if (w == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 257 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 258 } else if (w == PIXY_START_WORD_CC) {
swilkins8 0:ef0e3c67dc5b 259 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 260 } else {
swilkins8 0:ef0e3c67dc5b 261 return blockCount;
swilkins8 0:ef0e3c67dc5b 262 }
swilkins8 0:ef0e3c67dc5b 263 }
swilkins8 0:ef0e3c67dc5b 264 return blockCount;
swilkins8 0:ef0e3c67dc5b 265 }
swilkins8 0:ef0e3c67dc5b 266
swilkins8 0:ef0e3c67dc5b 267 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setServos(uint16_t s0, uint16_t s1)
swilkins8 0:ef0e3c67dc5b 268 {
swilkins8 0:ef0e3c67dc5b 269 uint8_t outBuf[6];
swilkins8 0:ef0e3c67dc5b 270 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 271 outBuf[1] = 0xff;
swilkins8 0:ef0e3c67dc5b 272 *(uint16_t *)(outBuf + 2) = s0;
swilkins8 0:ef0e3c67dc5b 273 *(uint16_t *)(outBuf + 4) = s1;
swilkins8 0:ef0e3c67dc5b 274 return link->send(outBuf, 6);
swilkins8 0:ef0e3c67dc5b 275 }
swilkins8 0:ef0e3c67dc5b 276
swilkins8 0:ef0e3c67dc5b 277 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setBrightness(uint8_t brightness)
swilkins8 0:ef0e3c67dc5b 278 {
swilkins8 0:ef0e3c67dc5b 279 uint8_t outBuf[3];
swilkins8 0:ef0e3c67dc5b 280 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 281 outBuf[1] = 0xfe;
swilkins8 0:ef0e3c67dc5b 282 outBuf[2] = brightness;
swilkins8 0:ef0e3c67dc5b 283 return link->send(outBuf, 3);
swilkins8 0:ef0e3c67dc5b 284 }
swilkins8 0:ef0e3c67dc5b 285
swilkins8 0:ef0e3c67dc5b 286 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setLED(uint8_t r, uint8_t g, uint8_t b)
swilkins8 0:ef0e3c67dc5b 287 {
swilkins8 0:ef0e3c67dc5b 288 uint8_t outBuf[5];
swilkins8 0:ef0e3c67dc5b 289 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 290 outBuf[1] = 0xfd;
swilkins8 0:ef0e3c67dc5b 291 outBuf[2] = r;
swilkins8 0:ef0e3c67dc5b 292 outBuf[3] = g;
swilkins8 0:ef0e3c67dc5b 293 outBuf[4] = b;
swilkins8 0:ef0e3c67dc5b 294 return link->send(outBuf, 5);
swilkins8 0:ef0e3c67dc5b 295 }
swilkins8 0:ef0e3c67dc5b 296
swilkins8 0:ef0e3c67dc5b 297 #endif