Diff: Pixy.h

Revision:

0:ef0e3c67dc5b

Child:

1:6587541f3aa8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.h	Mon Mar 14 00:35:23 2016 +0000
@@ -0,0 +1,75 @@
+#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
\ No newline at end of file