Luis Enriquez / CACHAN_HOMOG

Homologation Cachan

Fork of TPixy-Interface by Stephen Wilkins

TPixy.h@4:ee635fc62b6e, 2018-06-09 (annotated)

Committer:
lenriquez389
Date:
Sat Jun 09 03:56:25 2018 +0000
Revision:
4:ee635fc62b6e
Parent:
3:66df7d295245 
PIXY

Who changed what in which revision?

UserRevisionLine numberNew contents of line
swilkins8 0:ef0e3c67dc5b 1 #ifndef _TPIXY_H
swilkins8 0:ef0e3c67dc5b 2 #define _TPIXY_H
swilkins8 0:ef0e3c67dc5b 3 #include "mbed.h"
swilkins8 0:ef0e3c67dc5b 4 #include "TPixyInterface.h"
swilkins8 0:ef0e3c67dc5b 5
swilkins8 0:ef0e3c67dc5b 6 // Communication/misc parameters
swilkins8 0:ef0e3c67dc5b 7 #define PIXY_INITIAL_ARRAYSIZE 30
swilkins8 0:ef0e3c67dc5b 8 #define PIXY_MAXIMUM_ARRAYSIZE 130
swilkins8 0:ef0e3c67dc5b 9 #define PIXY_START_WORD 0xaa55
swilkins8 0:ef0e3c67dc5b 10 #define PIXY_START_WORD_CC 0xaa56
swilkins8 0:ef0e3c67dc5b 11 #define PIXY_START_WORDX 0x55aa
swilkins8 0:ef0e3c67dc5b 12 #define PIXY_MAX_SIGNATURE 7
swilkins8 0:ef0e3c67dc5b 13 #define PIXY_DEFAULT_ARGVAL 0xffff
swilkins8 0:ef0e3c67dc5b 14
swilkins8 0:ef0e3c67dc5b 15 // Pixy x-y position values
swilkins8 0:ef0e3c67dc5b 16 #define PIXY_MIN_X 0L
swilkins8 0:ef0e3c67dc5b 17 #define PIXY_MAX_X 319L
swilkins8 0:ef0e3c67dc5b 18 #define PIXY_MIN_Y 0L
swilkins8 0:ef0e3c67dc5b 19 #define PIXY_MAX_Y 199L
lenriquez389 4:ee635fc62b6e 20 #define PIXY_CENTER_X ((PIXY_MAX_X-PIXY_MIN_X)/2)
swilkins8 0:ef0e3c67dc5b 21 // RC-servo values
lenriquez389 4:ee635fc62b6e 22 #define PIXY_RCS_MIN_POS 650L
swilkins8 0:ef0e3c67dc5b 23 #define PIXY_RCS_MAX_POS 1000L
swilkins8 0:ef0e3c67dc5b 24 #define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2)
swilkins8 0:ef0e3c67dc5b 25
swilkins8 0:ef0e3c67dc5b 26 enum BlockType {
swilkins8 0:ef0e3c67dc5b 27 NORMAL_BLOCK,
swilkins8 0:ef0e3c67dc5b 28 CC_BLOCK
swilkins8 0:ef0e3c67dc5b 29 };
swilkins8 0:ef0e3c67dc5b 30
swilkins8 0:ef0e3c67dc5b 31 struct Block {
swilkins8 0:ef0e3c67dc5b 32 uint16_t signature;
swilkins8 0:ef0e3c67dc5b 33 uint16_t x;
swilkins8 0:ef0e3c67dc5b 34 uint16_t y;
swilkins8 0:ef0e3c67dc5b 35 uint16_t width;
swilkins8 0:ef0e3c67dc5b 36 uint16_t height;
swilkins8 0:ef0e3c67dc5b 37 uint16_t angle;
swilkins8 0:ef0e3c67dc5b 38 };
swilkins8 0:ef0e3c67dc5b 39
swilkins8 0:ef0e3c67dc5b 40 template <class TPixyInterface> class TPixy
swilkins8 0:ef0e3c67dc5b 41 {
swilkins8 0:ef0e3c67dc5b 42 public:
swilkins8 0:ef0e3c67dc5b 43 Serial* serial;
swilkins8 0:ef0e3c67dc5b 44 Block *blocks;
swilkins8 0:ef0e3c67dc5b 45 TPixy(TPixyInterface* type, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL);
swilkins8 0:ef0e3c67dc5b 46 ~TPixy();
swilkins8 3:66df7d295245 47 uint16_t getBlocks(uint16_t maxBlocks = 1000);
swilkins8 0:ef0e3c67dc5b 48 int8_t setServos(uint16_t s0, uint16_t s1);
swilkins8 0:ef0e3c67dc5b 49 void init();
swilkins8 0:ef0e3c67dc5b 50
swilkins8 0:ef0e3c67dc5b 51 private:
swilkins8 0:ef0e3c67dc5b 52 TPixyInterface* link;
swilkins8 0:ef0e3c67dc5b 53 BlockType blockType;
swilkins8 0:ef0e3c67dc5b 54 bool getStart();
swilkins8 0:ef0e3c67dc5b 55 void resize();
swilkins8 0:ef0e3c67dc5b 56 bool skipStart;
swilkins8 0:ef0e3c67dc5b 57 uint16_t blockCount;
swilkins8 0:ef0e3c67dc5b 58 uint16_t blockArraySize;
swilkins8 0:ef0e3c67dc5b 59 };
swilkins8 0:ef0e3c67dc5b 60
swilkins8 0:ef0e3c67dc5b 61 template <class TPixyInterface> void TPixy<TPixyInterface>::init()
swilkins8 0:ef0e3c67dc5b 62 {
swilkins8 0:ef0e3c67dc5b 63 link->init();
swilkins8 0:ef0e3c67dc5b 64 }
swilkins8 0:ef0e3c67dc5b 65
swilkins8 0:ef0e3c67dc5b 66
swilkins8 0:ef0e3c67dc5b 67 template <class TPixyInterface> TPixy<TPixyInterface>::TPixy(TPixyInterface* type, Serial* serialOut, uint16_t arg) : serial(serialOut), link(type)
swilkins8 0:ef0e3c67dc5b 68 {
swilkins8 0:ef0e3c67dc5b 69 skipStart = false;
swilkins8 0:ef0e3c67dc5b 70 blockCount = 0;
swilkins8 0:ef0e3c67dc5b 71 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
swilkins8 0:ef0e3c67dc5b 72 blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
swilkins8 0:ef0e3c67dc5b 73 link->setArg(arg);
swilkins8 0:ef0e3c67dc5b 74 }
swilkins8 0:ef0e3c67dc5b 75
swilkins8 0:ef0e3c67dc5b 76 template <class TPixyInterface> TPixy<TPixyInterface>::~TPixy()
swilkins8 0:ef0e3c67dc5b 77 {
swilkins8 0:ef0e3c67dc5b 78 free(blocks);
swilkins8 0:ef0e3c67dc5b 79 }
swilkins8 0:ef0e3c67dc5b 80
swilkins8 0:ef0e3c67dc5b 81 template <class TPixyInterface> bool TPixy<TPixyInterface>::getStart()
swilkins8 0:ef0e3c67dc5b 82 {
swilkins8 0:ef0e3c67dc5b 83 uint16_t w, lastw;
swilkins8 0:ef0e3c67dc5b 84 lastw = 0xffff;
swilkins8 0:ef0e3c67dc5b 85 while(true) {
swilkins8 0:ef0e3c67dc5b 86 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 87 if (w == 0 && lastw == 0) {
swilkins8 0:ef0e3c67dc5b 88 wait_ms(10);
swilkins8 0:ef0e3c67dc5b 89 return false;
swilkins8 0:ef0e3c67dc5b 90 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 91 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 92 return true;
swilkins8 0:ef0e3c67dc5b 93 } else if (w == PIXY_START_WORD_CC && lastw == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 94 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 95 return true;
swilkins8 3:66df7d295245 96 } else if (w == PIXY_START_WORDX) {
swilkins8 0:ef0e3c67dc5b 97 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 98 serial->printf("reorder");
swilkins8 0:ef0e3c67dc5b 99 }
swilkins8 0:ef0e3c67dc5b 100 link->getByte(); // resync
swilkins8 0:ef0e3c67dc5b 101 }
swilkins8 0:ef0e3c67dc5b 102 lastw = w;
swilkins8 0:ef0e3c67dc5b 103 }
swilkins8 0:ef0e3c67dc5b 104 }
swilkins8 0:ef0e3c67dc5b 105
swilkins8 0:ef0e3c67dc5b 106 template <class TPixyInterface> void TPixy<TPixyInterface>::resize()
swilkins8 0:ef0e3c67dc5b 107 {
swilkins8 0:ef0e3c67dc5b 108 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
swilkins8 0:ef0e3c67dc5b 109 blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
swilkins8 0:ef0e3c67dc5b 110 }
swilkins8 0:ef0e3c67dc5b 111
swilkins8 0:ef0e3c67dc5b 112 template <class TPixyInterface> uint16_t TPixy<TPixyInterface>::getBlocks(uint16_t maxBlocks)
swilkins8 0:ef0e3c67dc5b 113 {
swilkins8 0:ef0e3c67dc5b 114 uint8_t i;
swilkins8 0:ef0e3c67dc5b 115 uint16_t w, checksum, sum;
swilkins8 0:ef0e3c67dc5b 116 Block *block;
swilkins8 0:ef0e3c67dc5b 117
swilkins8 0:ef0e3c67dc5b 118 if (!skipStart) {
swilkins8 0:ef0e3c67dc5b 119 if (getStart() == false) {
swilkins8 0:ef0e3c67dc5b 120 return 0;
swilkins8 0:ef0e3c67dc5b 121 }
swilkins8 0:ef0e3c67dc5b 122 } else {
swilkins8 0:ef0e3c67dc5b 123 skipStart = false;
swilkins8 0:ef0e3c67dc5b 124 }
swilkins8 0:ef0e3c67dc5b 125 for(blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
swilkins8 0:ef0e3c67dc5b 126 checksum = link->getWord();
lenriquez389 4:ee635fc62b6e 127 if (checksum == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 128 skipStart = true;
swilkins8 0:ef0e3c67dc5b 129 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 130 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 131 serial->printf("skip");
swilkins8 0:ef0e3c67dc5b 132 }
swilkins8 0:ef0e3c67dc5b 133 return blockCount;
swilkins8 0:ef0e3c67dc5b 134 } else if (checksum == PIXY_START_WORD_CC) {
swilkins8 0:ef0e3c67dc5b 135 skipStart = true;
swilkins8 0:ef0e3c67dc5b 136 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 137 return blockCount;
swilkins8 0:ef0e3c67dc5b 138 } else if (checksum == 0) {
swilkins8 0:ef0e3c67dc5b 139 return blockCount;
swilkins8 0:ef0e3c67dc5b 140 }
swilkins8 0:ef0e3c67dc5b 141 if (blockCount > blockArraySize) {
swilkins8 0:ef0e3c67dc5b 142 resize();
swilkins8 0:ef0e3c67dc5b 143 }
swilkins8 0:ef0e3c67dc5b 144 block = blocks + blockCount;
swilkins8 0:ef0e3c67dc5b 145
swilkins8 0:ef0e3c67dc5b 146 for (i = 0, sum = 0; i < sizeof(Block)/sizeof(uint16_t); i++) {
swilkins8 0:ef0e3c67dc5b 147 if (blockType == NORMAL_BLOCK && i >= 5) { // skip
swilkins8 0:ef0e3c67dc5b 148 block->angle = 0;
swilkins8 0:ef0e3c67dc5b 149 break;
swilkins8 0:ef0e3c67dc5b 150 }
swilkins8 0:ef0e3c67dc5b 151 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 152 sum += w;
swilkins8 0:ef0e3c67dc5b 153 *((uint16_t *)block + i) = w;
swilkins8 0:ef0e3c67dc5b 154 }
swilkins8 0:ef0e3c67dc5b 155
swilkins8 0:ef0e3c67dc5b 156 if (checksum == sum) {
swilkins8 0:ef0e3c67dc5b 157 blockCount++;
swilkins8 0:ef0e3c67dc5b 158 } else {
swilkins8 0:ef0e3c67dc5b 159 w = link->getWord();
swilkins8 0:ef0e3c67dc5b 160 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 161 serial->printf("cs error");
swilkins8 0:ef0e3c67dc5b 162 }
swilkins8 0:ef0e3c67dc5b 163 }
swilkins8 0:ef0e3c67dc5b 164 if (w == PIXY_START_WORD) {
swilkins8 0:ef0e3c67dc5b 165 blockType = NORMAL_BLOCK;
swilkins8 0:ef0e3c67dc5b 166 } else if (w == PIXY_START_WORD_CC) {
swilkins8 0:ef0e3c67dc5b 167 blockType = CC_BLOCK;
swilkins8 0:ef0e3c67dc5b 168 } else {
swilkins8 0:ef0e3c67dc5b 169 return blockCount;
swilkins8 0:ef0e3c67dc5b 170 }
swilkins8 0:ef0e3c67dc5b 171 }
swilkins8 0:ef0e3c67dc5b 172 return blockCount;
swilkins8 0:ef0e3c67dc5b 173 }
swilkins8 0:ef0e3c67dc5b 174
swilkins8 0:ef0e3c67dc5b 175 template <class TPixyInterface> int8_t TPixy<TPixyInterface>::setServos(uint16_t s0, uint16_t s1)
swilkins8 0:ef0e3c67dc5b 176 {
swilkins8 0:ef0e3c67dc5b 177 uint8_t outBuf[6];
swilkins8 0:ef0e3c67dc5b 178 outBuf[0] = 0x00;
swilkins8 0:ef0e3c67dc5b 179 outBuf[1] = 0xff;
swilkins8 0:ef0e3c67dc5b 180 *(uint16_t *)(outBuf + 2) = s0;
swilkins8 0:ef0e3c67dc5b 181 *(uint16_t *)(outBuf + 4) = s1;
swilkins8 0:ef0e3c67dc5b 182 return link->send(outBuf, 6);
swilkins8 0:ef0e3c67dc5b 183 }
swilkins8 0:ef0e3c67dc5b 184
swilkins8 0:ef0e3c67dc5b 185 #endif