1 | //
|
---|
2 | // begin license header
|
---|
3 | //
|
---|
4 | // This file is part of Pixy CMUcam5 or "Pixy" for short
|
---|
5 | //
|
---|
6 | // All Pixy source code is provided under the terms of the
|
---|
7 | // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
|
---|
8 | // Those wishing to use Pixy source code, software and/or
|
---|
9 | // technologies under different licensing terms should contact us at
|
---|
10 | // cmucam@cs.cmu.edu. Such licensing terms are available for
|
---|
11 | // all portions of the Pixy codebase presented here.
|
---|
12 | //
|
---|
13 | // end license header
|
---|
14 | //
|
---|
15 | // This file is for defining the Block struct and the Pixy template class.
|
---|
16 | // (TPixy). TPixy takes a communication link as a template parameter so that
|
---|
17 | // all communication modes (SPI, I2C and UART) can share the same code.
|
---|
18 | //
|
---|
19 |
|
---|
20 | #ifndef _TPIXY_H
|
---|
21 | #define _TPIXY_H
|
---|
22 |
|
---|
23 | #include "Arduino.h"
|
---|
24 |
|
---|
25 | // Communication/misc parameters
|
---|
26 | #define PIXY_INITIAL_ARRAYSIZE 30
|
---|
27 | #define PIXY_MAXIMUM_ARRAYSIZE 130
|
---|
28 | #define PIXY_START_WORD 0xaa55
|
---|
29 | #define PIXY_START_WORD_CC 0xaa56
|
---|
30 | #define PIXY_START_WORDX 0x55aa
|
---|
31 | #define PIXY_MAX_SIGNATURE 7
|
---|
32 | #define PIXY_DEFAULT_ARGVAL 0xffff
|
---|
33 |
|
---|
34 | // Pixy x-y position values
|
---|
35 | #define PIXY_MIN_X 0L
|
---|
36 | #define PIXY_MAX_X 319L
|
---|
37 | #define PIXY_MIN_Y 0L
|
---|
38 | #define PIXY_MAX_Y 199L
|
---|
39 |
|
---|
40 | // RC-servo values
|
---|
41 | #define PIXY_RCS_MIN_POS 0L
|
---|
42 | #define PIXY_RCS_MAX_POS 1000L
|
---|
43 | #define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2)
|
---|
44 |
|
---|
45 |
|
---|
46 | enum BlockType
|
---|
47 | {
|
---|
48 | NORMAL_BLOCK,
|
---|
49 | CC_BLOCK
|
---|
50 | };
|
---|
51 |
|
---|
52 | struct Block
|
---|
53 | {
|
---|
54 | // print block structure!
|
---|
55 | void print()
|
---|
56 | {
|
---|
57 | int i, j;
|
---|
58 | char buf[128], sig[6], d;
|
---|
59 | bool flag;
|
---|
60 | if (signature>PIXY_MAX_SIGNATURE) // color code! (CC)
|
---|
61 | {
|
---|
62 | // convert signature number to an octal string
|
---|
63 | for (i=12, j=0, flag=false; i>=0; i-=3)
|
---|
64 | {
|
---|
65 | d = (signature>>i)&0x07;
|
---|
66 | if (d>0 && !flag)
|
---|
67 | flag = true;
|
---|
68 | if (flag)
|
---|
69 | sig[j++] = d + '0';
|
---|
70 | }
|
---|
71 | sig[j] = '\0';
|
---|
72 | sprintf(buf, "CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d\n", sig, signature, x, y, width, height, angle);
|
---|
73 | }
|
---|
74 | else // regular block. Note, angle is always zero, so no need to print
|
---|
75 | sprintf(buf, "sig: %d x: %d y: %d width: %d height: %d\n", signature, x, y, width, height);
|
---|
76 | Serial.print(buf);
|
---|
77 | }
|
---|
78 | uint16_t signature;
|
---|
79 | uint16_t x;
|
---|
80 | uint16_t y;
|
---|
81 | uint16_t width;
|
---|
82 | uint16_t height;
|
---|
83 | uint16_t angle;
|
---|
84 | };
|
---|
85 |
|
---|
86 |
|
---|
87 |
|
---|
88 | template <class LinkType> class TPixy
|
---|
89 | {
|
---|
90 | public:
|
---|
91 | TPixy(uint16_t arg=PIXY_DEFAULT_ARGVAL);
|
---|
92 | ~TPixy();
|
---|
93 |
|
---|
94 | uint16_t getBlocks(uint16_t maxBlocks=1000);
|
---|
95 | int8_t setServos(uint16_t s0, uint16_t s1);
|
---|
96 | int8_t setBrightness(uint8_t brightness);
|
---|
97 | int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
|
---|
98 | void init();
|
---|
99 |
|
---|
100 | Block *blocks;
|
---|
101 |
|
---|
102 | private:
|
---|
103 | boolean getStart();
|
---|
104 | void resize();
|
---|
105 |
|
---|
106 | LinkType link;
|
---|
107 | boolean skipStart;
|
---|
108 | BlockType blockType;
|
---|
109 | uint16_t blockCount;
|
---|
110 | uint16_t blockArraySize;
|
---|
111 | };
|
---|
112 |
|
---|
113 |
|
---|
114 | template <class LinkType> TPixy<LinkType>::TPixy(uint16_t arg)
|
---|
115 | {
|
---|
116 | skipStart = false;
|
---|
117 | blockCount = 0;
|
---|
118 | blockArraySize = PIXY_INITIAL_ARRAYSIZE;
|
---|
119 | blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
|
---|
120 | link.setArg(arg);
|
---|
121 | }
|
---|
122 |
|
---|
123 | template <class LinkType> void TPixy<LinkType>::init()
|
---|
124 | {
|
---|
125 | link.init();
|
---|
126 | }
|
---|
127 |
|
---|
128 | template <class LinkType> TPixy<LinkType>::~TPixy()
|
---|
129 | {
|
---|
130 | free(blocks);
|
---|
131 | }
|
---|
132 |
|
---|
133 | template <class LinkType> boolean TPixy<LinkType>::getStart()
|
---|
134 | {
|
---|
135 | uint16_t w, lastw;
|
---|
136 |
|
---|
137 | lastw = 0xffff;
|
---|
138 |
|
---|
139 | while(true)
|
---|
140 | {
|
---|
141 | w = link.getWord();
|
---|
142 | if (w==0 && lastw==0)
|
---|
143 | {
|
---|
144 | delayMicroseconds(10);
|
---|
145 | return false;
|
---|
146 | }
|
---|
147 | else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD)
|
---|
148 | {
|
---|
149 | blockType = NORMAL_BLOCK;
|
---|
150 | return true;
|
---|
151 | }
|
---|
152 | else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD)
|
---|
153 | {
|
---|
154 | blockType = CC_BLOCK;
|
---|
155 | return true;
|
---|
156 | }
|
---|
157 | else if (w==PIXY_START_WORDX)
|
---|
158 | {
|
---|
159 | Serial.println("reorder");
|
---|
160 | link.getByte(); // resync
|
---|
161 | }
|
---|
162 | lastw = w;
|
---|
163 | }
|
---|
164 | }
|
---|
165 |
|
---|
166 | template <class LinkType> void TPixy<LinkType>::resize()
|
---|
167 | {
|
---|
168 | blockArraySize += PIXY_INITIAL_ARRAYSIZE;
|
---|
169 | blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
|
---|
170 | }
|
---|
171 |
|
---|
172 | template <class LinkType> uint16_t TPixy<LinkType>::getBlocks(uint16_t maxBlocks)
|
---|
173 | {
|
---|
174 | uint8_t i;
|
---|
175 | uint16_t w, checksum, sum;
|
---|
176 | Block *block;
|
---|
177 |
|
---|
178 | if (!skipStart)
|
---|
179 | {
|
---|
180 | if (getStart()==false)
|
---|
181 | return 0;
|
---|
182 | }
|
---|
183 | else
|
---|
184 | skipStart = false;
|
---|
185 |
|
---|
186 | for(blockCount=0; blockCount<maxBlocks && blockCount<PIXY_MAXIMUM_ARRAYSIZE;)
|
---|
187 | {
|
---|
188 | checksum = link.getWord();
|
---|
189 | if (checksum==PIXY_START_WORD) // we've reached the beginning of the next frame
|
---|
190 | {
|
---|
191 | skipStart = true;
|
---|
192 | blockType = NORMAL_BLOCK;
|
---|
193 | //Serial.println("skip");
|
---|
194 | return blockCount;
|
---|
195 | }
|
---|
196 | else if (checksum==PIXY_START_WORD_CC)
|
---|
197 | {
|
---|
198 | skipStart = true;
|
---|
199 | blockType = CC_BLOCK;
|
---|
200 | return blockCount;
|
---|
201 | }
|
---|
202 | else if (checksum==0)
|
---|
203 | return blockCount;
|
---|
204 |
|
---|
205 | if (blockCount>blockArraySize)
|
---|
206 | resize();
|
---|
207 |
|
---|
208 | block = blocks + blockCount;
|
---|
209 |
|
---|
210 | for (i=0, sum=0; i<sizeof(Block)/sizeof(uint16_t); i++)
|
---|
211 | {
|
---|
212 | if (blockType==NORMAL_BLOCK && i>=5) // skip
|
---|
213 | {
|
---|
214 | block->angle = 0;
|
---|
215 | break;
|
---|
216 | }
|
---|
217 | w = link.getWord();
|
---|
218 | sum += w;
|
---|
219 | *((uint16_t *)block + i) = w;
|
---|
220 | }
|
---|
221 |
|
---|
222 | if (checksum==sum)
|
---|
223 | blockCount++;
|
---|
224 | else
|
---|
225 | Serial.println("cs error");
|
---|
226 |
|
---|
227 | w = link.getWord();
|
---|
228 | if (w==PIXY_START_WORD)
|
---|
229 | blockType = NORMAL_BLOCK;
|
---|
230 | else if (w==PIXY_START_WORD_CC)
|
---|
231 | blockType = CC_BLOCK;
|
---|
232 | else
|
---|
233 | return blockCount;
|
---|
234 | }
|
---|
235 | }
|
---|
236 |
|
---|
237 | template <class LinkType> int8_t TPixy<LinkType>::setServos(uint16_t s0, uint16_t s1)
|
---|
238 | {
|
---|
239 | uint8_t outBuf[6];
|
---|
240 |
|
---|
241 | outBuf[0] = 0x00;
|
---|
242 | outBuf[1] = 0xff;
|
---|
243 | *(uint16_t *)(outBuf + 2) = s0;
|
---|
244 | *(uint16_t *)(outBuf + 4) = s1;
|
---|
245 |
|
---|
246 | return link.send(outBuf, 6);
|
---|
247 | }
|
---|
248 |
|
---|
249 | template <class LinkType> int8_t TPixy<LinkType>::setBrightness(uint8_t brightness)
|
---|
250 | {
|
---|
251 | uint8_t outBuf[3];
|
---|
252 |
|
---|
253 | outBuf[0] = 0x00;
|
---|
254 | outBuf[1] = 0xfe;
|
---|
255 | outBuf[2] = brightness;
|
---|
256 |
|
---|
257 | return link.send(outBuf, 3);
|
---|
258 | }
|
---|
259 |
|
---|
260 | template <class LinkType> int8_t TPixy<LinkType>::setLED(uint8_t r, uint8_t g, uint8_t b)
|
---|
261 | {
|
---|
262 | uint8_t outBuf[5];
|
---|
263 |
|
---|
264 | outBuf[0] = 0x00;
|
---|
265 | outBuf[1] = 0xfd;
|
---|
266 | outBuf[2] = r;
|
---|
267 | outBuf[3] = g;
|
---|
268 | outBuf[4] = b;
|
---|
269 |
|
---|
270 | return link.send(outBuf, 5);
|
---|
271 | }
|
---|
272 |
|
---|
273 | #endif
|
---|