1 | #include "r2ca.h"
|
---|
2 |
|
---|
3 | #include <SPI.h>
|
---|
4 | #include <Pixy.h>
|
---|
5 |
|
---|
6 | Pixy pixy;
|
---|
7 |
|
---|
8 | #define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
|
---|
9 | #define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2)
|
---|
10 |
|
---|
11 | class ServoLoop
|
---|
12 | {
|
---|
13 | public:
|
---|
14 | ServoLoop(int32_t pgain, int32_t dgain);
|
---|
15 |
|
---|
16 | void update(int32_t error);
|
---|
17 |
|
---|
18 | int32_t m_pos;
|
---|
19 | int32_t m_prevError;
|
---|
20 | int32_t m_pgain;
|
---|
21 | int32_t m_dgain;
|
---|
22 | };
|
---|
23 |
|
---|
24 | ServoLoop panLoop(300, 500);
|
---|
25 | ServoLoop tiltLoop(500, 700);
|
---|
26 |
|
---|
27 | ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
|
---|
28 | {
|
---|
29 | m_pos = PIXY_RCS_CENTER_POS;
|
---|
30 | m_pgain = pgain;
|
---|
31 | m_dgain = dgain;
|
---|
32 | m_prevError = 0x80000000L;
|
---|
33 | }
|
---|
34 |
|
---|
35 | void ServoLoop::update(int32_t error)
|
---|
36 | {
|
---|
37 | long int vel;
|
---|
38 | char buf[32];
|
---|
39 | if (m_prevError!=0x80000000)
|
---|
40 | {
|
---|
41 | vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
|
---|
42 | //sprintf(buf, "%ld\n", vel);
|
---|
43 | //Serial.print(buf);
|
---|
44 | m_pos += vel;
|
---|
45 | if (m_pos>PIXY_RCS_MAX_POS)
|
---|
46 | m_pos = PIXY_RCS_MAX_POS;
|
---|
47 | else if (m_pos<PIXY_RCS_MIN_POS)
|
---|
48 | m_pos = PIXY_RCS_MIN_POS;
|
---|
49 | }
|
---|
50 | m_prevError = error;
|
---|
51 | }
|
---|
52 |
|
---|
53 | void setup()
|
---|
54 | {
|
---|
55 | Serial.begin(115200);
|
---|
56 | Serial.print("Starting...\n");
|
---|
57 | pixy.init();
|
---|
58 | pixy.setServos(PIXY_RCS_CENTER_POS, PIXY_RCS_CENTER_POS);
|
---|
59 | }
|
---|
60 |
|
---|
61 | void loop()
|
---|
62 | {
|
---|
63 | static int i = 0;
|
---|
64 | int j;
|
---|
65 | uint16_t blocks;
|
---|
66 | char buf[32];
|
---|
67 | int32_t panError, tiltError;
|
---|
68 |
|
---|
69 | blocks = pixy.getBlocks();
|
---|
70 |
|
---|
71 | if (blocks)
|
---|
72 | {
|
---|
73 | panError = X_CENTER-pixy.blocks[0].x;
|
---|
74 | tiltError = pixy.blocks[0].y-Y_CENTER;
|
---|
75 |
|
---|
76 | panLoop.update(panError);
|
---|
77 | tiltLoop.update(tiltError);
|
---|
78 |
|
---|
79 | pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
|
---|
80 |
|
---|
81 | i++;
|
---|
82 |
|
---|
83 | // do this (print) every 50 frames because printing every
|
---|
84 | // frame would bog down the Arduino
|
---|
85 | if (i%50==0)
|
---|
86 | {
|
---|
87 | sprintf(buf, "Detected %d:\n", blocks);
|
---|
88 | Serial.print(buf);
|
---|
89 | for (j=0; j<blocks; j++)
|
---|
90 | {
|
---|
91 | sprintf(buf, " block %d: ", j);
|
---|
92 | Serial.print(buf);
|
---|
93 | pixy.blocks[j].print();
|
---|
94 | Serial.println("");
|
---|
95 | }
|
---|
96 | }
|
---|
97 | }
|
---|
98 | delay(10);
|
---|
99 | }
|
---|