pixy Libari 2
Dependents: PES PES1 PES4 PES3
Pixy.cpp@1:a57585d5e81b, 2017-03-29 (annotated)
- Committer:
- scholfa2
- Date:
- Wed Mar 29 12:34:18 2017 +0000
- Revision:
- 1:a57585d5e81b
- Parent:
- 0:ed8dc4531ac1
- Child:
- 2:b0a6783fb615
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
acracan | 0:ed8dc4531ac1 | 1 | #include "Pixy.h" |
acracan | 0:ed8dc4531ac1 | 2 | |
scholfa2 | 1:a57585d5e81b | 3 | |
scholfa2 | 1:a57585d5e81b | 4 | Pixy::Pixy(PinName mosi_sda_tx, PinName miso_scl_rx) |
acracan | 0:ed8dc4531ac1 | 5 | { |
scholfa2 | 1:a57585d5e81b | 6 | m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx); |
acracan | 0:ed8dc4531ac1 | 7 | pc = 0; |
acracan | 0:ed8dc4531ac1 | 8 | skipStart = false; |
acracan | 0:ed8dc4531ac1 | 9 | blockCount = 0; |
acracan | 0:ed8dc4531ac1 | 10 | blockArraySize = PIXY_INITIAL_ARRAYSIZE; |
acracan | 0:ed8dc4531ac1 | 11 | blocks = new Block[blockArraySize]; |
scholfa2 | 1:a57585d5e81b | 12 | |
acracan | 0:ed8dc4531ac1 | 13 | } |
acracan | 0:ed8dc4531ac1 | 14 | |
acracan | 0:ed8dc4531ac1 | 15 | Pixy::~Pixy() |
acracan | 0:ed8dc4531ac1 | 16 | { |
acracan | 0:ed8dc4531ac1 | 17 | delete[] blocks; |
acracan | 0:ed8dc4531ac1 | 18 | delete m_link; |
acracan | 0:ed8dc4531ac1 | 19 | } |
acracan | 0:ed8dc4531ac1 | 20 | |
acracan | 0:ed8dc4531ac1 | 21 | uint16_t Pixy::getBlocks(uint16_t maxBlocks) |
acracan | 0:ed8dc4531ac1 | 22 | { |
acracan | 0:ed8dc4531ac1 | 23 | uint8_t i; |
acracan | 0:ed8dc4531ac1 | 24 | uint16_t w, checksum, sum; |
acracan | 0:ed8dc4531ac1 | 25 | Block *block; |
acracan | 0:ed8dc4531ac1 | 26 | |
acracan | 0:ed8dc4531ac1 | 27 | if (!skipStart) { |
acracan | 0:ed8dc4531ac1 | 28 | if (getStart() == false) |
acracan | 0:ed8dc4531ac1 | 29 | return 0; |
acracan | 0:ed8dc4531ac1 | 30 | } else |
acracan | 0:ed8dc4531ac1 | 31 | skipStart = false; |
acracan | 0:ed8dc4531ac1 | 32 | |
acracan | 0:ed8dc4531ac1 | 33 | for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) { |
acracan | 0:ed8dc4531ac1 | 34 | checksum = m_link->getWord(); |
acracan | 0:ed8dc4531ac1 | 35 | if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame |
acracan | 0:ed8dc4531ac1 | 36 | skipStart = true; |
scholfa2 | 1:a57585d5e81b | 37 | // if (pc) |
scholfa2 | 1:a57585d5e81b | 38 | // pc->printf("skip\n\r"); |
acracan | 0:ed8dc4531ac1 | 39 | return blockCount; |
acracan | 0:ed8dc4531ac1 | 40 | } else if (checksum == 0) |
acracan | 0:ed8dc4531ac1 | 41 | return blockCount; |
acracan | 0:ed8dc4531ac1 | 42 | |
acracan | 0:ed8dc4531ac1 | 43 | if (blockCount > blockArraySize) |
acracan | 0:ed8dc4531ac1 | 44 | resize(); |
acracan | 0:ed8dc4531ac1 | 45 | |
acracan | 0:ed8dc4531ac1 | 46 | block = blocks + blockCount; |
acracan | 0:ed8dc4531ac1 | 47 | |
acracan | 0:ed8dc4531ac1 | 48 | for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) { |
acracan | 0:ed8dc4531ac1 | 49 | w = m_link->getWord(); |
acracan | 0:ed8dc4531ac1 | 50 | sum += w; |
acracan | 0:ed8dc4531ac1 | 51 | *((uint16_t *)block + i) = w; |
acracan | 0:ed8dc4531ac1 | 52 | } |
acracan | 0:ed8dc4531ac1 | 53 | |
scholfa2 | 1:a57585d5e81b | 54 | if (checksum == sum){ |
acracan | 0:ed8dc4531ac1 | 55 | blockCount++; |
scholfa2 | 1:a57585d5e81b | 56 | //w = m_link->getWord(); |
scholfa2 | 1:a57585d5e81b | 57 | } |
acracan | 0:ed8dc4531ac1 | 58 | else if (pc) |
acracan | 0:ed8dc4531ac1 | 59 | pc->printf("cs error\n\r"); |
acracan | 0:ed8dc4531ac1 | 60 | |
acracan | 0:ed8dc4531ac1 | 61 | w = m_link->getWord(); |
acracan | 0:ed8dc4531ac1 | 62 | if (w != PIXY_START_WORD) |
acracan | 0:ed8dc4531ac1 | 63 | return blockCount; |
acracan | 0:ed8dc4531ac1 | 64 | } |
acracan | 0:ed8dc4531ac1 | 65 | return blockCount; |
acracan | 0:ed8dc4531ac1 | 66 | } |
acracan | 0:ed8dc4531ac1 | 67 | |
acracan | 0:ed8dc4531ac1 | 68 | void Pixy::setAddress(uint8_t addr) |
acracan | 0:ed8dc4531ac1 | 69 | { |
acracan | 0:ed8dc4531ac1 | 70 | m_link->setAddress(addr); |
acracan | 0:ed8dc4531ac1 | 71 | } |
acracan | 0:ed8dc4531ac1 | 72 | |
acracan | 0:ed8dc4531ac1 | 73 | void Pixy::setSerialOutput(Serial *pc) |
acracan | 0:ed8dc4531ac1 | 74 | { |
acracan | 0:ed8dc4531ac1 | 75 | this->pc = pc; |
acracan | 0:ed8dc4531ac1 | 76 | } |
acracan | 0:ed8dc4531ac1 | 77 | |
acracan | 0:ed8dc4531ac1 | 78 | bool Pixy::getStart() |
acracan | 0:ed8dc4531ac1 | 79 | { |
acracan | 0:ed8dc4531ac1 | 80 | uint16_t w, lastw; |
acracan | 0:ed8dc4531ac1 | 81 | |
acracan | 0:ed8dc4531ac1 | 82 | lastw = 0xffff; |
acracan | 0:ed8dc4531ac1 | 83 | while (true) { |
acracan | 0:ed8dc4531ac1 | 84 | w = m_link->getWord(); |
acracan | 0:ed8dc4531ac1 | 85 | if (w == 0 && lastw == 0) { |
acracan | 0:ed8dc4531ac1 | 86 | wait_ms(10); |
acracan | 0:ed8dc4531ac1 | 87 | return false; |
acracan | 0:ed8dc4531ac1 | 88 | } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) |
acracan | 0:ed8dc4531ac1 | 89 | return true; |
acracan | 0:ed8dc4531ac1 | 90 | else if (w == PIXY_START_WORDX) { |
acracan | 0:ed8dc4531ac1 | 91 | if (pc) |
acracan | 0:ed8dc4531ac1 | 92 | pc->printf("reorder\n\r"); |
acracan | 0:ed8dc4531ac1 | 93 | m_link->getByte(); // resync |
acracan | 0:ed8dc4531ac1 | 94 | } |
acracan | 0:ed8dc4531ac1 | 95 | lastw = w; |
acracan | 0:ed8dc4531ac1 | 96 | } |
acracan | 0:ed8dc4531ac1 | 97 | } |
acracan | 0:ed8dc4531ac1 | 98 | |
acracan | 0:ed8dc4531ac1 | 99 | void Pixy::resize() |
acracan | 0:ed8dc4531ac1 | 100 | { |
acracan | 0:ed8dc4531ac1 | 101 | Block *newBlocks; |
acracan | 0:ed8dc4531ac1 | 102 | blockArraySize += PIXY_INITIAL_ARRAYSIZE; |
acracan | 0:ed8dc4531ac1 | 103 | newBlocks = new Block[blockArraySize]; |
acracan | 0:ed8dc4531ac1 | 104 | memcpy(newBlocks, blocks, sizeof(Block) * blockCount); |
acracan | 0:ed8dc4531ac1 | 105 | delete[] blocks; |
acracan | 0:ed8dc4531ac1 | 106 | blocks = newBlocks; |
acracan | 0:ed8dc4531ac1 | 107 | } |