File content as of revision 8:6cd36ded7c4e:

#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) {
            return 0;
        }
    } else {
        skipStart = false;
    }

    for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
        
        checksum = m_link->getWord();
        if ((checksum == PIXY_START_WORD)||(checksum == PIXY_START_WORD_CC)) {
            skipStart = true;
            return blockCount;
        } else if (checksum == 0) {
            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);

    }
    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();
        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;
}