A library for interfacing with the Pixy color recognition camera
Dependents: PixyCamera MbedOS_Robot_Team ManualControlFinal PlayBack ... more
TPixy.h
00001 // 00002 // begin license header 00003 // 00004 // This file is part of Pixy CMUcam5 or "Pixy" for short 00005 // 00006 // All Pixy source code is provided under the terms of the 00007 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). 00008 // Those wishing to use Pixy source code, software and/or 00009 // technologies under different licensing terms should contact us at 00010 // cmucam@cs.cmu.edu. Such licensing terms are available for 00011 // all portions of the Pixy codebase presented here. 00012 // 00013 // end license header 00014 // 00015 // This file is for defining the Block struct and the Pixy template class. 00016 // (TPixy). TPixy takes a communication link as a template parameter so that 00017 // all communication modes (SPI, I2C and UART) can share the same code. 00018 // 00019 00020 #ifndef _TPIXY_H 00021 #define _TPIXY_H 00022 00023 #include "mbed.h" 00024 #include "TPixyInterface.h" 00025 00026 // Communication/misc parameters 00027 #define PIXY_INITIAL_ARRAYSIZE 30 00028 #define PIXY_MAXIMUM_ARRAYSIZE 130 00029 #define PIXY_START_WORD 0xaa55 00030 #define PIXY_START_WORD_CC 0xaa56 00031 #define PIXY_START_WORDX 0x55aa 00032 #define PIXY_MAX_SIGNATURE 7 00033 #define PIXY_DEFAULT_ARGVAL 0xffff 00034 00035 // Pixy x-y position values 00036 #define PIXY_MIN_X 0L 00037 #define PIXY_MAX_X 319L 00038 #define PIXY_MIN_Y 0L 00039 #define PIXY_MAX_Y 199L 00040 00041 // RC-servo values 00042 #define PIXY_RCS_MIN_POS 0L 00043 #define PIXY_RCS_MAX_POS 1000L 00044 #define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2) 00045 00046 enum BlockType { 00047 NORMAL_BLOCK, 00048 CC_BLOCK 00049 }; 00050 00051 /** A structure for containing Block data 00052 * signature the signature of the block 00053 * x the x position of the block on the screen 00054 * y the y position of the block on the screen 00055 * width the width of the block on the screen 00056 * height the height of the block on the screen 00057 * angle the angle of the block 00058 */ 00059 struct Block { 00060 uint16_t signature; 00061 uint16_t x; 00062 uint16_t y; 00063 uint16_t width; 00064 uint16_t height; 00065 uint16_t angle; 00066 }; 00067 00068 /** The superclass handler for the Pixy camera from a given connection handler (SPI, I2C, etc) 00069 * Used for instantiating a Pixy object that interfaces via an abtritrary connection handler 00070 */ 00071 template <class TPixyInterface> class TPixy 00072 { 00073 public: 00074 Serial* serial; 00075 Block *blocks; 00076 00077 /** Creates a TPixy object with a given connection handler 00078 * @param type the pointer to the interface connection handler 00079 * @param serialOut the optional serial output pointer to print out Pixy camera data 00080 * @param arg an optional integer argument used for custom implementations of the Pixy library 00081 */ 00082 TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL); 00083 ~TPixy(); 00084 00085 /** Prints the given Block out to the given serial port 00086 * @param &block the Block to print 00087 */ 00088 void printBlock(Block &block); 00089 /** Returns the number of Blocks found in the current view and updates all Blocks' info 00090 * @param maxBlocks the maximum number of Blocks to search for 00091 * @return the number of Blocks found 00092 */ 00093 uint16_t getBlocks(uint16_t maxBlocks = 1000); 00094 /** Sets the PWM value of the Pixy servos 00095 * @param s0 the value of the left servo 00096 * @param s1 the value of the right servo 00097 * @return the interface return value for sending the servo command 00098 */ 00099 int8_t setServos(uint16_t s0, uint16_t s1); 00100 /** Sets the brightness of the Pixy RGB LED 00101 * @param brightness the brightness of the LED 00102 * @return the interface return value for sending the brightness command 00103 */ 00104 int8_t setBrightness(uint8_t brightness); 00105 /** Sets the color of the Pixy RGB LED 00106 * @param r the red color value 00107 * @param g the green color value 00108 * @param b the blue color value 00109 */ 00110 int8_t setLED(uint8_t r, uint8_t g, uint8_t b); 00111 /** Initializes the Pixy 00112 */ 00113 void init(); 00114 00115 private: 00116 TPixyInterface* link; 00117 BlockType blockType; 00118 /** Returns if the Pixy is ready to receive/send a message 00119 */ 00120 bool getStart(); 00121 /** Resizes the block array to add an additional block 00122 */ 00123 void resize(); 00124 bool skipStart; 00125 uint16_t blockCount; 00126 uint16_t blockArraySize; 00127 }; 00128 00129 template <class TPixyInterface> void TPixy<TPixyInterface>::init() 00130 { 00131 link->init(); 00132 } 00133 00134 00135 template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type) 00136 { 00137 skipStart = false; 00138 blockCount = 0; 00139 blockArraySize = PIXY_INITIAL_ARRAYSIZE; 00140 blocks = (Block *)malloc(sizeof(Block)*blockArraySize); 00141 link->setArg(arg); 00142 } 00143 00144 template <class TPixyInterface> void TPixy<TPixyInterface>::printBlock(Block &block) 00145 { 00146 int i, j; 00147 char sig[6], d; 00148 bool flag; 00149 if (block.signature > PIXY_MAX_SIGNATURE) { // color code! (CC) 00150 // convert signature number to an octal string 00151 for (i = 12, j = 0, flag = false; i >= 0; i -= 3) { 00152 d = (block.signature >> i) & 0x07; 00153 if (d > 0 && !flag) { 00154 flag = true; 00155 } 00156 if (flag) { 00157 sig[j++] = d + '0'; 00158 } 00159 } 00160 sig[j] = '\0'; 00161 if (serial != NULL) { 00162 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); 00163 } 00164 } else {}// regular block. Note, angle is always zero, so no need to print 00165 serial->printf("sig: %d x: %d y: %d width: %d height: %d\n", block.signature, block.x, block.y, block.width, block.height); 00166 } 00167 00168 template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy() 00169 { 00170 free(blocks); 00171 } 00172 00173 template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart() 00174 { 00175 uint16_t w, lastw; 00176 lastw = 0xffff; 00177 while(true) { 00178 w = link->getWord(); 00179 if (w == 0 && lastw == 0) { 00180 wait_ms(10); 00181 return false; 00182 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) { 00183 blockType = NORMAL_BLOCK; 00184 return true; 00185 } else if (w == PIXY_START_WORD_CC && lastw == PIXY_START_WORD) { 00186 blockType = CC_BLOCK; 00187 return true; 00188 } else if (w == PIXY_START_WORDX) { 00189 if (serial != NULL) { 00190 serial->printf("reorder"); 00191 } 00192 link->getByte(); // resync 00193 } 00194 lastw = w; 00195 } 00196 } 00197 00198 template <class TPixyInterface> void TPixy<TPixyInterface>::resize() 00199 { 00200 blockArraySize += PIXY_INITIAL_ARRAYSIZE; 00201 blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize); 00202 } 00203 00204 template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::getBlocks(uint16_t maxBlocks) 00205 { 00206 uint8_t i; 00207 uint16_t w, checksum, sum; 00208 Block *block; 00209 00210 if (!skipStart) { 00211 if (getStart() == false) { 00212 return 0; 00213 } 00214 } else { 00215 skipStart = false; 00216 } 00217 for(blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) { 00218 checksum = link->getWord(); 00219 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame 00220 skipStart = true; 00221 blockType = NORMAL_BLOCK; 00222 if (serial != NULL) { 00223 serial->printf("skip"); 00224 } 00225 return blockCount; 00226 } else if (checksum == PIXY_START_WORD_CC) { 00227 skipStart = true; 00228 blockType = CC_BLOCK; 00229 return blockCount; 00230 } else if (checksum == 0) { 00231 return blockCount; 00232 } 00233 if (blockCount > blockArraySize) { 00234 resize(); 00235 } 00236 block = blocks + blockCount; 00237 00238 for (i = 0, sum = 0; i < sizeof(Block)/sizeof(uint16_t); i++) { 00239 if (blockType == NORMAL_BLOCK && i >= 5) { // skip 00240 block->angle = 0; 00241 break; 00242 } 00243 w = link->getWord(); 00244 sum += w; 00245 *((uint16_t *)block + i) = w; 00246 } 00247 00248 if (checksum == sum) { 00249 blockCount++; 00250 } else { 00251 w = link->getWord(); 00252 if (serial != NULL) { 00253 serial->printf("cs error"); 00254 } 00255 } 00256 if (w == PIXY_START_WORD) { 00257 blockType = NORMAL_BLOCK; 00258 } else if (w == PIXY_START_WORD_CC) { 00259 blockType = CC_BLOCK; 00260 } else { 00261 return blockCount; 00262 } 00263 } 00264 return blockCount; 00265 } 00266 00267 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setServos(uint16_t s0, uint16_t s1) 00268 { 00269 uint8_t outBuf[6]; 00270 outBuf[0] = 0x00; 00271 outBuf[1] = 0xff; 00272 *(uint16_t *)(outBuf + 2) = s0; 00273 *(uint16_t *)(outBuf + 4) = s1; 00274 return link->send(outBuf, 6); 00275 } 00276 00277 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setBrightness(uint8_t brightness) 00278 { 00279 uint8_t outBuf[3]; 00280 outBuf[0] = 0x00; 00281 outBuf[1] = 0xfe; 00282 outBuf[2] = brightness; 00283 return link->send(outBuf, 3); 00284 } 00285 00286 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setLED(uint8_t r, uint8_t g, uint8_t b) 00287 { 00288 uint8_t outBuf[5]; 00289 outBuf[0] = 0x00; 00290 outBuf[1] = 0xfd; 00291 outBuf[2] = r; 00292 outBuf[3] = g; 00293 outBuf[4] = b; 00294 return link->send(outBuf, 5); 00295 } 00296 00297 #endif
Generated on Tue Jul 12 2022 18:15:33 by 1.7.2