This code just added an algorithm to calculate angle form the pixy center
Dependents: pixyHelloWorld2_copy_servoTest pixy_pan_tilt Servo_camera_pan Camera_Gimbal_Tracking
Fork of pixy by
Pixy.cpp
- Committer:
- LucasUIUC
- Date:
- 2015-07-01
- Revision:
- 2:a58a6a71ae7b
- Parent:
- 0:ed8dc4531ac1
File content as of revision 2:a58a6a71ae7b:
#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; }