Revision 0:4a23b84b733e, committed 2017-05-17

Comitter:

ZHAW_Prometheus

Date:

Wed May 17 13:08:18 2017 +0000

Commit message:

PixyTest2 Vers. 17.05.2017

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.cpp	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,125 @@
+#include "Pixy.h"
+
+Pixy::Pixy(Pixy::LinkType linkType, PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk)
+{
+    switch (linkType) {
+        case SPI:
+            m_link = new PixyLinkSPI(mosi_sda_tx, miso_scl_rx, sclk);
+            break;
+        case I2C:
+            m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx);
+            break;
+        case UART:
+            m_link = new PixyLinkUART(mosi_sda_tx, miso_scl_rx);
+            break;
+    };
+    pc = 0;
+    skipStart = false;
+    blockCount = 0;
+    blockArraySize = PIXY_INITIAL_ARRAYSIZE;
+    blocks = new Block[blockArraySize];
+}
+
+Pixy::~Pixy()
+{
+    delete[] blocks;
+    delete m_link;
+}
+
+uint16_t Pixy::getBlocks(uint16_t maxBlocks)
+{
+    uint8_t i;
+    uint16_t w, checksum, sum;
+    Block *block;
+
+    if (!skipStart) {
+        if (getStart() == false)
+            return 0;
+    } else
+        skipStart = false;
+
+    for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
+        checksum = m_link->getWord();
+        if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
+            skipStart = true;
+            //if (pc)
+            //  pc->printf("skip\n\r");
+            return blockCount;
+        } else if (checksum == 0)
+            return blockCount;
+
+        if (blockCount > blockArraySize)
+            resize();
+
+        block = blocks + blockCount;
+
+        for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) {
+            w = m_link->getWord();
+            sum += w;
+            *((uint16_t *)block + i) = w;
+        }
+
+        if (checksum == sum)
+            blockCount++;
+        else if (pc)
+            pc->printf("cs error\n\r");
+
+        w = m_link->getWord();
+        if (w != PIXY_START_WORD)
+            return blockCount;
+    }
+    return blockCount;
+}
+
+int8_t Pixy::setServos(uint16_t s0, uint16_t s1)
+{
+    uint8_t outBuf[6];
+
+    outBuf[0] = 0x00;
+    outBuf[1] = 0xff;
+    *(uint16_t *)(outBuf + 2) = s0;
+    *(uint16_t *)(outBuf + 4) = s1;
+
+    return m_link->send(outBuf, 6);
+}
+
+void Pixy::setAddress(uint8_t addr)
+{
+    m_link->setAddress(addr);
+}
+
+void Pixy::setSerialOutput(Serial *pc)
+{
+    this->pc = pc;
+}
+
+bool Pixy::getStart()
+{
+    uint16_t w, lastw;
+
+    lastw = 0xffff;
+    while (true) {
+        w = m_link->getWord();
+        if (w == 0 && lastw == 0) {
+            wait_ms(10);
+            return false;
+        } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
+            return true;
+        else if (w == PIXY_START_WORDX) {
+            if (pc)
+                pc->printf("reorder\n\r");
+            m_link->getByte(); // resync
+        }
+        lastw = w;
+    }
+}
+
+void Pixy::resize()
+{
+    Block *newBlocks;
+    blockArraySize += PIXY_INITIAL_ARRAYSIZE;
+    newBlocks = new Block[blockArraySize];
+    memcpy(newBlocks, blocks, sizeof(Block) * blockCount);
+    delete[] blocks;
+    blocks = newBlocks;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.h	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,50 @@
+#ifndef TUIASI_PIXY_H
+#define TUIASI_PIXY_H
+
+#include "mbed.h"
+#include "PixyLinkSPI.h"
+#include "PixyLinkI2C.h"
+#include "PixyLinkUART.h"
+
+struct Block {
+    void print(Serial &pc) {
+        pc.printf("sig: %d x: %d y: %d width: %d height: %d\n", signature, x, y, width, height);
+    };
+    uint16_t signature;
+    uint16_t x;
+    uint16_t y;
+    uint16_t width;
+    uint16_t height;
+};
+
+class Pixy
+{
+public:
+    enum LinkType {SPI, I2C, UART};
+    Pixy(LinkType linkType, PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk = NC);
+    ~Pixy();
+    uint16_t getBlocks(uint16_t maxBlocks=1000);
+    int8_t setServos(uint16_t s0, uint16_t s1);
+    void setAddress(uint8_t addr);
+    void setSerialOutput(Serial *pc);
+
+    Block *blocks;
+
+private:
+    static const uint8_t PIXY_INITIAL_ARRAYSIZE = 30;
+    static const uint8_t PIXY_MAXIMUM_ARRAYSIZE = 130;
+    static const uint16_t PIXY_START_WORD = 0xaa55;
+    static const uint16_t PIXY_START_WORDX = 0x55aa;
+    static const uint8_t PIXY_DEFAULT_ADDR = 0x54;  // I2C
+
+    bool getStart();
+    void resize();
+
+    bool skipStart;
+    uint16_t blockCount;
+    uint16_t blockArraySize;
+
+    PixyLink *m_link;
+    Serial *pc;
+};
+#endif //TUIASI_PIXY_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PixyLink.h	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,21 @@
+#ifndef TUIASI_PIXYLINK_H
+#define TUIASI_PIXYLINK_H
+
+#include "stdint.h"
+
+class PixyLink
+{
+public:
+    PixyLink() { m_addr = 0; };
+    PixyLink(uint8_t addr) : m_addr(addr) {};
+    void setAddress(uint8_t addr) {
+        m_addr = addr;
+    };
+    virtual uint16_t getWord() = 0;
+    virtual uint8_t getByte() = 0;
+    virtual int8_t send(uint8_t *data, uint8_t len) = 0;
+protected:
+    uint8_t m_addr;
+};
+
+#endif //TUIASI_PIXYLINK_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PixyLinkI2C.h	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,35 @@
+#ifndef TUIASI_PIXYLINKI2C_H
+#define TUIASI_PIXYLINKI2C_H
+
+#include "I2C.h"
+#include "PixyLink.h"
+
+class PixyLinkI2C : public PixyLink, private I2C
+{
+public:
+    PixyLinkI2C(PinName sda, PinName scl) :
+        PixyLink(PIXY_DEFAULT_ADDR), I2C(sda, scl) {
+    };
+
+    virtual uint16_t getWord() {
+        uint8_t data[2] = {0, 0};
+        I2C::read((int)m_addr, (char *)data, 2);
+        return ((uint16_t)data[1] << 8) | data[0];
+    };
+
+    virtual uint8_t getByte() {
+        uint8_t data = 0;
+        I2C::read((int)m_addr, (char*)&data, 1);
+        return data;
+    };
+
+    virtual int8_t send(uint8_t *data, uint8_t len) {
+        return I2C::write((int)m_addr, (char*)data, len);
+    };
+
+
+private:
+    static const uint8_t PIXY_DEFAULT_ADDR = 0x54;
+};
+
+#endif //TUIASI_PIXYLINKI2C_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PixyLinkSPI.h	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,46 @@
+#ifndef TUIASI_PIXYLINKSPI_H
+#define TUIASI_PIXYLINKSPI_H
+
+#include "SPI.h"
+#include "PixyLink.h"
+
+class PixyLinkSPI : public PixyLink, private SPI
+{
+public:
+    PixyLinkSPI(PinName mosi, PinName miso, PinName sclk) :
+        SPI(mosi, miso, sclk), outLen(0), outIndex(0) {
+    };
+    
+    virtual uint16_t getWord() {
+        uint16_t w = ((uint16_t)getByte()) << 8;
+        return w | getByte();
+    };
+    
+    virtual uint8_t getByte() {
+        uint8_t c = 0x00;
+        if (outIndex < outLen) {
+            c = outBuf[outIndex++];
+        }
+        return write(c);
+    };
+
+    virtual int8_t send(uint8_t *data, uint8_t len) {
+        if (len > PIXY_OUTBUF_SIZE || outLen != 0)
+            return -1;
+        memcpy(outBuf, data, len);
+        outLen = len;
+        outIndex = 0;
+        return len;
+    };
+
+private:
+    static const uint8_t PIXY_OUTBUF_SIZE = 6;
+    static const uint8_t PIXY_SYNC_BYTE = 0x5a;
+    static const uint8_t PIXY_SYNC_BYTE_DATA = 0x5b;
+
+    uint8_t outBuf[PIXY_OUTBUF_SIZE];
+    uint8_t outLen;
+    uint8_t outIndex;
+
+};
+#endif //TUIASI_PIXYLINKSPI_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PixyLinkUART.h	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,34 @@
+#ifndef TUIASI_PIXYLINKUART_H
+#define TUIASI_PIXYLINKUART_H
+
+#include "Serial.h"
+#include "PixyLink.h"
+
+class PixyLinkUART : public PixyLink, private Serial
+{
+public:
+    PixyLinkUART(PinName tx, PinName rx) :
+        Serial(tx, rx) {
+        baud(19200);
+    };
+
+    virtual uint16_t getWord() {
+        uint8_t data[2] = {0, 0};
+        read(data, 2);
+        return ((uint16_t)data[1] << 8) | data[0];
+    };
+
+    virtual uint8_t getByte() {
+        uint8_t data = 0;
+        read(&data, 1);
+        return data;
+    };
+
+    virtual int8_t send(uint8_t *data, uint8_t len) {
+        return write(data, len);
+    };
+
+
+private:
+};
+#endif //TUIASI_PIXYLINKUART_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,40 @@
+#include "mbed.h"
+#include "Pixy.h"
+#include "cstdlib"
+ 
+Pixy pixy(Pixy::UART, PC_5, PB_10);
+Serial pc(SERIAL_TX, SERIAL_RX);
+ 
+int main() {
+    wait(3);
+    //pc.printf("ready\n\r");
+    printf("ready\n\r");
+    
+    pixy.setSerialOutput(&pc);
+    while (1) {
+        //printf("while");
+        static int i = 0;
+        int j;
+        uint16_t blocks;
+  
+        blocks = pixy.getBlocks();
+  
+        if (blocks) {
+            i++;
+    
+            if (i % 50 == 0) {
+                //pc.printf("Detected %d:\n\r", blocks);
+                printf("Detected %d:\n\r", blocks);
+                for (j = 0; j < blocks; j++) {
+                    printf("  block %d: ", j);
+                    printf("sig: %d x: %d y: %d width: %d height: %d\n", pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
+                    
+                    
+                    //pc.printf("  block %d: ", j);
+                    //pixy.blocks[j].print(pc);
+                }
+            }
+        }
+        wait( 0.01);
+    } 
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed May 17 13:08:18 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/ef9c61f8c49f
\ No newline at end of file