Pixy.cpp

00001 #include "Pixy.h"
00002 
00003 Pixy::Pixy(Pixy::LinkType linkType, PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk)
00004 {
00005     switch (linkType) {
00006         case SPI:
00007             m_link = new PixyLinkSPI(mosi_sda_tx, miso_scl_rx, sclk);
00008             break;
00009         case I2C:
00010             m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx);
00011             break;
00012         case UART:
00013             m_link = new PixyLinkUART(mosi_sda_tx, miso_scl_rx);
00014             break;
00015     };
00016     pc = 0;
00017     skipStart = false;
00018     blockCount = 0;
00019     blockArraySize = PIXY_INITIAL_ARRAYSIZE;
00020     blocks = new Block[blockArraySize];
00021 }
00022 
00023 Pixy::~Pixy()
00024 {
00025     delete[] blocks;
00026     delete m_link;
00027 }
00028 
00029 uint16_t Pixy::getBlocks(uint16_t maxBlocks)
00030 {
00031     uint8_t i;
00032     uint16_t w, checksum, sum;
00033     Block *block;
00034 
00035     if (!skipStart) {
00036         if (getStart() == false)
00037             return 0;
00038     } else
00039         skipStart = false;
00040 
00041     for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
00042         checksum = m_link->getWord();
00043         if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
00044             skipStart = true;
00045             //if (pc)
00046               //pc->printf("skip\n\r");
00047             return blockCount;
00048         } else if (checksum == 0)
00049             return blockCount;
00050 
00051         if (blockCount > blockArraySize)
00052             resize();
00053 
00054         block = blocks + blockCount;
00055 
00056         for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) {
00057             w = m_link->getWord();
00058             sum += w;
00059             *((uint16_t *)block + i) = w;
00060         }
00061 
00062         if (checksum == sum)
00063             blockCount++;
00064         //else if (pc)
00065             //pc->printf("cs error\n\r");
00066 
00067         w = m_link->getWord();
00068         if (w != PIXY_START_WORD)
00069             return blockCount;
00070     }
00071     return blockCount;
00072 }
00073 
00074 int8_t Pixy::setServos(uint16_t s0, uint16_t s1)
00075 {
00076     uint8_t outBuf[6];
00077 
00078     outBuf[0] = 0x00;
00079     outBuf[1] = 0xff;
00080     *(uint16_t *)(outBuf + 2) = s0;
00081     *(uint16_t *)(outBuf + 4) = s1;
00082 
00083     return m_link->send(outBuf, 6);
00084 }
00085 
00086 void Pixy::setAddress(uint8_t addr)
00087 {
00088     m_link->setAddress(addr);
00089 }
00090 
00091 void Pixy::setSerialOutput(Serial *pc)
00092 {
00093     this->pc = pc;
00094 }
00095 
00096 bool Pixy::getStart()
00097 {
00098     uint16_t w, lastw;
00099 
00100     lastw = 0xffff;
00101     while (true) {
00102         w = m_link->getWord();
00103         if (w == 0 && lastw == 0) {
00104             wait_ms(10);
00105             return false;
00106         } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
00107             return true;
00108         else if (w == PIXY_START_WORDX) {
00109             //if (pc)
00110                // pc->printf("reorder\n\r");
00111             m_link->getByte(); // resync
00112         }
00113         lastw = w;
00114     }
00115 }
00116 
00117 void Pixy::resize()
00118 {
00119     Block *newBlocks;
00120     blockArraySize += PIXY_INITIAL_ARRAYSIZE;
00121     newBlocks = new Block[blockArraySize];
00122     memcpy(newBlocks, blocks, sizeof(Block) * blockCount);
00123     delete[] blocks;
00124     blocks = newBlocks;
00125 }