[260] | 1 | #include "r2ca.h"
|
---|
[236] | 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 | }
|
---|