Shurjo Banerjee / pixy

Senior Design Stuff

Fork of pixy by Qisi Wang

Pixy.cpp@2:8bbb50d09c95, 2015-04-19 (annotated)

Committer:
shurjo_1234
Date:
Sun Apr 19 09:54:17 2015 +0000
Revision:
2:8bbb50d09c95
Parent:
1:910d941e3cc2 
Justin sucks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acracan 0:ed8dc4531ac1 1 #include "Pixy.h"
acracan 0:ed8dc4531ac1 2
acracan 0:ed8dc4531ac1 3 Pixy::Pixy(Pixy::LinkType linkType, PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk)
acracan 0:ed8dc4531ac1 4 {
acracan 0:ed8dc4531ac1 5 switch (linkType) {
acracan 0:ed8dc4531ac1 6 case SPI:
acracan 0:ed8dc4531ac1 7 m_link = new PixyLinkSPI(mosi_sda_tx, miso_scl_rx, sclk);
acracan 0:ed8dc4531ac1 8 break;
acracan 0:ed8dc4531ac1 9 case I2C:
acracan 0:ed8dc4531ac1 10 m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx);
acracan 0:ed8dc4531ac1 11 break;
acracan 0:ed8dc4531ac1 12 case UART:
acracan 0:ed8dc4531ac1 13 m_link = new PixyLinkUART(mosi_sda_tx, miso_scl_rx);
acracan 0:ed8dc4531ac1 14 break;
acracan 0:ed8dc4531ac1 15 };
acracan 0:ed8dc4531ac1 16 pc = 0;
acracan 0:ed8dc4531ac1 17 skipStart = false;
acracan 0:ed8dc4531ac1 18 blockCount = 0;
acracan 0:ed8dc4531ac1 19 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
acracan 0:ed8dc4531ac1 20 blocks = new Block[blockArraySize];
acracan 0:ed8dc4531ac1 21 }
acracan 0:ed8dc4531ac1 22
acracan 0:ed8dc4531ac1 23 Pixy::~Pixy()
acracan 0:ed8dc4531ac1 24 {
acracan 0:ed8dc4531ac1 25 delete[] blocks;
acracan 0:ed8dc4531ac1 26 delete m_link;
acracan 0:ed8dc4531ac1 27 }
acracan 0:ed8dc4531ac1 28
acracan 0:ed8dc4531ac1 29 uint16_t Pixy::getBlocks(uint16_t maxBlocks)
acracan 0:ed8dc4531ac1 30 {
acracan 0:ed8dc4531ac1 31 uint8_t i;
acracan 0:ed8dc4531ac1 32 uint16_t w, checksum, sum;
CheeseW 1:910d941e3cc2 33 //Block *block;
acracan 0:ed8dc4531ac1 34
acracan 0:ed8dc4531ac1 35 if (!skipStart) {
acracan 0:ed8dc4531ac1 36 if (getStart() == false)
acracan 0:ed8dc4531ac1 37 return 0;
acracan 0:ed8dc4531ac1 38 } else
acracan 0:ed8dc4531ac1 39 skipStart = false;
CheeseW 1:910d941e3cc2 40 for (int i = 0; i < PIXY_INITIAL_ARRAYSIZE; i++)
CheeseW 1:910d941e3cc2 41 {
CheeseW 1:910d941e3cc2 42 blocks[i].width = 0;
CheeseW 1:910d941e3cc2 43 }
acracan 0:ed8dc4531ac1 44 for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
acracan 0:ed8dc4531ac1 45 checksum = m_link->getWord();
acracan 0:ed8dc4531ac1 46 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
acracan 0:ed8dc4531ac1 47 skipStart = true;
acracan 0:ed8dc4531ac1 48 //if (pc)
acracan 0:ed8dc4531ac1 49 // pc->printf("skip\n\r");
acracan 0:ed8dc4531ac1 50 return blockCount;
acracan 0:ed8dc4531ac1 51 } else if (checksum == 0)
acracan 0:ed8dc4531ac1 52 return blockCount;
acracan 0:ed8dc4531ac1 53
CheeseW 1:910d941e3cc2 54 //if (blockCount > blockArraySize)
CheeseW 1:910d941e3cc2 55 //resize();
CheeseW 1:910d941e3cc2 56
CheeseW 1:910d941e3cc2 57 //block = blocks + blockCount;
CheeseW 1:910d941e3cc2 58 uint16_t signature = m_link->getWord();
CheeseW 1:910d941e3cc2 59 uint16_t x = m_link->getWord();
CheeseW 1:910d941e3cc2 60 uint16_t y = m_link->getWord();
CheeseW 1:910d941e3cc2 61 uint16_t width = m_link->getWord();
CheeseW 1:910d941e3cc2 62 uint16_t height = m_link->getWord();
CheeseW 1:910d941e3cc2 63
CheeseW 1:910d941e3cc2 64 sum = signature + x +y + width + height;
acracan 0:ed8dc4531ac1 65
shurjo_1234 2:8bbb50d09c95 66 // getting the largest block of the corresponding sig
shurjo_1234 2:8bbb50d09c95 67 if (checksum == sum && blocks[signature-1].width == 0)
CheeseW 1:910d941e3cc2 68 {
CheeseW 1:910d941e3cc2 69 blocks[signature-1].x = x;
CheeseW 1:910d941e3cc2 70 blocks[signature-1].y = y;
CheeseW 1:910d941e3cc2 71 blocks[signature-1].width = width;
CheeseW 1:910d941e3cc2 72 blocks[signature-1].height = height;
acracan 0:ed8dc4531ac1 73 blockCount++;
CheeseW 1:910d941e3cc2 74 }
acracan 0:ed8dc4531ac1 75 else if (pc)
acracan 0:ed8dc4531ac1 76 pc->printf("cs error\n\r");
acracan 0:ed8dc4531ac1 77
acracan 0:ed8dc4531ac1 78 w = m_link->getWord();
acracan 0:ed8dc4531ac1 79 if (w != PIXY_START_WORD)
acracan 0:ed8dc4531ac1 80 return blockCount;
acracan 0:ed8dc4531ac1 81 }
acracan 0:ed8dc4531ac1 82 return blockCount;
acracan 0:ed8dc4531ac1 83 }
acracan 0:ed8dc4531ac1 84
acracan 0:ed8dc4531ac1 85 int8_t Pixy::setServos(uint16_t s0, uint16_t s1)
acracan 0:ed8dc4531ac1 86 {
acracan 0:ed8dc4531ac1 87 uint8_t outBuf[6];
acracan 0:ed8dc4531ac1 88
acracan 0:ed8dc4531ac1 89 outBuf[0] = 0x00;
acracan 0:ed8dc4531ac1 90 outBuf[1] = 0xff;
acracan 0:ed8dc4531ac1 91 *(uint16_t *)(outBuf + 2) = s0;
acracan 0:ed8dc4531ac1 92 *(uint16_t *)(outBuf + 4) = s1;
acracan 0:ed8dc4531ac1 93
acracan 0:ed8dc4531ac1 94 return m_link->send(outBuf, 6);
acracan 0:ed8dc4531ac1 95 }
acracan 0:ed8dc4531ac1 96
acracan 0:ed8dc4531ac1 97 void Pixy::setAddress(uint8_t addr)
acracan 0:ed8dc4531ac1 98 {
acracan 0:ed8dc4531ac1 99 m_link->setAddress(addr);
acracan 0:ed8dc4531ac1 100 }
acracan 0:ed8dc4531ac1 101
acracan 0:ed8dc4531ac1 102 void Pixy::setSerialOutput(Serial *pc)
acracan 0:ed8dc4531ac1 103 {
acracan 0:ed8dc4531ac1 104 this->pc = pc;
acracan 0:ed8dc4531ac1 105 }
acracan 0:ed8dc4531ac1 106
acracan 0:ed8dc4531ac1 107 bool Pixy::getStart()
acracan 0:ed8dc4531ac1 108 {
acracan 0:ed8dc4531ac1 109 uint16_t w, lastw;
acracan 0:ed8dc4531ac1 110
acracan 0:ed8dc4531ac1 111 lastw = 0xffff;
acracan 0:ed8dc4531ac1 112 while (true) {
acracan 0:ed8dc4531ac1 113 w = m_link->getWord();
acracan 0:ed8dc4531ac1 114 if (w == 0 && lastw == 0) {
acracan 0:ed8dc4531ac1 115 wait_ms(10);
acracan 0:ed8dc4531ac1 116 return false;
acracan 0:ed8dc4531ac1 117 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
acracan 0:ed8dc4531ac1 118 return true;
acracan 0:ed8dc4531ac1 119 else if (w == PIXY_START_WORDX) {
acracan 0:ed8dc4531ac1 120 if (pc)
acracan 0:ed8dc4531ac1 121 pc->printf("reorder\n\r");
acracan 0:ed8dc4531ac1 122 m_link->getByte(); // resync
acracan 0:ed8dc4531ac1 123 }
acracan 0:ed8dc4531ac1 124 lastw = w;
acracan 0:ed8dc4531ac1 125 }
acracan 0:ed8dc4531ac1 126 }
acracan 0:ed8dc4531ac1 127
acracan 0:ed8dc4531ac1 128 void Pixy::resize()
acracan 0:ed8dc4531ac1 129 {
acracan 0:ed8dc4531ac1 130 Block *newBlocks;
acracan 0:ed8dc4531ac1 131 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
acracan 0:ed8dc4531ac1 132 newBlocks = new Block[blockArraySize];
acracan 0:ed8dc4531ac1 133 memcpy(newBlocks, blocks, sizeof(Block) * blockCount);
acracan 0:ed8dc4531ac1 134 delete[] blocks;
acracan 0:ed8dc4531ac1 135 blocks = newBlocks;
acracan 0:ed8dc4531ac1 136 }