Diff: Pixy.cpp.orig

Revision:

6:f1c641be779e

diff -r 6aa0f748a636 -r f1c641be779e Pixy.cpp.orig
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.cpp.orig	Fri Mar 04 17:33:49 2016 +0000
@@ -0,0 +1,148 @@
+#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, type;
+    Block *block;;
+    if (!skipStart) {
+        if (getStart(&type) == false) {
+            //pc->printf("fuck1 \n\r");
+            return 0;
+        }
+    } else {
+        skipStart = false;
+    }
+
+    for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
+        //pc->printf("tick \n\r");
+        
+        checksum = m_link->getWord();
+        if ((checksum == PIXY_START_WORD)||(checksum == PIXY_START_WORD_CC)) {
+            skipStart = true;
+            //pc->printf("fuck2 \n\r");
+            return blockCount;
+        } else if (checksum == 0) {
+            //pc->printf("fuck3 \n\r");
+            return blockCount;
+        }
+
+        if (blockCount > blockArraySize) resize();
+
+        block = blocks + blockCount;
+        
+        *((uint16_t *)block) = type;
+        
+        for (i = 1, sum = 0; i < (sizeof(Block)/sizeof(uint16_t))-1; i++) {
+            w = m_link->getWord();
+            sum += w;
+            *((int16_t *)block + i) = w;
+        }
+        
+        // Read in the extra word if its a color code
+        if (block->type  == PIXY_START_WORD_CC) {
+            w = m_link->getWord();
+            sum += w;
+            *((uint16_t *)block + i) = w;
+        }
+
+        if (checksum == sum) blockCount++;
+        else if (pc) {
+            //pc->printf("cs error: csum: %d, sum: %d \n\r", checksum, sum);
+            //pc->printf("ERROR: ");
+            block->print(*pc);
+        }
+        
+        // Read in the next block type
+        w = m_link->getWord();
+        if (w == PIXY_START_WORD) {
+            type = PIXY_START_WORD;
+        } else if (w == PIXY_START_WORD_CC) {
+            type = PIXY_START_WORD_CC;
+        } else {
+            return blockCount;
+        }
+    }
+    //pc->printf("fuck4 \n\r");
+    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 *type)
+{
+    uint16_t w, lastw;
+
+    lastw = 0xffff;
+    while (true) {
+        w = m_link->getWord();
+        //pc->printf("word: %04x \r\n", w);
+        if (w == 0 && lastw == 0) {
+            wait_ms(10);
+            return false;
+        } else if ((w == PIXY_START_WORD || w == PIXY_START_WORD_CC) && lastw == PIXY_START_WORD) {
+            *type = w;
+            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