File content as of revision 0:ef0e3c67dc5b:

#ifndef _PIXY_H
#define _PIXY_H

#include "TPixy.h"
#include "TPixyInterface.h"

#define X_CENTER        ((PIXY_MAX_X-PIXY_MIN_X)/2)
#define Y_CENTER        ((PIXY_MAX_Y-PIXY_MIN_Y)/2)

class PixySPI : public TPixy<PixyInterfaceSPI>
{
public:
    PixySPI(SPI* spi, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL) :
        TPixy<PixyInterfaceSPI>((new PixyInterfaceSPI(spi)), serialOut, arg) {}
};


/* Not Implemented */
/*
class PixyI2C : TPixy<PixyInterfaceI2C> {
    public:
        PixyI2C(I2C* i2c, Serial* serialOut = NULL, uint16_t arg = PIXY_DEFAULT_ARGVAL) : i2cInterface(i2c), TPixy<PixyInterfaceI2C>(i2cInterface, serialOut, arg);
    private:
        TPixyInterface<I2C>* i2cInterface;
    };
    */
    
class ServoLoop
{
public:
    ServoLoop(int32_t pgain, int32_t dgain);
    void update(int32_t error);

    int32_t m_pos;
    int32_t m_prevError;
    int32_t m_pgain;
    int32_t m_dgain;
};

template <class TPixyInterface> void runPanTiltDemo(TPixy<TPixyInterface>* pixy, Serial* serial)
{
    ServoLoop panLoop(-150, -300);
    ServoLoop tiltLoop(200, 250);
    if (serial != NULL) {
        serial->printf("Starting...\n");
    }   
    static int i = 0;
    int j;
    uint16_t blocks;
    int32_t panError, tiltError;
    pixy->init();
    while(true) {
        blocks = pixy->getBlocks();
        if (blocks) {
            panError = X_CENTER - pixy->blocks[0].x;
            tiltError = pixy->blocks[0].y - Y_CENTER;

            panLoop.update(panError);
            tiltLoop.update(tiltError);

            pixy->setServos(panLoop.m_pos, tiltLoop.m_pos);
            i++;

            if (i % 50 == 0 && serial != NULL) {
                serial->printf("Detected %d:\n", blocks);
                //toPC.printf(buf);
                for (j = 0; j < blocks; j++) {
                    serial->printf("  block %d: ", j);
                    pixy->printBlock(pixy->blocks[j]);
                }
            }
        }
    }
}
#endif