Ruprecht Altenburger / Mbed OS TPixy-Interface

SPI read Pixi Cam V1

TPixy.h@2:fa582d9d91b5, 2016-03-14 (annotated)

Committer:
swilkins8
Date:
Mon Mar 14 17:50:40 2016 +0000
Revision:
2:fa582d9d91b5
Parent:
0:ef0e3c67dc5b
Child:
3:66df7d295245
Started comments/documentation for the TPixy class

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