same
Fork of PixyLibrary by
TPixy2.h@0:56a3009221d3, 2014-08-12 (annotated)
- Committer:
- MBM
- Date:
- Tue Aug 12 11:01:31 2014 +0000
- Revision:
- 0:56a3009221d3
The PixyLibrary was created from the Arduino libraries for the CMUcam5 Pixy found on http://cmucam.org/projects/cmucam5/files.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MBM | 0:56a3009221d3 | 1 | //-------------------------------------------------------------------------------------------- |
MBM | 0:56a3009221d3 | 2 | //Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip |
MBM | 0:56a3009221d3 | 3 | // |
MBM | 0:56a3009221d3 | 4 | //Modifications made by: Mathieu Malone |
MBM | 0:56a3009221d3 | 5 | //Modifications: Modified Arduino code to function with mbed development platform |
MBM | 0:56a3009221d3 | 6 | //Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication |
MBM | 0:56a3009221d3 | 7 | // between the mbed platform and putty terminal through USB |
MBM | 0:56a3009221d3 | 8 | // |
MBM | 0:56a3009221d3 | 9 | //Latest update by: Mathieu Malone |
MBM | 0:56a3009221d3 | 10 | //Date of last update: July 24th, 2014 |
MBM | 0:56a3009221d3 | 11 | //-------------------------------------------------------------------------------------------- |
MBM | 0:56a3009221d3 | 12 | // |
MBM | 0:56a3009221d3 | 13 | // begin license header |
MBM | 0:56a3009221d3 | 14 | // |
MBM | 0:56a3009221d3 | 15 | // This file is part of Pixy CMUcam5 or "Pixy" for short |
MBM | 0:56a3009221d3 | 16 | // |
MBM | 0:56a3009221d3 | 17 | // All Pixy source code is provided under the terms of the |
MBM | 0:56a3009221d3 | 18 | // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). |
MBM | 0:56a3009221d3 | 19 | // Those wishing to use Pixy source code, software and/or |
MBM | 0:56a3009221d3 | 20 | // technologies under different licensing terms should contact us at |
MBM | 0:56a3009221d3 | 21 | // cmucam@cs.cmu.edu. Such licensing terms are available for |
MBM | 0:56a3009221d3 | 22 | // all portions of the Pixy codebase presented here. |
MBM | 0:56a3009221d3 | 23 | // |
MBM | 0:56a3009221d3 | 24 | // end license header |
MBM | 0:56a3009221d3 | 25 | // |
MBM | 0:56a3009221d3 | 26 | |
MBM | 0:56a3009221d3 | 27 | /* |
MBM | 0:56a3009221d3 | 28 | 06.04.2014 v0.1.3 John Leimon |
MBM | 0:56a3009221d3 | 29 | + Added init() for initializing Pixy, which should |
MBM | 0:56a3009221d3 | 30 | be called from the setup() function. See comment |
MBM | 0:56a3009221d3 | 31 | in Pixy.h for details. |
MBM | 0:56a3009221d3 | 32 | */ |
MBM | 0:56a3009221d3 | 33 | |
MBM | 0:56a3009221d3 | 34 | #ifndef _TPIXY_H2 |
MBM | 0:56a3009221d3 | 35 | #define _TPIXY_H2 |
MBM | 0:56a3009221d3 | 36 | |
MBM | 0:56a3009221d3 | 37 | #define PIXY_INITIAL_ARRAYSIZE2 30 |
MBM | 0:56a3009221d3 | 38 | #define PIXY_MAXIMUM_ARRAYSIZE2 130 |
MBM | 0:56a3009221d3 | 39 | #define PIXY_START_WORD2 0xaa55 |
MBM | 0:56a3009221d3 | 40 | #define PIXY_START_WORDX2 0x55aa |
MBM | 0:56a3009221d3 | 41 | #define PIXY_DEFAULT_ADDR2 0x54 // I2C |
MBM | 0:56a3009221d3 | 42 | |
MBM | 0:56a3009221d3 | 43 | Serial pc2(USBTX, USBRX); |
MBM | 0:56a3009221d3 | 44 | |
MBM | 0:56a3009221d3 | 45 | struct Block2 |
MBM | 0:56a3009221d3 | 46 | { |
MBM | 0:56a3009221d3 | 47 | void print2() |
MBM | 0:56a3009221d3 | 48 | { |
MBM | 0:56a3009221d3 | 49 | char buf2[64]; |
MBM | 0:56a3009221d3 | 50 | sprintf(buf2, "sig: %d x2: %d y2: %d width2: %d height2: %d\n", signature2, x2, y2, width2, height2); |
MBM | 0:56a3009221d3 | 51 | printf(buf2); |
MBM | 0:56a3009221d3 | 52 | Xp2 = x2; |
MBM | 0:56a3009221d3 | 53 | Yp2= y2; |
MBM | 0:56a3009221d3 | 54 | sig2 = signature2; |
MBM | 0:56a3009221d3 | 55 | } |
MBM | 0:56a3009221d3 | 56 | uint16_t signature2; |
MBM | 0:56a3009221d3 | 57 | uint16_t x2; |
MBM | 0:56a3009221d3 | 58 | uint16_t y2; |
MBM | 0:56a3009221d3 | 59 | uint16_t width2; |
MBM | 0:56a3009221d3 | 60 | uint16_t height2; |
MBM | 0:56a3009221d3 | 61 | }; |
MBM | 0:56a3009221d3 | 62 | |
MBM | 0:56a3009221d3 | 63 | template <class LinkType2> class TPixy2 |
MBM | 0:56a3009221d3 | 64 | { |
MBM | 0:56a3009221d3 | 65 | public: |
MBM | 0:56a3009221d3 | 66 | TPixy2(uint8_t addr2=PIXY_DEFAULT_ADDR2); |
MBM | 0:56a3009221d3 | 67 | ~TPixy2(); |
MBM | 0:56a3009221d3 | 68 | |
MBM | 0:56a3009221d3 | 69 | uint16_t getBlocks2(uint16_t maxBlocks2=1000); |
MBM | 0:56a3009221d3 | 70 | int8_t setServos2(uint16_t s02, uint16_t s12); |
MBM | 0:56a3009221d3 | 71 | void init2(); |
MBM | 0:56a3009221d3 | 72 | |
MBM | 0:56a3009221d3 | 73 | Block2 *blocks2; |
MBM | 0:56a3009221d3 | 74 | |
MBM | 0:56a3009221d3 | 75 | private: |
MBM | 0:56a3009221d3 | 76 | bool getStart2(); |
MBM | 0:56a3009221d3 | 77 | void resize2(); |
MBM | 0:56a3009221d3 | 78 | |
MBM | 0:56a3009221d3 | 79 | LinkType2 link2; |
MBM | 0:56a3009221d3 | 80 | bool skipStart2; |
MBM | 0:56a3009221d3 | 81 | uint16_t blockCount2; |
MBM | 0:56a3009221d3 | 82 | uint16_t blockArraySize2; |
MBM | 0:56a3009221d3 | 83 | }; |
MBM | 0:56a3009221d3 | 84 | |
MBM | 0:56a3009221d3 | 85 | |
MBM | 0:56a3009221d3 | 86 | template <class LinkType2> TPixy2<LinkType2>::TPixy2(uint8_t addr2) |
MBM | 0:56a3009221d3 | 87 | { |
MBM | 0:56a3009221d3 | 88 | skipStart2 = false; |
MBM | 0:56a3009221d3 | 89 | blockCount2 = 0; |
MBM | 0:56a3009221d3 | 90 | blockArraySize2 = PIXY_INITIAL_ARRAYSIZE2; |
MBM | 0:56a3009221d3 | 91 | blocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2); |
MBM | 0:56a3009221d3 | 92 | link2.setAddress2(addr2); |
MBM | 0:56a3009221d3 | 93 | } |
MBM | 0:56a3009221d3 | 94 | |
MBM | 0:56a3009221d3 | 95 | template <class LinkType2> void TPixy2<LinkType2>::init2() |
MBM | 0:56a3009221d3 | 96 | { |
MBM | 0:56a3009221d3 | 97 | link2.init2(); |
MBM | 0:56a3009221d3 | 98 | } |
MBM | 0:56a3009221d3 | 99 | |
MBM | 0:56a3009221d3 | 100 | template <class LinkType2> TPixy2<LinkType2>::~TPixy2() |
MBM | 0:56a3009221d3 | 101 | { |
MBM | 0:56a3009221d3 | 102 | free(blocks2); |
MBM | 0:56a3009221d3 | 103 | } |
MBM | 0:56a3009221d3 | 104 | |
MBM | 0:56a3009221d3 | 105 | template <class LinkType2> bool TPixy2<LinkType2>::getStart2() |
MBM | 0:56a3009221d3 | 106 | { |
MBM | 0:56a3009221d3 | 107 | uint16_t w2, lastw2; |
MBM | 0:56a3009221d3 | 108 | |
MBM | 0:56a3009221d3 | 109 | lastw2 = 0xffff; |
MBM | 0:56a3009221d3 | 110 | |
MBM | 0:56a3009221d3 | 111 | while(true) |
MBM | 0:56a3009221d3 | 112 | { |
MBM | 0:56a3009221d3 | 113 | w2 = link2.getWord2(); |
MBM | 0:56a3009221d3 | 114 | if (w2==0 && lastw2==0) |
MBM | 0:56a3009221d3 | 115 | { |
MBM | 0:56a3009221d3 | 116 | wait(0.00001); |
MBM | 0:56a3009221d3 | 117 | return false; |
MBM | 0:56a3009221d3 | 118 | } |
MBM | 0:56a3009221d3 | 119 | else if (w2==PIXY_START_WORD2 && lastw2==PIXY_START_WORD2) |
MBM | 0:56a3009221d3 | 120 | return true; |
MBM | 0:56a3009221d3 | 121 | else if (w2==PIXY_START_WORDX2) |
MBM | 0:56a3009221d3 | 122 | { |
MBM | 0:56a3009221d3 | 123 | pc2.printf("reorder2"); |
MBM | 0:56a3009221d3 | 124 | link2.getByte2(); // resync |
MBM | 0:56a3009221d3 | 125 | } |
MBM | 0:56a3009221d3 | 126 | lastw2 = w2; |
MBM | 0:56a3009221d3 | 127 | } |
MBM | 0:56a3009221d3 | 128 | } |
MBM | 0:56a3009221d3 | 129 | |
MBM | 0:56a3009221d3 | 130 | template <class LinkType2> void TPixy2<LinkType2>::resize2() |
MBM | 0:56a3009221d3 | 131 | { |
MBM | 0:56a3009221d3 | 132 | Block2 *newBlocks2; |
MBM | 0:56a3009221d3 | 133 | blockArraySize2 += PIXY_INITIAL_ARRAYSIZE2; |
MBM | 0:56a3009221d3 | 134 | newBlocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2); |
MBM | 0:56a3009221d3 | 135 | memcpy(newBlocks2, blocks2, sizeof(Block2)*blockCount2); |
MBM | 0:56a3009221d3 | 136 | free(blocks2); |
MBM | 0:56a3009221d3 | 137 | blocks2 = newBlocks2; |
MBM | 0:56a3009221d3 | 138 | } |
MBM | 0:56a3009221d3 | 139 | |
MBM | 0:56a3009221d3 | 140 | template <class LinkType2> uint16_t TPixy2<LinkType2>::getBlocks2(uint16_t maxBlocks2) |
MBM | 0:56a3009221d3 | 141 | { |
MBM | 0:56a3009221d3 | 142 | uint8_t i2; |
MBM | 0:56a3009221d3 | 143 | uint16_t w2, checksum2, sum2; |
MBM | 0:56a3009221d3 | 144 | Block2 *block2; |
MBM | 0:56a3009221d3 | 145 | |
MBM | 0:56a3009221d3 | 146 | if (!skipStart2) |
MBM | 0:56a3009221d3 | 147 | { |
MBM | 0:56a3009221d3 | 148 | if (getStart2()==false) |
MBM | 0:56a3009221d3 | 149 | return 0; |
MBM | 0:56a3009221d3 | 150 | } |
MBM | 0:56a3009221d3 | 151 | else |
MBM | 0:56a3009221d3 | 152 | skipStart2 = false; |
MBM | 0:56a3009221d3 | 153 | |
MBM | 0:56a3009221d3 | 154 | for(blockCount2=0; blockCount2<maxBlocks2 && blockCount2<PIXY_MAXIMUM_ARRAYSIZE2;) |
MBM | 0:56a3009221d3 | 155 | { |
MBM | 0:56a3009221d3 | 156 | checksum2 = link2.getWord2(); |
MBM | 0:56a3009221d3 | 157 | if (checksum2==PIXY_START_WORD2) // we've reached the beginning of the next frame |
MBM | 0:56a3009221d3 | 158 | { |
MBM | 0:56a3009221d3 | 159 | skipStart2 = true; |
MBM | 0:56a3009221d3 | 160 | //Serial.println("skip"); |
MBM | 0:56a3009221d3 | 161 | return blockCount2; |
MBM | 0:56a3009221d3 | 162 | } |
MBM | 0:56a3009221d3 | 163 | else if (checksum2==0) |
MBM | 0:56a3009221d3 | 164 | return blockCount2; |
MBM | 0:56a3009221d3 | 165 | |
MBM | 0:56a3009221d3 | 166 | if (blockCount2>blockArraySize2) |
MBM | 0:56a3009221d3 | 167 | resize2(); |
MBM | 0:56a3009221d3 | 168 | |
MBM | 0:56a3009221d3 | 169 | block2 = blocks2 + blockCount2; |
MBM | 0:56a3009221d3 | 170 | |
MBM | 0:56a3009221d3 | 171 | for (i2=0, sum2=0; i2<sizeof(Block2)/sizeof(uint16_t); i2++) |
MBM | 0:56a3009221d3 | 172 | { |
MBM | 0:56a3009221d3 | 173 | w2 = link2.getWord2(); |
MBM | 0:56a3009221d3 | 174 | sum2 += w2; |
MBM | 0:56a3009221d3 | 175 | *((uint16_t *)block2 + i2) = w2; |
MBM | 0:56a3009221d3 | 176 | } |
MBM | 0:56a3009221d3 | 177 | |
MBM | 0:56a3009221d3 | 178 | if (checksum2==sum2) |
MBM | 0:56a3009221d3 | 179 | blockCount2++; |
MBM | 0:56a3009221d3 | 180 | else |
MBM | 0:56a3009221d3 | 181 | pc2.printf("cs error 2"); |
MBM | 0:56a3009221d3 | 182 | |
MBM | 0:56a3009221d3 | 183 | w2 = link2.getWord2(); |
MBM | 0:56a3009221d3 | 184 | if (w2!=PIXY_START_WORD2) |
MBM | 0:56a3009221d3 | 185 | return blockCount2; |
MBM | 0:56a3009221d3 | 186 | } |
MBM | 0:56a3009221d3 | 187 | return maxBlocks2; |
MBM | 0:56a3009221d3 | 188 | } |
MBM | 0:56a3009221d3 | 189 | |
MBM | 0:56a3009221d3 | 190 | template <class LinkType2> int8_t TPixy2<LinkType2>::setServos2(uint16_t s02, uint16_t s12) |
MBM | 0:56a3009221d3 | 191 | { |
MBM | 0:56a3009221d3 | 192 | uint8_t outBuf2[6]; |
MBM | 0:56a3009221d3 | 193 | |
MBM | 0:56a3009221d3 | 194 | outBuf2[0] = 0x00; |
MBM | 0:56a3009221d3 | 195 | outBuf2[1] = 0xff; |
MBM | 0:56a3009221d3 | 196 | *(uint16_t *)(outBuf2 + 2) = s02; |
MBM | 0:56a3009221d3 | 197 | *(uint16_t *)(outBuf2 + 4) = s12; |
MBM | 0:56a3009221d3 | 198 | |
MBM | 0:56a3009221d3 | 199 | return link2.send2(outBuf2, 6); |
MBM | 0:56a3009221d3 | 200 | } |
MBM | 0:56a3009221d3 | 201 | |
MBM | 0:56a3009221d3 | 202 | #endif |