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