Revision 10:b1bdc51e1c50, committed 2020-11-24

Comitter:

maspadaru

Date:

Tue Nov 24 14:26:05 2020 +0000

Parent:

9:4e71ddde3770

Commit message:

added Pixy2

Changed in this revision

--- a/physcom.h	Sat Nov 03 12:52:25 2018 +0000
+++ b/physcom.h	Tue Nov 24 14:26:05 2020 +0000
@@ -7,5 +7,6 @@
 #include "ov7670/ov7670reg.h"
 #include "rpc/rpc.h"
 #include "rpc/RPCFunction.h"
+#include "pixy2/Pixy2.h"
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy2/Pixy2.h	Tue Nov 24 14:26:05 2020 +0000
@@ -0,0 +1,82 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+// Arduino ICSP SPI link class
+
+#ifndef _PIXY2_H
+#define _PIXY2_H
+
+#include "TPixy2.h"
+#include "mbed.h"
+
+#define PIXY_SPI_CLOCKRATE 2000000
+
+class Link2SPI {
+public:
+    Link2SPI() {
+        spi = new SPI(p5, p6, p7);
+    }
+
+    Link2SPI(SPI *spi) {
+        this->spi = spi;
+    }
+
+    int8_t open(uint32_t arg)
+    {
+        spi->format(8, 3);
+        spi->frequency(PIXY_SPI_CLOCKRATE);
+
+        return 0;
+    }
+
+    void close()
+    {
+    }
+
+    int16_t recv(uint8_t* buf, uint8_t len, uint16_t* cs = NULL)
+    {
+        uint8_t i;
+
+        if (cs) {
+            *cs = 0;
+        }
+        for (i = 0; i < len; i++) {
+            buf[i] = spi->write(0x00);
+            if (cs) {
+                *cs += buf[i];
+            }
+        }
+
+        return len;
+    }
+
+    int16_t send(uint8_t* buf, uint8_t len)
+    {
+        uint8_t i;
+
+        for (i = 0; i < len; i++) {
+            spi->write(buf[i]);
+        }
+
+        return len;
+    }
+
+private:
+    SPI *spi;
+};
+
+typedef TPixy2<Link2SPI> Pixy2;
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy2/Pixy2CCC.h	Tue Nov 24 14:26:05 2020 +0000
@@ -0,0 +1,159 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+// This file is for defining the Block struct and the Pixy template class version 2.
+// (TPixy2).  TPixy takes a communication link as a template parameter so that
+// all communication modes (SPI, I2C and UART) can share the same code.
+//
+
+#ifndef _PIXY2CCC_H
+#define _PIXY2CCC_H
+
+#include <mbed.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#define CCC_MAX_SIGNATURE 7
+
+#define CCC_RESPONSE_BLOCKS 0x21
+#define CCC_REQUEST_BLOCKS 0x20
+
+// Defines for sigmap:
+// You can bitwise "or" these together to make a custom sigmap.
+// For example if you're only interested in receiving blocks
+// with signatures 1 and 5, you could use a sigmap of
+// PIXY_SIG1 | PIXY_SIG5
+#define CCC_SIG1 1
+#define CCC_SIG2 2
+#define CCC_SIG3 4
+#define CCC_SIG4 8
+#define CCC_SIG5 16
+#define CCC_SIG6 32
+#define CCC_SIG7 64
+#define CCC_COLOR_CODES 128
+
+#define CCC_SIG_ALL 0xff // all bits or'ed together
+
+
+/**
+ * @brief Structure of CCC blocks
+ * @see Pixy2CCC::getBlocks
+ */
+struct Block {
+    // print block structure!
+    void print(Serial* serial)
+    {
+        if (serial) {
+            int i, j;
+            char sig[6], d;
+            bool flag;
+            if (m_signature > CCC_MAX_SIGNATURE) // color code! (CC)
+            {
+                // convert signature number to an octal string
+                for (i = 12, j = 0, flag = false; i >= 0; i -= 3) {
+                    d = (m_signature >> i) & 0x07;
+                    if (d > 0 && !flag)
+                        flag = true;
+                    if (flag)
+                        sig[j++] = d + '0';
+                }
+                sig[j] = '\0';
+                serial->printf("CC block sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle: %d index: %d age: %d\n", sig, m_signature, m_x, m_y, m_width, m_height, m_angle, m_index, m_age);
+            } else { // regular block.  Note, angle is always zero, so no need to print
+                serial->printf("sig: %d x: %d y: %d width: %d height: %d index: %d age: %d\n", m_signature, m_x, m_y, m_width, m_height, m_index, m_age);
+            }
+        }
+    }
+
+    uint16_t m_signature;
+    uint16_t m_x;
+    uint16_t m_y;
+    uint16_t m_width;
+    uint16_t m_height;
+    int16_t m_angle;
+    uint8_t m_index;
+    uint8_t m_age;
+};
+
+template <class LinkType>
+class TPixy2;
+
+template <class LinkType>
+class Pixy2CCC {
+public:
+    Pixy2CCC(TPixy2<LinkType>* pixy)
+    {
+        m_pixy = pixy;
+    }
+
+    /**
+     * Gets all detected blocks in the most recent frame.
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t getBlocks(bool wait = true, uint8_t sigmap = CCC_SIG_ALL, uint8_t maxBlocks = 0xff);
+
+    /* The number of blocks contained in the blocks member variable. */
+    uint8_t numBlocks;
+    /** 
+     * This array contains all of the block data as a result of getBlocks(). 
+     * The blocks in this array are sorted by area, with the largest blocks appearing 
+     * first in the blocks array.
+     */
+    Block* blocks;
+
+private:
+    TPixy2<LinkType>* m_pixy;
+};
+
+template <class LinkType>
+int8_t Pixy2CCC<LinkType>::getBlocks(bool wait, uint8_t sigmap, uint8_t maxBlocks)
+{
+    blocks = NULL;
+    numBlocks = 0;
+
+    while (1) {
+        // fill in request data
+        m_pixy->m_bufPayload[0] = sigmap;
+        m_pixy->m_bufPayload[1] = maxBlocks;
+        m_pixy->m_length = 2;
+        m_pixy->m_type = CCC_REQUEST_BLOCKS;
+
+        // send request
+        m_pixy->sendPacket();
+        if (m_pixy->recvPacket() == 0) {
+            if (m_pixy->m_type == CCC_RESPONSE_BLOCKS) {
+                blocks = (Block*)m_pixy->m_buf;
+                numBlocks = m_pixy->m_length / sizeof(Block);
+                return numBlocks;
+            }
+            // deal with busy and program changing states from Pixy (we'll wait)
+            else if (m_pixy->m_type == PIXY_TYPE_RESPONSE_ERROR) {
+                if ((int8_t)m_pixy->m_buf[0] == PIXY_RESULT_BUSY) {
+                    if (!wait)
+                        return PIXY_RESULT_BUSY; // new data not available yet
+                } else if ((int8_t)m_pixy->m_buf[0] != PIXY_RESULT_PROG_CHANGING)
+                    return m_pixy->m_buf[0];
+            }
+        } else
+            return PIXY_RESULT_ERROR; // some kind of bitstream error
+
+        // If we're waiting for frame data, don't thrash Pixy with requests.
+        // We can give up half a millisecond of latency (worst case)
+        wait_ms(500);
+    }
+}
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy2/Pixy2Line.h	Tue Nov 24 14:26:05 2020 +0000
@@ -0,0 +1,350 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+// This file is for defining the Block struct and the Pixy template class version 2.
+// (TPixy2).  TPixy takes a communication link as a template parameter so that
+// all communication modes (SPI, I2C and UART) can share the same code.
+//
+
+#ifndef _PIXY2LINE_H
+#define _PIXY2LINE_H
+
+#define LINE_REQUEST_GET_FEATURES 0x30
+#define LINE_RESPONSE_GET_FEATURES 0x31
+#define LINE_REQUEST_SET_MODE 0x36
+#define LINE_REQUEST_SET_VECTOR 0x38
+#define LINE_REQUEST_SET_NEXT_TURN_ANGLE 0x3a
+#define LINE_REQUEST_SET_DEFAULT_TURN_ANGLE 0x3c
+#define LINE_REQUEST_REVERSE_VECTOR 0x3e
+
+#define LINE_GET_MAIN_FEATURES 0x00
+#define LINE_GET_ALL_FEATURES 0x01
+
+#define LINE_MODE_TURN_DELAYED 0x01
+#define LINE_MODE_MANUAL_SELECT_VECTOR 0x02
+#define LINE_MODE_WHITE_LINE 0x80
+
+// features
+#define LINE_VECTOR 0x01
+#define LINE_INTERSECTION 0x02
+#define LINE_BARCODE 0x04
+#define LINE_ALL_FEATURES (LINE_VECTOR | LINE_INTERSECTION | LINE_BARCODE)
+
+#define LINE_FLAG_INVALID 0x02
+#define LINE_FLAG_INTERSECTION_PRESENT 0x04
+
+#define LINE_MAX_INTERSECTION_LINES 6
+
+#include <stdint.h>
+
+struct Vector {
+    void print(Serial* serial)
+    {
+        if (serial) {
+            serial->printf("vector: (%d %d) (%d %d) index: %d flags %d\n", m_x0, m_y0, m_x1, m_y1, m_index, m_flags);
+        }
+    }
+
+    uint8_t m_x0;
+    uint8_t m_y0;
+    uint8_t m_x1;
+    uint8_t m_y1;
+    uint8_t m_index;
+    uint8_t m_flags;
+};
+
+struct IntersectionLine {
+    uint8_t m_index;
+    uint8_t m_reserved;
+    int16_t m_angle;
+};
+
+struct Intersection {
+    void print(Serial* serial)
+    {
+        if (serial) {
+            uint8_t i;
+            serial->printf("intersection: (%d %d)\n", m_x, m_y);
+            for (i = 0; i < m_n; i++) {
+                serial->printf("  %d: index: %d angle: %d\n", i, m_intLines[i].m_index, m_intLines[i].m_angle);
+            }
+        }
+    }
+
+    uint8_t m_x;
+    uint8_t m_y;
+
+    uint8_t m_n;
+    uint8_t m_reserved;
+    IntersectionLine m_intLines[LINE_MAX_INTERSECTION_LINES];
+};
+
+struct Barcode {
+    void print(Serial* serial)
+    {
+        if (serial) {
+            serial->printf("Barcode: (%d %d), val: %d flags: %d\n", m_x, m_y, m_code, m_flags);
+        }
+    }
+
+    uint8_t m_x;
+    uint8_t m_y;
+    uint8_t m_flags;
+    uint8_t m_code;
+};
+
+template <class LinkType>
+class TPixy2;
+
+template <class LinkType>
+class Pixy2Line {
+public:
+    Pixy2Line(TPixy2<LinkType>* pixy)
+    {
+        m_pixy = pixy;
+    }
+
+    /**
+     * This function gets the latest features including the Vector, any intersection that connects to the Vector, and barcodes. 
+     * 
+     * The results are returned in the variables {@link vectors}, {@link intersections}, and {@link barcodes}, respectively.
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t getMainFeatures(uint8_t features = LINE_ALL_FEATURES, bool wait = true)
+    {
+        return getFeatures(LINE_GET_MAIN_FEATURES, features, wait);
+    }
+
+    /**
+     * This function returns all lines, intersections and barcodes that the line tracking algorithm detects. 
+     * 
+     * The results are returned in the variables {@link vectors}, {@link intersections}, and {@link barcodes}, respectively.
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t getAllFeatures(uint8_t features = LINE_ALL_FEATURES, bool wait = true)
+    {
+        return getFeatures(LINE_GET_ALL_FEATURES, features, wait);
+    }
+
+    /**
+     * This function sets various modes in the line tracking algorithm.
+     * 
+     * @param mode argument consists of a bitwise-ORing of the following Values: 
+     *      LINE_MODE_TURN_DELAYED : 
+     *          will prevent the line tracking algorithm from choosing the path automatically; 
+     *      LINE_MODE_MANUAL_SELECT_VECTOR :
+     *          will prevent the line tracking algorithm from choosing the Vector automatically; 
+     *      LINE_MODE_WHITE_LINE :
+     *          will instruct the line tracking algorithm to look for light lines on a dark background.
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setMode(uint8_t mode);
+
+    /**
+     * This function tells the line tracking algorithm which path it should take at the next intersection.
+     * 
+     * setNextTurn() will remember the turn angle you give it, and execute it at the next intersection. 
+     * The line tracking algorithm will then go back to the default turn angle for subsequent intersections.
+     * 
+     * @param angle turn angle in degrees, with 0 being straight ahead. Valide angles are between -180 and 180
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setNextTurn(int16_t angle);
+
+    /**
+     * This function tells the line tracking algorithm which path to choose by default upon encountering an intersection.
+     * 
+     * @param angle turn angle in degrees, with 0 being straight ahead. Valide angles are between -180 and 180
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setDefaultTurn(int16_t angle);
+
+    /**
+     * Set the vector the line tracking algorithm will follow if the LINE_MODE_MANUAL_SELECT_VECTOR mode bit is set.
+     * 
+     * @param index vector provided by index of the line
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setVector(uint8_t index);
+
+    /**
+     * Reverse direction of the vector the line tracking algorithm is currently following.
+     * 
+     * The Vector has a direction. It normally points up, from the bottom of the camera frame to the top of the 
+     * camera frame for a forward-moving robot. Calling reverseVector() will invert the vector. 
+     * This will typically cause your robot to back-up and change directions.
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t reverseVector();
+
+    uint8_t numVectors;
+    Vector* vectors;
+
+    uint8_t numIntersections;
+    Intersection* intersections;
+
+    uint8_t numBarcodes;
+    Barcode* barcodes;
+
+private:
+    int8_t getFeatures(uint8_t type, uint8_t features, bool wait);
+    TPixy2<LinkType>* m_pixy;
+};
+
+template <class LinkType>
+int8_t Pixy2Line<LinkType>::getFeatures(uint8_t type, uint8_t features, bool wait)
+{
+    int8_t res;
+    uint8_t offset, fsize, ftype, *fdata;
+
+    vectors = NULL;
+    numVectors = 0;
+    intersections = NULL;
+    numIntersections = 0;
+    barcodes = NULL;
+    numBarcodes = 0;
+
+    while (1) {
+        // fill in request data
+        m_pixy->m_length = 2;
+        m_pixy->m_type = LINE_REQUEST_GET_FEATURES;
+        m_pixy->m_bufPayload[0] = type;
+        m_pixy->m_bufPayload[1] = features;
+
+        // send request
+        m_pixy->sendPacket();
+        if (m_pixy->recvPacket() == 0) {
+            if (m_pixy->m_type == LINE_RESPONSE_GET_FEATURES) {
+                // parse line response
+                for (offset = 0, res = 0; m_pixy->m_length > offset; offset += fsize + 2) {
+                    ftype = m_pixy->m_buf[offset];
+                    fsize = m_pixy->m_buf[offset + 1];
+                    fdata = &m_pixy->m_buf[offset + 2];
+                    if (ftype == LINE_VECTOR) {
+                        vectors = (Vector*)fdata;
+                        numVectors = fsize / sizeof(Vector);
+                        res |= LINE_VECTOR;
+                    } else if (ftype == LINE_INTERSECTION) {
+                        intersections = (Intersection*)fdata;
+                        numIntersections = fsize / sizeof(Intersection);
+                        res |= LINE_INTERSECTION;
+                    } else if (ftype == LINE_BARCODE) {
+                        barcodes = (Barcode*)fdata;
+                        numBarcodes = fsize / sizeof(Barcode);
+                        ;
+                        res |= LINE_BARCODE;
+                    } else
+                        break; // parse error
+                }
+                return res;
+            } else if (m_pixy->m_type == PIXY_TYPE_RESPONSE_ERROR) {
+                // if it's not a busy response, return the error
+                if ((int8_t)m_pixy->m_buf[0] != PIXY_RESULT_BUSY)
+                    return m_pixy->m_buf[0];
+                else if (!wait) // we're busy
+                    return PIXY_RESULT_BUSY; // new data not available yet
+            }
+        } else
+            return PIXY_RESULT_ERROR; // some kind of bitstream error
+
+        // If we're waiting for frame data, don't thrash Pixy with requests.
+        // We can give up half a millisecond of latency (worst case)
+        wait_ms(500);
+    }
+}
+
+template <class LinkType>
+int8_t Pixy2Line<LinkType>::setMode(uint8_t mode)
+{
+    uint32_t res;
+
+    *(int8_t*)m_pixy->m_bufPayload = mode;
+    m_pixy->m_length = 1;
+    m_pixy->m_type = LINE_REQUEST_SET_MODE;
+    m_pixy->sendPacket();
+    if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
+        res = *(uint32_t*)m_pixy->m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t Pixy2Line<LinkType>::setNextTurn(int16_t angle)
+{
+    uint32_t res;
+
+    *(int16_t*)m_pixy->m_bufPayload = angle;
+    m_pixy->m_length = 2;
+    m_pixy->m_type = LINE_REQUEST_SET_NEXT_TURN_ANGLE;
+    m_pixy->sendPacket();
+    if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
+        res = *(uint32_t*)m_pixy->m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t Pixy2Line<LinkType>::setDefaultTurn(int16_t angle)
+{
+    uint32_t res;
+
+    *(int16_t*)m_pixy->m_bufPayload = angle;
+    m_pixy->m_length = 2;
+    m_pixy->m_type = LINE_REQUEST_SET_DEFAULT_TURN_ANGLE;
+    m_pixy->sendPacket();
+    if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
+        res = *(uint32_t*)m_pixy->m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t Pixy2Line<LinkType>::setVector(uint8_t index)
+{
+    uint32_t res;
+
+    *(int8_t*)m_pixy->m_bufPayload = index;
+    m_pixy->m_length = 1;
+    m_pixy->m_type = LINE_REQUEST_SET_VECTOR;
+    m_pixy->sendPacket();
+    if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
+        res = *(uint32_t*)m_pixy->m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t Pixy2Line<LinkType>::reverseVector()
+{
+    uint32_t res;
+
+    m_pixy->m_length = 0;
+    m_pixy->m_type = LINE_REQUEST_REVERSE_VECTOR;
+    m_pixy->sendPacket();
+    if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
+        res = *(uint32_t*)m_pixy->m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy2/Pixy2Video.h	Tue Nov 24 14:26:05 2020 +0000
@@ -0,0 +1,80 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+// This file is for defining the Block struct and the Pixy template class version 2.
+// (TPixy2).  TPixy takes a communication link as a template parameter so that
+// all communication modes (SPI, I2C and UART) can share the same code.
+//
+
+#ifndef _PIXY2VIDEO_H
+#define _PIXY2VIDEO_H
+
+#define VIDEO_REQUEST_GET_RGB 0x70
+
+template <class LinkType>
+class TPixy2;
+
+template <class LinkType>
+class Pixy2Video {
+public:
+    Pixy2Video(TPixy2<LinkType>* pixy)
+    {
+        m_pixy = pixy;
+    }
+
+    /**
+     * This function gets the RGB value of a pixel in the camera frame.
+     * 
+     * @param x x-position of the pixel. top-left is 0,0
+     * @param y y-position of the pixel. top-left is 0,0
+     * @param r uint8_t that will be populated with the red value
+     * @param g uint8_t that will be populated with the gree value
+     * @param b uint8_t that will be populated with the blue value
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+    */
+    int8_t getRGB(uint16_t x, uint16_t y, uint8_t* r, uint8_t* g, uint8_t* b, bool saturate = true);
+
+private:
+    TPixy2<LinkType>* m_pixy;
+};
+
+template <class LinkType>
+int8_t Pixy2Video<LinkType>::getRGB(uint16_t x, uint16_t y, uint8_t* r, uint8_t* g, uint8_t* b, bool saturate)
+{
+    while (1) {
+        *(int16_t*)(m_pixy->m_bufPayload + 0) = x;
+        *(int16_t*)(m_pixy->m_bufPayload + 2) = y;
+        *(m_pixy->m_bufPayload + 4) = saturate;
+        m_pixy->m_length = 5;
+        m_pixy->m_type = VIDEO_REQUEST_GET_RGB;
+        m_pixy->sendPacket();
+        if (m_pixy->recvPacket() == 0) {
+            if (m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
+                *b = *(m_pixy->m_buf + 0);
+                *g = *(m_pixy->m_buf + 1);
+                *r = *(m_pixy->m_buf + 2);
+                return 0;
+            }
+            // deal with program changing
+            else if (m_pixy->m_type == PIXY_TYPE_RESPONSE_ERROR && (int8_t)m_pixy->m_buf[0] == PIXY_RESULT_PROG_CHANGING) {
+                wait_ms(500); // don't be a drag
+                continue;
+            }
+        }
+        return PIXY_RESULT_ERROR;
+    }
+}
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy2/TPixy2.h	Tue Nov 24 14:26:05 2020 +0000
@@ -0,0 +1,492 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+// Main Pixy template class.  This class takes a link class and uses
+// it to communicate with Pixy over I2C, SPI, UART or USB using the
+// Pixy packet protocol.
+
+#ifndef _TPIXY2_H
+#define _TPIXY2_H
+
+// uncomment to turn on debug prints to console
+#define PIXY_DEBUG
+
+#define PIXY_DEFAULT_ARGVAL 0x80000000
+#define PIXY_BUFFERSIZE 0x104
+#define PIXY_CHECKSUM_SYNC 0xc1af
+#define PIXY_NO_CHECKSUM_SYNC 0xc1ae
+#define PIXY_SEND_HEADER_SIZE 4
+#define PIXY_MAX_PROGNAME 33
+
+#define PIXY_TYPE_REQUEST_CHANGE_PROG 0x02
+#define PIXY_TYPE_REQUEST_RESOLUTION 0x0c
+#define PIXY_TYPE_RESPONSE_RESOLUTION 0x0d
+#define PIXY_TYPE_REQUEST_VERSION 0x0e
+#define PIXY_TYPE_RESPONSE_VERSION 0x0f
+#define PIXY_TYPE_RESPONSE_RESULT 0x01
+#define PIXY_TYPE_RESPONSE_ERROR 0x03
+#define PIXY_TYPE_REQUEST_BRIGHTNESS 0x10
+#define PIXY_TYPE_REQUEST_SERVO 0x12
+#define PIXY_TYPE_REQUEST_LED 0x14
+#define PIXY_TYPE_REQUEST_LAMP 0x16
+#define PIXY_TYPE_REQUEST_FPS 0x18
+
+#define PIXY_RESULT_OK 0
+#define PIXY_RESULT_ERROR -1
+#define PIXY_RESULT_BUSY -2
+#define PIXY_RESULT_CHECKSUM_ERROR -3
+#define PIXY_RESULT_TIMEOUT -4
+#define PIXY_RESULT_BUTTON_OVERRIDE -5
+#define PIXY_RESULT_PROG_CHANGING -6
+
+// RC-servo values
+#define PIXY_RCS_MIN_POS 0
+#define PIXY_RCS_MAX_POS 1000L
+#define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS - PIXY_RCS_MIN_POS) / 2)
+
+#include "Pixy2CCC.h"
+#include "Pixy2Line.h"
+#include "Pixy2Video.h"
+#include "mbed.h"
+
+struct Version {
+    void print(Serial* serial)
+    {
+        if (serial) {
+            serial->printf("hardware ver: 0x%x firmware ver: %d.%d.%d %s\n", hardware, firmwareMajor, firmwareMinor, firmwareBuild, firmwareType);
+        }
+    }
+
+    // This variable contains the hardware version number.
+    uint16_t hardware;
+    // This variable contains the firmware major version number.
+    uint8_t firmwareMajor;
+    // This variable contains the firmware minor version number.
+    uint8_t firmwareMinor;
+    // This variable contains the firmware build version number.
+    uint16_t firmwareBuild;
+    // This variable contains the a string description of the firmware type.
+    char firmwareType[10];
+};
+
+template <class LinkType>
+class TPixy2 {
+public:
+    TPixy2();
+    TPixy2(SPI* spi);
+    ~TPixy2();
+
+    int8_t init(uint32_t arg = PIXY_DEFAULT_ARGVAL);
+
+    /** 
+     * Queries and receives the firmware and hardware version of Pixy2.
+     * It is called automatically as part of init().
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t getVersion();
+
+    /** 
+     * Instructs Pixy2 to switch programs.
+     * 
+     * @param prog string with the program name, Values: @TODO
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t changeProg(const char* prog);
+
+    /** 
+     * Sets the servo positions of servos plugged into Pixy2's two RC servo connectors.
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setServos(uint16_t s0, uint16_t s1);
+
+    /** 
+     * Sets the relative exposure level of Pixy2's image sensor.
+     * 
+     * @param brightness value between 0(lowest brightness) and 255(max brightness)
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setCameraBrightness(uint8_t brightness);
+
+    /** 
+     * Sets Pixy2's RGB LED value.
+     * 
+     * @param r red led brightness value between 0 and 255
+     * @param g green led brightness value between 0 and 255
+     * @param b blue led brighenss value between 0 and 255
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
+
+    /** 
+     * Turns on/off Pixy2's integrated light source.
+     * 
+     * @param upper controls the two white LEDs along the top edge of Pixy2's PCB. Values: 0: off; 1: on
+     * @param lower controls the RGB LED, turning on/off all three color channel, Values: 0: off; 1: on
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t setLamp(uint8_t upper, uint8_t lower);
+
+    /** 
+     * Gets the width and height of the frames used by the current program.
+     * After calling this function, the width and height can be found in 
+     * the frameWidth and frameHeight member variables
+     * 
+     * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
+     */
+    int8_t getResolution();
+
+    /** 
+     * Gets Pixy2's framerate.
+     * 
+     * @returns a value in the range between 2 and 62 frames per second.
+     */
+    int8_t getFPS();
+
+    Version* version;
+    uint16_t frameWidth;
+    uint16_t frameHeight;
+
+    // Color connected components, color codes
+    Pixy2CCC<LinkType> ccc;
+    friend class Pixy2CCC<LinkType>;
+
+    // Line following
+    Pixy2Line<LinkType> line;
+    friend class Pixy2Line<LinkType>;
+
+    // Video
+    Pixy2Video<LinkType> video;
+    friend class Pixy2Video<LinkType>;
+
+    LinkType m_link;
+
+private:
+    void prepareBuffer();
+    int16_t getSync();
+    int16_t recvPacket();
+    int16_t sendPacket();
+
+    uint8_t* m_buf;
+    uint8_t* m_bufPayload;
+    uint8_t m_type;
+    uint8_t m_length;
+    bool m_cs;
+};
+
+template <class LinkType>
+TPixy2<LinkType>::TPixy2()
+    : ccc(this)
+    , line(this)
+    , video(this)
+{
+    prepareBuffer();
+}
+
+template <class LinkType>
+TPixy2<LinkType>::TPixy2(SPI* spi)
+    : ccc(this)
+    , line(this)
+    , video(this)
+{
+    prepareBuffer();
+}
+
+template <class LinkType>
+TPixy2<LinkType>::~TPixy2()
+{
+    m_link.close();
+    free(m_buf);
+}
+
+template <class LinkType>
+void TPixy2<LinkType>::prepareBuffer()
+{
+    // allocate buffer space for send/receive
+    m_buf = (uint8_t*)malloc(PIXY_BUFFERSIZE);
+    // shifted buffer is used for sending, so we have space to write header information
+    m_bufPayload = m_buf + PIXY_SEND_HEADER_SIZE;
+    frameWidth = frameHeight = 0;
+    version = NULL;
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::init(uint32_t arg)
+{
+    uint32_t t0;
+    int8_t res;
+    Timer timer;
+
+    res = m_link.open(arg);
+    if (res < 0)
+        return res;
+
+    timer.start();
+    // wait for pixy to be ready -- that is, Pixy takes a second or 2 boot up
+    // getVersion is an effective "ping".  We timeout after 5s.
+    for (t0 = timer.read_ms(); timer.read_ms() - t0 < 5000;) {
+        if (getVersion() >= 0) // successful version get -> pixy is ready
+        {
+            getResolution(); // get resolution so we have it
+            return PIXY_RESULT_OK;
+        }
+        wait_ms(5000); // delay for sync
+    }
+    // timeout
+    return PIXY_RESULT_TIMEOUT;
+}
+
+template <class LinkType>
+int16_t TPixy2<LinkType>::getSync()
+{
+    uint8_t i, j, c, cprev;
+    int16_t res;
+    uint16_t start;
+
+    // parse bytes until we find sync
+    for (i = j = 0, cprev = 0; true; i++) {
+        res = m_link.recv(&c, 1);
+        if (res >= PIXY_RESULT_OK) {
+            // since we're using little endian, previous byte is least significant byte
+            start = cprev;
+            // current byte is most significant byte
+            start |= c << 8;
+            cprev = c;
+            if (start == PIXY_CHECKSUM_SYNC) {
+                m_cs = true;
+                return PIXY_RESULT_OK;
+            }
+            if (start == PIXY_NO_CHECKSUM_SYNC) {
+                m_cs = false;
+                return PIXY_RESULT_OK;
+            }
+        }
+        // If we've read some bytes and no sync, then wait and try again.
+        // And do that several more times before we give up.
+        // Pixy guarantees to respond within 100us.
+        if (i >= 4) {
+            if (j >= 4) {
+#ifdef PIXY_DEBUG
+                // Serial.println("error: no response");
+#endif
+                return PIXY_RESULT_ERROR;
+            }
+            wait_ms(25);
+            j++;
+            i = 0;
+        }
+    }
+}
+
+template <class LinkType>
+int16_t TPixy2<LinkType>::recvPacket()
+{
+    uint16_t csCalc, csSerial;
+    int16_t res;
+
+    res = getSync();
+    if (res < 0)
+        return res;
+
+    if (m_cs) {
+        res = m_link.recv(m_buf, 4);
+        if (res < 0)
+            return res;
+
+        m_type = m_buf[0];
+        m_length = m_buf[1];
+
+        csSerial = *(uint16_t*)&m_buf[2];
+
+        res = m_link.recv(m_buf, m_length, &csCalc);
+        if (res < 0)
+            return res;
+
+        if (csSerial != csCalc) {
+#ifdef PIXY_DEBUG
+            // Serial.println("error: checksum");
+#endif
+            return PIXY_RESULT_CHECKSUM_ERROR;
+        }
+    } else {
+        res = m_link.recv(m_buf, 2);
+        if (res < 0)
+            return res;
+
+        m_type = m_buf[0];
+        m_length = m_buf[1];
+
+        res = m_link.recv(m_buf, m_length);
+        if (res < 0)
+            return res;
+    }
+    return PIXY_RESULT_OK;
+}
+
+template <class LinkType>
+int16_t TPixy2<LinkType>::sendPacket()
+{
+    // write header info at beginnig of buffer
+    m_buf[0] = PIXY_NO_CHECKSUM_SYNC & 0xff;
+    m_buf[1] = PIXY_NO_CHECKSUM_SYNC >> 8;
+    m_buf[2] = m_type;
+    m_buf[3] = m_length;
+    // send whole thing -- header and data in one call
+    return m_link.send(m_buf, m_length + PIXY_SEND_HEADER_SIZE);
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::changeProg(const char* prog)
+{
+    int32_t res;
+
+    // poll for program to change
+    while (1) {
+        strncpy((char*)m_bufPayload, prog, PIXY_MAX_PROGNAME);
+        m_length = PIXY_MAX_PROGNAME;
+        m_type = PIXY_TYPE_REQUEST_CHANGE_PROG;
+        sendPacket();
+        if (recvPacket() == 0) {
+            res = *(uint32_t*)m_buf;
+            if (res > 0) {
+                getResolution(); // get resolution so we have it
+                return PIXY_RESULT_OK; // success
+            }
+        } else
+            return PIXY_RESULT_ERROR; // some kind of bitstream error
+        wait_ms(1000);
+    }
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::getVersion()
+{
+    m_length = 0;
+    m_type = PIXY_TYPE_REQUEST_VERSION;
+    sendPacket();
+    if (recvPacket() == 0) {
+        if (m_type == PIXY_TYPE_RESPONSE_VERSION) {
+            version = (Version*)m_buf;
+            return m_length;
+        } else if (m_type == PIXY_TYPE_RESPONSE_ERROR)
+            return PIXY_RESULT_BUSY;
+    }
+    return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::getResolution()
+{
+    m_length = 1;
+    m_bufPayload[0] = 0; // for future types of queries
+    m_type = PIXY_TYPE_REQUEST_RESOLUTION;
+    sendPacket();
+    if (recvPacket() == 0) {
+        if (m_type == PIXY_TYPE_RESPONSE_RESOLUTION) {
+            frameWidth = *(uint16_t*)m_buf;
+            frameHeight = *(uint16_t*)(m_buf + sizeof(uint16_t));
+            return PIXY_RESULT_OK; // success
+        } else
+            return PIXY_RESULT_ERROR;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::setCameraBrightness(uint8_t brightness)
+{
+    uint32_t res;
+
+    m_bufPayload[0] = brightness;
+    m_length = 1;
+    m_type = PIXY_TYPE_REQUEST_BRIGHTNESS;
+    sendPacket();
+    if (recvPacket() == 0) // && m_type==PIXY_TYPE_RESPONSE_RESULT && m_length==4)
+    {
+        res = *(uint32_t*)m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::setServos(uint16_t s0, uint16_t s1)
+{
+    uint32_t res;
+
+    *(int16_t*)(m_bufPayload + 0) = s0;
+    *(int16_t*)(m_bufPayload + 2) = s1;
+    m_length = 4;
+    m_type = PIXY_TYPE_REQUEST_SERVO;
+    sendPacket();
+    if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
+        res = *(uint32_t*)m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::setLED(uint8_t r, uint8_t g, uint8_t b)
+{
+    uint32_t res;
+
+    m_bufPayload[0] = r;
+    m_bufPayload[1] = g;
+    m_bufPayload[2] = b;
+    m_length = 3;
+    m_type = PIXY_TYPE_REQUEST_LED;
+    sendPacket();
+    if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
+        res = *(uint32_t*)m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::setLamp(uint8_t upper, uint8_t lower)
+{
+    uint32_t res;
+
+    m_bufPayload[0] = upper;
+    m_bufPayload[1] = lower;
+    m_length = 2;
+    m_type = PIXY_TYPE_REQUEST_LAMP;
+    sendPacket();
+    if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
+        res = *(uint32_t*)m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+template <class LinkType>
+int8_t TPixy2<LinkType>::getFPS()
+{
+    uint32_t res;
+
+    m_length = 0; // no args
+    m_type = PIXY_TYPE_REQUEST_FPS;
+    sendPacket();
+    if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
+        res = *(uint32_t*)m_buf;
+        return (int8_t)res;
+    } else
+        return PIXY_RESULT_ERROR; // some kind of bitstream error
+}
+
+#endif
+