Senior Design Stuff
Fork of pixy by
Pixy.cpp
- Committer:
- shurjo_1234
- Date:
- 2015-04-19
- Revision:
- 2:8bbb50d09c95
- Parent:
- 1:910d941e3cc2
File content as of revision 2:8bbb50d09c95:
#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 (int i = 0; i < PIXY_INITIAL_ARRAYSIZE; i++) { blocks[i].width = 0; } 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; uint16_t signature = m_link->getWord(); uint16_t x = m_link->getWord(); uint16_t y = m_link->getWord(); uint16_t width = m_link->getWord(); uint16_t height = m_link->getWord(); sum = signature + x +y + width + height; // getting the largest block of the corresponding sig if (checksum == sum && blocks[signature-1].width == 0) { blocks[signature-1].x = x; blocks[signature-1].y = y; blocks[signature-1].width = width; blocks[signature-1].height = height; 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; }