same
Fork of PixyLibrary by
TPixy1.h@1:4b14159ab9bb, 2015-04-13 (annotated)
- Committer:
- johnylafleur
- Date:
- Mon Apr 13 12:32:52 2015 +0000
- Revision:
- 1:4b14159ab9bb
- Parent:
- 0:56a3009221d3
Same library
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_H1 |
MBM | 0:56a3009221d3 | 35 | #define _TPIXY_H1 |
MBM | 0:56a3009221d3 | 36 | |
MBM | 0:56a3009221d3 | 37 | #define PIXY_INITIAL_ARRAYSIZE1 30 |
MBM | 0:56a3009221d3 | 38 | #define PIXY_MAXIMUM_ARRAYSIZE1 130 |
MBM | 0:56a3009221d3 | 39 | #define PIXY_START_WORD1 0xaa55 |
MBM | 0:56a3009221d3 | 40 | #define PIXY_START_WORDX1 0x55aa |
MBM | 0:56a3009221d3 | 41 | #define PIXY_DEFAULT_ADDR1 0x54 // I2C |
MBM | 0:56a3009221d3 | 42 | |
MBM | 0:56a3009221d3 | 43 | Serial pc1(USBTX, USBRX); |
MBM | 0:56a3009221d3 | 44 | |
MBM | 0:56a3009221d3 | 45 | struct Block1 |
MBM | 0:56a3009221d3 | 46 | { |
MBM | 0:56a3009221d3 | 47 | void print1() |
MBM | 0:56a3009221d3 | 48 | { |
MBM | 0:56a3009221d3 | 49 | char buf1[64]; |
MBM | 0:56a3009221d3 | 50 | sprintf(buf1, "sig: %d x1: %d y1: %d width1: %d height1: %d\n", signature1, x1, y1, width1, height1); |
MBM | 0:56a3009221d3 | 51 | printf(buf1); |
MBM | 0:56a3009221d3 | 52 | Xp1 = x1; |
MBM | 0:56a3009221d3 | 53 | Yp1 = y1; |
MBM | 0:56a3009221d3 | 54 | sig1 = signature1; |
MBM | 0:56a3009221d3 | 55 | } |
MBM | 0:56a3009221d3 | 56 | uint16_t signature1; |
MBM | 0:56a3009221d3 | 57 | uint16_t x1; |
MBM | 0:56a3009221d3 | 58 | uint16_t y1; |
MBM | 0:56a3009221d3 | 59 | uint16_t width1; |
MBM | 0:56a3009221d3 | 60 | uint16_t height1; |
MBM | 0:56a3009221d3 | 61 | }; |
MBM | 0:56a3009221d3 | 62 | |
MBM | 0:56a3009221d3 | 63 | template <class LinkType1> class TPixy1 |
MBM | 0:56a3009221d3 | 64 | { |
MBM | 0:56a3009221d3 | 65 | public: |
MBM | 0:56a3009221d3 | 66 | TPixy1(uint8_t addr1=PIXY_DEFAULT_ADDR1); |
MBM | 0:56a3009221d3 | 67 | ~TPixy1(); |
MBM | 0:56a3009221d3 | 68 | |
MBM | 0:56a3009221d3 | 69 | uint16_t getBlocks1(uint16_t maxBlocks1=1000); |
MBM | 0:56a3009221d3 | 70 | int8_t setServos1(uint16_t s01, uint16_t s11); |
MBM | 0:56a3009221d3 | 71 | void init1(); |
MBM | 0:56a3009221d3 | 72 | |
MBM | 0:56a3009221d3 | 73 | Block1 *blocks1; |
MBM | 0:56a3009221d3 | 74 | |
MBM | 0:56a3009221d3 | 75 | private: |
MBM | 0:56a3009221d3 | 76 | bool getStart1(); |
MBM | 0:56a3009221d3 | 77 | void resize1(); |
MBM | 0:56a3009221d3 | 78 | |
MBM | 0:56a3009221d3 | 79 | LinkType1 link1; |
MBM | 0:56a3009221d3 | 80 | bool skipStart1; |
MBM | 0:56a3009221d3 | 81 | uint16_t blockCount1; |
MBM | 0:56a3009221d3 | 82 | uint16_t blockArraySize1; |
MBM | 0:56a3009221d3 | 83 | }; |
MBM | 0:56a3009221d3 | 84 | |
MBM | 0:56a3009221d3 | 85 | |
MBM | 0:56a3009221d3 | 86 | template <class LinkType1> TPixy1<LinkType1>::TPixy1(uint8_t addr1) |
MBM | 0:56a3009221d3 | 87 | { |
MBM | 0:56a3009221d3 | 88 | skipStart1 = false; |
MBM | 0:56a3009221d3 | 89 | blockCount1 = 0; |
MBM | 0:56a3009221d3 | 90 | blockArraySize1 = PIXY_INITIAL_ARRAYSIZE1; |
MBM | 0:56a3009221d3 | 91 | blocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1); |
MBM | 0:56a3009221d3 | 92 | link1.setAddress1(addr1); |
MBM | 0:56a3009221d3 | 93 | } |
MBM | 0:56a3009221d3 | 94 | |
MBM | 0:56a3009221d3 | 95 | template <class LinkType1> void TPixy1<LinkType1>::init1() |
MBM | 0:56a3009221d3 | 96 | { |
MBM | 0:56a3009221d3 | 97 | link1.init1(); |
MBM | 0:56a3009221d3 | 98 | } |
MBM | 0:56a3009221d3 | 99 | |
MBM | 0:56a3009221d3 | 100 | template <class LinkType1> TPixy1<LinkType1>::~TPixy1() |
MBM | 0:56a3009221d3 | 101 | { |
MBM | 0:56a3009221d3 | 102 | free(blocks1); |
MBM | 0:56a3009221d3 | 103 | } |
MBM | 0:56a3009221d3 | 104 | |
MBM | 0:56a3009221d3 | 105 | template <class LinkType1> bool TPixy1<LinkType1>::getStart1() |
MBM | 0:56a3009221d3 | 106 | { |
MBM | 0:56a3009221d3 | 107 | uint16_t w1, lastw1; |
MBM | 0:56a3009221d3 | 108 | |
MBM | 0:56a3009221d3 | 109 | lastw1 = 0xffff; |
MBM | 0:56a3009221d3 | 110 | |
MBM | 0:56a3009221d3 | 111 | while(true) |
MBM | 0:56a3009221d3 | 112 | { |
MBM | 0:56a3009221d3 | 113 | w1 = link1.getWord1(); |
MBM | 0:56a3009221d3 | 114 | if (w1==0 && lastw1==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 (w1==PIXY_START_WORD1 && lastw1==PIXY_START_WORD1) |
MBM | 0:56a3009221d3 | 120 | return true; |
MBM | 0:56a3009221d3 | 121 | else if (w1==PIXY_START_WORDX1) |
MBM | 0:56a3009221d3 | 122 | { |
MBM | 0:56a3009221d3 | 123 | pc1.printf("reorder1"); |
MBM | 0:56a3009221d3 | 124 | link1.getByte1(); // resync |
MBM | 0:56a3009221d3 | 125 | } |
MBM | 0:56a3009221d3 | 126 | lastw1 = w1; |
MBM | 0:56a3009221d3 | 127 | } |
MBM | 0:56a3009221d3 | 128 | } |
MBM | 0:56a3009221d3 | 129 | |
MBM | 0:56a3009221d3 | 130 | template <class LinkType1> void TPixy1<LinkType1>::resize1() |
MBM | 0:56a3009221d3 | 131 | { |
MBM | 0:56a3009221d3 | 132 | Block1 *newBlocks1; |
MBM | 0:56a3009221d3 | 133 | blockArraySize1 += PIXY_INITIAL_ARRAYSIZE1; |
MBM | 0:56a3009221d3 | 134 | newBlocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1); |
MBM | 0:56a3009221d3 | 135 | memcpy(newBlocks1, blocks1, sizeof(Block1)*blockCount1); |
MBM | 0:56a3009221d3 | 136 | free(blocks1); |
MBM | 0:56a3009221d3 | 137 | blocks1 = newBlocks1; |
MBM | 0:56a3009221d3 | 138 | } |
MBM | 0:56a3009221d3 | 139 | |
MBM | 0:56a3009221d3 | 140 | template <class LinkType1> uint16_t TPixy1<LinkType1>::getBlocks1(uint16_t maxBlocks1) |
MBM | 0:56a3009221d3 | 141 | { |
MBM | 0:56a3009221d3 | 142 | uint8_t i1; |
MBM | 0:56a3009221d3 | 143 | uint16_t w1, checksum1, sum1; |
MBM | 0:56a3009221d3 | 144 | Block1 *block1; |
MBM | 0:56a3009221d3 | 145 | |
MBM | 0:56a3009221d3 | 146 | if (!skipStart1) |
MBM | 0:56a3009221d3 | 147 | { |
MBM | 0:56a3009221d3 | 148 | if (getStart1()==false) |
MBM | 0:56a3009221d3 | 149 | return 0; |
MBM | 0:56a3009221d3 | 150 | } |
MBM | 0:56a3009221d3 | 151 | else |
MBM | 0:56a3009221d3 | 152 | skipStart1 = false; |
MBM | 0:56a3009221d3 | 153 | |
MBM | 0:56a3009221d3 | 154 | for(blockCount1=0; blockCount1<maxBlocks1 && blockCount1<PIXY_MAXIMUM_ARRAYSIZE1;) |
MBM | 0:56a3009221d3 | 155 | { |
MBM | 0:56a3009221d3 | 156 | checksum1 = link1.getWord1(); |
MBM | 0:56a3009221d3 | 157 | if (checksum1==PIXY_START_WORD1) // we've reached the beginning of the next frame |
MBM | 0:56a3009221d3 | 158 | { |
MBM | 0:56a3009221d3 | 159 | skipStart1 = true; |
MBM | 0:56a3009221d3 | 160 | //Serial.println("skip"); |
MBM | 0:56a3009221d3 | 161 | return blockCount1; |
MBM | 0:56a3009221d3 | 162 | } |
MBM | 0:56a3009221d3 | 163 | else if (checksum1==0) |
MBM | 0:56a3009221d3 | 164 | return blockCount1; |
MBM | 0:56a3009221d3 | 165 | |
MBM | 0:56a3009221d3 | 166 | if (blockCount1>blockArraySize1) |
MBM | 0:56a3009221d3 | 167 | resize1(); |
MBM | 0:56a3009221d3 | 168 | |
MBM | 0:56a3009221d3 | 169 | block1 = blocks1 + blockCount1; |
MBM | 0:56a3009221d3 | 170 | |
MBM | 0:56a3009221d3 | 171 | for (i1=0, sum1=0; i1<sizeof(Block1)/sizeof(uint16_t); i1++) |
MBM | 0:56a3009221d3 | 172 | { |
MBM | 0:56a3009221d3 | 173 | w1 = link1.getWord1(); |
MBM | 0:56a3009221d3 | 174 | sum1 += w1; |
MBM | 0:56a3009221d3 | 175 | *((uint16_t *)block1 + i1) = w1; |
MBM | 0:56a3009221d3 | 176 | } |
MBM | 0:56a3009221d3 | 177 | |
MBM | 0:56a3009221d3 | 178 | if (checksum1==sum1) |
MBM | 0:56a3009221d3 | 179 | blockCount1++; |
MBM | 0:56a3009221d3 | 180 | else |
MBM | 0:56a3009221d3 | 181 | pc1.printf("cs error 1"); |
MBM | 0:56a3009221d3 | 182 | |
MBM | 0:56a3009221d3 | 183 | w1 = link1.getWord1(); |
MBM | 0:56a3009221d3 | 184 | if (w1!=PIXY_START_WORD1) |
MBM | 0:56a3009221d3 | 185 | return blockCount1; |
MBM | 0:56a3009221d3 | 186 | } |
MBM | 0:56a3009221d3 | 187 | return maxBlocks1; |
MBM | 0:56a3009221d3 | 188 | } |
MBM | 0:56a3009221d3 | 189 | |
MBM | 0:56a3009221d3 | 190 | template <class LinkType1> int8_t TPixy1<LinkType1>::setServos1(uint16_t s01, uint16_t s11) |
MBM | 0:56a3009221d3 | 191 | { |
MBM | 0:56a3009221d3 | 192 | uint8_t outBuf1[6]; |
MBM | 0:56a3009221d3 | 193 | |
MBM | 0:56a3009221d3 | 194 | outBuf1[0] = 0x00; |
MBM | 0:56a3009221d3 | 195 | outBuf1[1] = 0xff; |
MBM | 0:56a3009221d3 | 196 | *(uint16_t *)(outBuf1 + 2) = s01; |
MBM | 0:56a3009221d3 | 197 | *(uint16_t *)(outBuf1 + 4) = s11; |
MBM | 0:56a3009221d3 | 198 | |
MBM | 0:56a3009221d3 | 199 | return link1.send1(outBuf1, 6); |
MBM | 0:56a3009221d3 | 200 | } |
MBM | 0:56a3009221d3 | 201 | |
MBM | 0:56a3009221d3 | 202 | #endif |
MBM | 0:56a3009221d3 | 203 | |
MBM | 0:56a3009221d3 | 204 | |
MBM | 0:56a3009221d3 | 205 | |
MBM | 0:56a3009221d3 | 206 | |
MBM | 0:56a3009221d3 | 207 |