Stephen Wilkins / TPixy-Interface

A library for interfacing with the Pixy color recognition camera

Dependents:   PixyCamera MbedOS_Robot_Team ManualControlFinal PlayBack ... more

Pixy.h@0:ef0e3c67dc5b, 2016-03-14 (annotated)

Committer:
swilkins8
Date:
Mon Mar 14 00:35:23 2016 +0000
Revision:
0:ef0e3c67dc5b
Child:
1:6587541f3aa8
Completed rewrite of code to expand interfacing options

Who changed what in which revision?

UserRevisionLine numberNew contents of line
swilkins8 0:ef0e3c67dc5b 1 #ifndef _PIXY_H
swilkins8 0:ef0e3c67dc5b 2 #define _PIXY_H
swilkins8 0:ef0e3c67dc5b 3
swilkins8 0:ef0e3c67dc5b 4 #include "TPixy.h"
swilkins8 0:ef0e3c67dc5b 5 #include "TPixyInterface.h"
swilkins8 0:ef0e3c67dc5b 6
swilkins8 0:ef0e3c67dc5b 7 #define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
swilkins8 0:ef0e3c67dc5b 8 #define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2)
swilkins8 0:ef0e3c67dc5b 9
swilkins8 0:ef0e3c67dc5b 10 class PixySPI : public TPixy<PixyInterfaceSPI>
swilkins8 0:ef0e3c67dc5b 11 {
swilkins8 0:ef0e3c67dc5b 12 public:
swilkins8 0:ef0e3c67dc5b 13 PixySPI(SPI* spi, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL) :
swilkins8 0:ef0e3c67dc5b 14 TPixy<PixyInterfaceSPI>((new PixyInterfaceSPI(spi)), serialOut, arg) {}
swilkins8 0:ef0e3c67dc5b 15 };
swilkins8 0:ef0e3c67dc5b 16
swilkins8 0:ef0e3c67dc5b 17
swilkins8 0:ef0e3c67dc5b 18 /* Not Implemented */
swilkins8 0:ef0e3c67dc5b 19 /*
swilkins8 0:ef0e3c67dc5b 20 class PixyI2C : TPixy<PixyInterfaceI2C> {
swilkins8 0:ef0e3c67dc5b 21 public:
swilkins8 0:ef0e3c67dc5b 22 PixyI2C(I2C* i2c, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL) : i2cInterface(i2c), TPixy<PixyInterfaceI2C>(i2cInterface, serialOut, arg);
swilkins8 0:ef0e3c67dc5b 23 private:
swilkins8 0:ef0e3c67dc5b 24 TPixyInterface<I2C>* i2cInterface;
swilkins8 0:ef0e3c67dc5b 25 };
swilkins8 0:ef0e3c67dc5b 26 */
swilkins8 0:ef0e3c67dc5b 27
swilkins8 0:ef0e3c67dc5b 28 class ServoLoop
swilkins8 0:ef0e3c67dc5b 29 {
swilkins8 0:ef0e3c67dc5b 30 public:
swilkins8 0:ef0e3c67dc5b 31 ServoLoop(int32_t pgain, int32_t dgain);
swilkins8 0:ef0e3c67dc5b 32 void update(int32_t error);
swilkins8 0:ef0e3c67dc5b 33
swilkins8 0:ef0e3c67dc5b 34 int32_t m_pos;
swilkins8 0:ef0e3c67dc5b 35 int32_t m_prevError;
swilkins8 0:ef0e3c67dc5b 36 int32_t m_pgain;
swilkins8 0:ef0e3c67dc5b 37 int32_t m_dgain;
swilkins8 0:ef0e3c67dc5b 38 };
swilkins8 0:ef0e3c67dc5b 39
swilkins8 0:ef0e3c67dc5b 40 template <class TPixyInterface> void runPanTiltDemo(TPixy<TPixyInterface>* pixy, Serial* serial)
swilkins8 0:ef0e3c67dc5b 41 {
swilkins8 0:ef0e3c67dc5b 42 ServoLoop panLoop(-150, -300);
swilkins8 0:ef0e3c67dc5b 43 ServoLoop tiltLoop(200, 250);
swilkins8 0:ef0e3c67dc5b 44 if (serial != NULL) {
swilkins8 0:ef0e3c67dc5b 45 serial->printf("Starting...\n");
swilkins8 0:ef0e3c67dc5b 46 }
swilkins8 0:ef0e3c67dc5b 47 static int i = 0;
swilkins8 0:ef0e3c67dc5b 48 int j;
swilkins8 0:ef0e3c67dc5b 49 uint16_t blocks;
swilkins8 0:ef0e3c67dc5b 50 int32_t panError, tiltError;
swilkins8 0:ef0e3c67dc5b 51 pixy->init();
swilkins8 0:ef0e3c67dc5b 52 while(true) {
swilkins8 0:ef0e3c67dc5b 53 blocks = pixy->getBlocks();
swilkins8 0:ef0e3c67dc5b 54 if (blocks) {
swilkins8 0:ef0e3c67dc5b 55 panError = X_CENTER - pixy->blocks[0].x;
swilkins8 0:ef0e3c67dc5b 56 tiltError = pixy->blocks[0].y - Y_CENTER;
swilkins8 0:ef0e3c67dc5b 57
swilkins8 0:ef0e3c67dc5b 58 panLoop.update(panError);
swilkins8 0:ef0e3c67dc5b 59 tiltLoop.update(tiltError);
swilkins8 0:ef0e3c67dc5b 60
swilkins8 0:ef0e3c67dc5b 61 pixy->setServos(panLoop.m_pos, tiltLoop.m_pos);
swilkins8 0:ef0e3c67dc5b 62 i++;
swilkins8 0:ef0e3c67dc5b 63
swilkins8 0:ef0e3c67dc5b 64 if (i % 50 == 0 && serial != NULL) {
swilkins8 0:ef0e3c67dc5b 65 serial->printf("Detected %d:\n", blocks);
swilkins8 0:ef0e3c67dc5b 66 //toPC.printf(buf);
swilkins8 0:ef0e3c67dc5b 67 for (j = 0; j < blocks; j++) {
swilkins8 0:ef0e3c67dc5b 68 serial->printf(" block %d: ", j);
swilkins8 0:ef0e3c67dc5b 69 pixy->printBlock(pixy->blocks[j]);
swilkins8 0:ef0e3c67dc5b 70 }
swilkins8 0:ef0e3c67dc5b 71 }
swilkins8 0:ef0e3c67dc5b 72 }
swilkins8 0:ef0e3c67dc5b 73 }
swilkins8 0:ef0e3c67dc5b 74 }
swilkins8 0:ef0e3c67dc5b 75 #endif