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(PinName tx, PinName rx)
00024 {
00025     m_link = new PixyLinkUART(tx,rx);
00026     skipStart = false;
00027     blockCount = 0;
00028     blockArraySize = PIXY_INITIAL_ARRAYSIZE;
00029     blocks = new Block[blockArraySize];
00030 }
00031 
00032 Pixy::~Pixy()
00033 {
00034     delete[] blocks;
00035     delete m_link;
00036 }
00037 
00038 uint16_t Pixy::getBlocks(uint16_t maxBlocks)
00039 {
00040     uint8_t i;
00041     uint16_t w, checksum, sum;
00042     Block *block;
00043 
00044     if (!skipStart) {
00045         if (getStart() == false)
00046             return 0;
00047     } else
00048         skipStart = false;
00049 
00050     for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
00051         checksum = m_link->getWord();
00052         if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
00053             skipStart = true;
00054             //if (pc)
00055             //  pc->printf("skip\n\r");
00056             return blockCount;
00057         } else if (checksum == 0)
00058             return blockCount;
00059 
00060         if (blockCount > blockArraySize)
00061             resize();
00062 
00063         block = blocks + blockCount;
00064 
00065         for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) {
00066             w = m_link->getWord();
00067             sum += w;
00068             *((uint16_t *)block + i) = w;
00069         }
00070 
00071         if (checksum == sum)
00072             blockCount++;
00073         else if (pc)
00074             //pc->printf("cs error\n\r");
00075 
00076         w = m_link->getWord();
00077         if (w != PIXY_START_WORD)
00078             return blockCount;
00079     }
00080     return blockCount;
00081 }
00082 
00083 int8_t Pixy::setServos(uint16_t s0, uint16_t s1)
00084 {
00085     uint8_t outBuf[6];
00086 
00087     outBuf[0] = 0x00;
00088     outBuf[1] = 0xff;
00089     *(uint16_t *)(outBuf + 2) = s0;
00090     *(uint16_t *)(outBuf + 4) = s1;
00091 
00092     return m_link->send(outBuf, 6);
00093 }
00094 
00095 void Pixy::setAddress(uint8_t addr)
00096 {
00097     m_link->setAddress(addr);
00098 }
00099 
00100 void Pixy::setSerialOutput(Serial *pc)
00101 {
00102     this->pc = pc;
00103 }
00104 
00105 bool Pixy::getStart()
00106 {
00107     uint16_t w, lastw;
00108 
00109     lastw = 0xffff;
00110     while (true) {
00111         w = m_link->getWord();
00112         if (w == 0 && lastw == 0) {
00113             wait_ms(10);
00114             return false;
00115         } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
00116             return true;
00117         else if (w == PIXY_START_WORDX) {
00118             if (pc)
00119                 //pc->printf("reorder\n\r");
00120             m_link->getByte(); // resync
00121         }
00122         lastw = w;
00123     }
00124 }
00125 
00126 void Pixy::resize()
00127 {
00128     Block *newBlocks;
00129     blockArraySize += PIXY_INITIAL_ARRAYSIZE;
00130     newBlocks = new Block[blockArraySize];
00131     memcpy(newBlocks, blocks, sizeof(Block) * blockCount);
00132     delete[] blocks;
00133     blocks = newBlocks;
00134 }