Port of Arduino Pixy Cmucam5 library
Fork of pixy by
Pixy.cpp
- Committer:
- balsamfir
- Date:
- 2016-02-29
- Revision:
- 4:6aa0f748a636
- Parent:
- 3:cc3240a789e9
- Child:
- 6:f1c641be779e
File content as of revision 4:6aa0f748a636:
#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; }