Michel MOULIN / pixy_lib_base

cette librairie contient les fonctions de base permettant de detecter un objet avec la camera pixy

Dependents:   pixy_base

TPixy.h@0:a1e10abfbd19, 2020-11-18 (annotated)

Committer:
michelmoulin
Date:
Wed Nov 18 13:12:20 2020 +0000
Revision:
0:a1e10abfbd19
cette version de programme permet d'afficher les coordonnees x et y d'un objet detecte par la camera pixy.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
michelmoulin 0:a1e10abfbd19 1 //
michelmoulin 0:a1e10abfbd19 2 // begin license header
michelmoulin 0:a1e10abfbd19 3 //
michelmoulin 0:a1e10abfbd19 4 // This file is part of Pixy CMUcam5 or "Pixy" for short
michelmoulin 0:a1e10abfbd19 5 //
michelmoulin 0:a1e10abfbd19 6 // All Pixy source code is provided under the terms of the
michelmoulin 0:a1e10abfbd19 7 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
michelmoulin 0:a1e10abfbd19 8 // Those wishing to use Pixy source code, software and/or
michelmoulin 0:a1e10abfbd19 9 // technologies under different licensing terms should contact us at
michelmoulin 0:a1e10abfbd19 10 // cmucam@cs.cmu.edu. Such licensing terms are available for
michelmoulin 0:a1e10abfbd19 11 // all portions of the Pixy codebase presented here.
michelmoulin 0:a1e10abfbd19 12 //
michelmoulin 0:a1e10abfbd19 13 // end license header
michelmoulin 0:a1e10abfbd19 14 //
michelmoulin 0:a1e10abfbd19 15 // This file is for defining the Block struct and the Pixy template class.
michelmoulin 0:a1e10abfbd19 16 // (TPixy). TPixy takes a communication link as a template parameter so that
michelmoulin 0:a1e10abfbd19 17 // all communication modes (SPI, I2C and UART) can share the same code.
michelmoulin 0:a1e10abfbd19 18 //
michelmoulin 0:a1e10abfbd19 19
michelmoulin 0:a1e10abfbd19 20 #ifndef _TPIXY_H
michelmoulin 0:a1e10abfbd19 21 #define _TPIXY_H
michelmoulin 0:a1e10abfbd19 22
michelmoulin 0:a1e10abfbd19 23 #include "mbed.h"
michelmoulin 0:a1e10abfbd19 24 #include "TPixyInterface.h"
michelmoulin 0:a1e10abfbd19 25
michelmoulin 0:a1e10abfbd19 26 // Communication/misc parameters
michelmoulin 0:a1e10abfbd19 27 #define PIXY_INITIAL_ARRAYSIZE 30
michelmoulin 0:a1e10abfbd19 28 #define PIXY_MAXIMUM_ARRAYSIZE 130
michelmoulin 0:a1e10abfbd19 29 #define PIXY_START_WORD 0xaa55
michelmoulin 0:a1e10abfbd19 30 #define PIXY_START_WORD_CC 0xaa56
michelmoulin 0:a1e10abfbd19 31 #define PIXY_START_WORDX 0x55aa
michelmoulin 0:a1e10abfbd19 32 #define PIXY_MAX_SIGNATURE 7
michelmoulin 0:a1e10abfbd19 33 #define PIXY_DEFAULT_ARGVAL 0xffff
michelmoulin 0:a1e10abfbd19 34
michelmoulin 0:a1e10abfbd19 35 // Pixy x-y position values
michelmoulin 0:a1e10abfbd19 36 #define PIXY_MIN_X 0L
michelmoulin 0:a1e10abfbd19 37 #define PIXY_MAX_X 319L
michelmoulin 0:a1e10abfbd19 38 #define PIXY_MIN_Y 0L
michelmoulin 0:a1e10abfbd19 39 #define PIXY_MAX_Y 199L
michelmoulin 0:a1e10abfbd19 40
michelmoulin 0:a1e10abfbd19 41 // RC-servo values
michelmoulin 0:a1e10abfbd19 42 #define PIXY_RCS_MIN_POS 0L
michelmoulin 0:a1e10abfbd19 43 #define PIXY_RCS_MAX_POS 1000L
michelmoulin 0:a1e10abfbd19 44 #define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2)
michelmoulin 0:a1e10abfbd19 45
michelmoulin 0:a1e10abfbd19 46 enum BlockType {
michelmoulin 0:a1e10abfbd19 47 NORMAL_BLOCK,
michelmoulin 0:a1e10abfbd19 48 CC_BLOCK
michelmoulin 0:a1e10abfbd19 49 };
michelmoulin 0:a1e10abfbd19 50
michelmoulin 0:a1e10abfbd19 51 struct Block {
michelmoulin 0:a1e10abfbd19 52 // print block structure!
michelmoulin 0:a1e10abfbd19 53 uint16_t signature;
michelmoulin 0:a1e10abfbd19 54 uint16_t x;
michelmoulin 0:a1e10abfbd19 55 uint16_t y;
michelmoulin 0:a1e10abfbd19 56 uint16_t width;
michelmoulin 0:a1e10abfbd19 57 uint16_t height;
michelmoulin 0:a1e10abfbd19 58 uint16_t angle;
michelmoulin 0:a1e10abfbd19 59 };
michelmoulin 0:a1e10abfbd19 60
michelmoulin 0:a1e10abfbd19 61 template <class TPixyInterface> class TPixy
michelmoulin 0:a1e10abfbd19 62 {
michelmoulin 0:a1e10abfbd19 63 public:
michelmoulin 0:a1e10abfbd19 64 Serial* serial;
michelmoulin 0:a1e10abfbd19 65 Block *blocks;
michelmoulin 0:a1e10abfbd19 66
michelmoulin 0:a1e10abfbd19 67 TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL);
michelmoulin 0:a1e10abfbd19 68 ~TPixy();
michelmoulin 0:a1e10abfbd19 69
michelmoulin 0:a1e10abfbd19 70 uint16_t getBlocks(uint16_t maxBlocks = 1000);
michelmoulin 0:a1e10abfbd19 71 int8_t setBrightness(uint8_t brightness);
michelmoulin 0:a1e10abfbd19 72 int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
michelmoulin 0:a1e10abfbd19 73
michelmoulin 0:a1e10abfbd19 74 private:
michelmoulin 0:a1e10abfbd19 75 TPixyInterface* link;
michelmoulin 0:a1e10abfbd19 76 BlockType blockType;
michelmoulin 0:a1e10abfbd19 77 bool getStart();
michelmoulin 0:a1e10abfbd19 78 void resize();
michelmoulin 0:a1e10abfbd19 79 bool skipStart;
michelmoulin 0:a1e10abfbd19 80 uint16_t blockCount;
michelmoulin 0:a1e10abfbd19 81 uint16_t blockArraySize;
michelmoulin 0:a1e10abfbd19 82 };
michelmoulin 0:a1e10abfbd19 83
michelmoulin 0:a1e10abfbd19 84 template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type)
michelmoulin 0:a1e10abfbd19 85 {
michelmoulin 0:a1e10abfbd19 86 skipStart = false;
michelmoulin 0:a1e10abfbd19 87 blockCount = 0;
michelmoulin 0:a1e10abfbd19 88 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
michelmoulin 0:a1e10abfbd19 89 blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
michelmoulin 0:a1e10abfbd19 90 link->setArg(arg);
michelmoulin 0:a1e10abfbd19 91 }
michelmoulin 0:a1e10abfbd19 92
michelmoulin 0:a1e10abfbd19 93 template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy()
michelmoulin 0:a1e10abfbd19 94 {
michelmoulin 0:a1e10abfbd19 95 free(blocks);
michelmoulin 0:a1e10abfbd19 96 }
michelmoulin 0:a1e10abfbd19 97
michelmoulin 0:a1e10abfbd19 98 template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart()
michelmoulin 0:a1e10abfbd19 99 {
michelmoulin 0:a1e10abfbd19 100 uint16_t w, lastw;
michelmoulin 0:a1e10abfbd19 101 lastw = 0xffff;
michelmoulin 0:a1e10abfbd19 102 while(true) {
michelmoulin 0:a1e10abfbd19 103 w = link->getWord();
michelmoulin 0:a1e10abfbd19 104 if (w == 0 && lastw == 0) {
michelmoulin 0:a1e10abfbd19 105 wait_ms(10);
michelmoulin 0:a1e10abfbd19 106 return false;
michelmoulin 0:a1e10abfbd19 107 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) {
michelmoulin 0:a1e10abfbd19 108 blockType = NORMAL_BLOCK;
michelmoulin 0:a1e10abfbd19 109 return true;
michelmoulin 0:a1e10abfbd19 110 } else if (w == PIXY_START_WORD_CC && lastw == PIXY_START_WORD) {
michelmoulin 0:a1e10abfbd19 111 blockType = CC_BLOCK;
michelmoulin 0:a1e10abfbd19 112 return true;
michelmoulin 0:a1e10abfbd19 113 } else if (w==PIXY_START_WORDX) {
michelmoulin 0:a1e10abfbd19 114 if (serial != NULL) {
michelmoulin 0:a1e10abfbd19 115 serial->printf("reorder");
michelmoulin 0:a1e10abfbd19 116 }
michelmoulin 0:a1e10abfbd19 117 link->getByte(); // resync
michelmoulin 0:a1e10abfbd19 118 }
michelmoulin 0:a1e10abfbd19 119 lastw = w;
michelmoulin 0:a1e10abfbd19 120 }
michelmoulin 0:a1e10abfbd19 121 }
michelmoulin 0:a1e10abfbd19 122
michelmoulin 0:a1e10abfbd19 123 template <class TPixyInterface> void TPixy<TPixyInterface>::resize()
michelmoulin 0:a1e10abfbd19 124 {
michelmoulin 0:a1e10abfbd19 125 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
michelmoulin 0:a1e10abfbd19 126 blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
michelmoulin 0:a1e10abfbd19 127 }
michelmoulin 0:a1e10abfbd19 128
michelmoulin 0:a1e10abfbd19 129 template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::getBlocks(uint16_t maxBlocks)
michelmoulin 0:a1e10abfbd19 130 {
michelmoulin 0:a1e10abfbd19 131 uint8_t i;
michelmoulin 0:a1e10abfbd19 132 uint16_t w, checksum, sum;
michelmoulin 0:a1e10abfbd19 133 Block *block;
michelmoulin 0:a1e10abfbd19 134
michelmoulin 0:a1e10abfbd19 135 if (!skipStart) {
michelmoulin 0:a1e10abfbd19 136 if (getStart() == false) {
michelmoulin 0:a1e10abfbd19 137 return 0;
michelmoulin 0:a1e10abfbd19 138 }
michelmoulin 0:a1e10abfbd19 139 } else {
michelmoulin 0:a1e10abfbd19 140 skipStart = false;
michelmoulin 0:a1e10abfbd19 141 }
michelmoulin 0:a1e10abfbd19 142 for(blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
michelmoulin 0:a1e10abfbd19 143 checksum = link->getWord();
michelmoulin 0:a1e10abfbd19 144 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
michelmoulin 0:a1e10abfbd19 145 skipStart = true;
michelmoulin 0:a1e10abfbd19 146 blockType = NORMAL_BLOCK;
michelmoulin 0:a1e10abfbd19 147 if (serial != NULL) {
michelmoulin 0:a1e10abfbd19 148 serial->printf("skip");
michelmoulin 0:a1e10abfbd19 149 }
michelmoulin 0:a1e10abfbd19 150 return blockCount;
michelmoulin 0:a1e10abfbd19 151 } else if (checksum == PIXY_START_WORD_CC) {
michelmoulin 0:a1e10abfbd19 152 skipStart = true;
michelmoulin 0:a1e10abfbd19 153 blockType = CC_BLOCK;
michelmoulin 0:a1e10abfbd19 154 return blockCount;
michelmoulin 0:a1e10abfbd19 155 } else if (checksum == 0) {
michelmoulin 0:a1e10abfbd19 156 return blockCount;
michelmoulin 0:a1e10abfbd19 157 }
michelmoulin 0:a1e10abfbd19 158 if (blockCount > blockArraySize) {
michelmoulin 0:a1e10abfbd19 159 resize();
michelmoulin 0:a1e10abfbd19 160 }
michelmoulin 0:a1e10abfbd19 161 block = blocks + blockCount;
michelmoulin 0:a1e10abfbd19 162
michelmoulin 0:a1e10abfbd19 163 for (i = 0, sum = 0; i < sizeof(Block)/sizeof(uint16_t); i++) {
michelmoulin 0:a1e10abfbd19 164 if (blockType == NORMAL_BLOCK && i >= 5) { // skip
michelmoulin 0:a1e10abfbd19 165 block->angle = 0;
michelmoulin 0:a1e10abfbd19 166 break;
michelmoulin 0:a1e10abfbd19 167 }
michelmoulin 0:a1e10abfbd19 168 w = link->getWord();
michelmoulin 0:a1e10abfbd19 169 sum += w;
michelmoulin 0:a1e10abfbd19 170 *((uint16_t *)block + i) = w;
michelmoulin 0:a1e10abfbd19 171 }
michelmoulin 0:a1e10abfbd19 172
michelmoulin 0:a1e10abfbd19 173 if (checksum == sum) {
michelmoulin 0:a1e10abfbd19 174 blockCount++;
michelmoulin 0:a1e10abfbd19 175 } else {
michelmoulin 0:a1e10abfbd19 176 w = link->getWord();
michelmoulin 0:a1e10abfbd19 177 if (serial != NULL) {
michelmoulin 0:a1e10abfbd19 178 serial->printf("cs error");
michelmoulin 0:a1e10abfbd19 179 }
michelmoulin 0:a1e10abfbd19 180 }
michelmoulin 0:a1e10abfbd19 181 if (w == PIXY_START_WORD) {
michelmoulin 0:a1e10abfbd19 182 blockType = NORMAL_BLOCK;
michelmoulin 0:a1e10abfbd19 183 } else if (w == PIXY_START_WORD_CC) {
michelmoulin 0:a1e10abfbd19 184 blockType = CC_BLOCK;
michelmoulin 0:a1e10abfbd19 185 } else {
michelmoulin 0:a1e10abfbd19 186 return blockCount;
michelmoulin 0:a1e10abfbd19 187 }
michelmoulin 0:a1e10abfbd19 188 }
michelmoulin 0:a1e10abfbd19 189 return blockCount;
michelmoulin 0:a1e10abfbd19 190 }
michelmoulin 0:a1e10abfbd19 191
michelmoulin 0:a1e10abfbd19 192 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setBrightness(uint8_t brightness)
michelmoulin 0:a1e10abfbd19 193 {
michelmoulin 0:a1e10abfbd19 194 uint8_t outBuf[3];
michelmoulin 0:a1e10abfbd19 195 outBuf[0] = 0x00;
michelmoulin 0:a1e10abfbd19 196 outBuf[1] = 0xfe;
michelmoulin 0:a1e10abfbd19 197 outBuf[2] = brightness;
michelmoulin 0:a1e10abfbd19 198 return link->send(outBuf, 3);
michelmoulin 0:a1e10abfbd19 199 }
michelmoulin 0:a1e10abfbd19 200
michelmoulin 0:a1e10abfbd19 201 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setLED(uint8_t r, uint8_t g, uint8_t b)
michelmoulin 0:a1e10abfbd19 202 {
michelmoulin 0:a1e10abfbd19 203 uint8_t outBuf[5];
michelmoulin 0:a1e10abfbd19 204 outBuf[0] = 0x00;
michelmoulin 0:a1e10abfbd19 205 outBuf[1] = 0xfd;
michelmoulin 0:a1e10abfbd19 206 outBuf[2] = r;
michelmoulin 0:a1e10abfbd19 207 outBuf[3] = g;
michelmoulin 0:a1e10abfbd19 208 outBuf[4] = b;
michelmoulin 0:a1e10abfbd19 209 return link->send(outBuf, 5);
michelmoulin 0:a1e10abfbd19 210 }
michelmoulin 0:a1e10abfbd19 211
michelmoulin 0:a1e10abfbd19 212 #endif