Luis Osorio
Fork of
pixy
by 
Arcadie Cracan
Pixy.cpp@1:683b8f2ba78e, 2017-07-05 (annotated)
- Committer:
- Bisca
- Date:
- Wed Jul 05 09:17:06 2017 +0000
- Revision:
- 1:683b8f2ba78e
- Parent:
- 0:ed8dc4531ac1
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 | |
| 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; |
| acracan | 0:ed8dc4531ac1 | 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; |
| acracan | 0:ed8dc4531ac1 | 40 | |
| acracan | 0:ed8dc4531ac1 | 41 | for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) { |
| acracan | 0:ed8dc4531ac1 | 42 | checksum = m_link->getWord(); |
| acracan | 0:ed8dc4531ac1 | 43 | if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame |
| acracan | 0:ed8dc4531ac1 | 44 | skipStart = true; |
| acracan | 0:ed8dc4531ac1 | 45 | //if (pc) |
| acracan | 0:ed8dc4531ac1 | 46 | // pc->printf("skip\n\r"); |
| acracan | 0:ed8dc4531ac1 | 47 | return blockCount; |
| acracan | 0:ed8dc4531ac1 | 48 | } else if (checksum == 0) |
| acracan | 0:ed8dc4531ac1 | 49 | return blockCount; |
| acracan | 0:ed8dc4531ac1 | 50 | |
| acracan | 0:ed8dc4531ac1 | 51 | if (blockCount > blockArraySize) |
| acracan | 0:ed8dc4531ac1 | 52 | resize(); |
| acracan | 0:ed8dc4531ac1 | 53 | |
| acracan | 0:ed8dc4531ac1 | 54 | block = blocks + blockCount; |
| acracan | 0:ed8dc4531ac1 | 55 | |
| acracan | 0:ed8dc4531ac1 | 56 | for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) { |
| acracan | 0:ed8dc4531ac1 | 57 | w = m_link->getWord(); |
| acracan | 0:ed8dc4531ac1 | 58 | sum += w; |
| acracan | 0:ed8dc4531ac1 | 59 | *((uint16_t *)block + i) = w; |
| acracan | 0:ed8dc4531ac1 | 60 | } |
| acracan | 0:ed8dc4531ac1 | 61 | |
| acracan | 0:ed8dc4531ac1 | 62 | if (checksum == sum) |
| acracan | 0:ed8dc4531ac1 | 63 | blockCount++; |
| acracan | 0:ed8dc4531ac1 | 64 | else if (pc) |
| acracan | 0:ed8dc4531ac1 | 65 | pc->printf("cs error\n\r"); |
| acracan | 0:ed8dc4531ac1 | 66 | |
| acracan | 0:ed8dc4531ac1 | 67 | w = m_link->getWord(); |
| acracan | 0:ed8dc4531ac1 | 68 | if (w != PIXY_START_WORD) |
| acracan | 0:ed8dc4531ac1 | 69 | return blockCount; |
| acracan | 0:ed8dc4531ac1 | 70 | } |
| acracan | 0:ed8dc4531ac1 | 71 | return blockCount; |
| acracan | 0:ed8dc4531ac1 | 72 | } |
| acracan | 0:ed8dc4531ac1 | 73 | |
| acracan | 0:ed8dc4531ac1 | 74 | int8_t Pixy::setServos(uint16_t s0, uint16_t s1) |
| acracan | 0:ed8dc4531ac1 | 75 | { |
| acracan | 0:ed8dc4531ac1 | 76 | uint8_t outBuf[6]; |
| acracan | 0:ed8dc4531ac1 | 77 | |
| acracan | 0:ed8dc4531ac1 | 78 | outBuf[0] = 0x00; |
| acracan | 0:ed8dc4531ac1 | 79 | outBuf[1] = 0xff; |
| acracan | 0:ed8dc4531ac1 | 80 | *(uint16_t *)(outBuf + 2) = s0; |
| acracan | 0:ed8dc4531ac1 | 81 | *(uint16_t *)(outBuf + 4) = s1; |
| acracan | 0:ed8dc4531ac1 | 82 | |
| acracan | 0:ed8dc4531ac1 | 83 | return m_link->send(outBuf, 6); |
| acracan | 0:ed8dc4531ac1 | 84 | } |
| acracan | 0:ed8dc4531ac1 | 85 | |
| acracan | 0:ed8dc4531ac1 | 86 | void Pixy::setAddress(uint8_t addr) |
| acracan | 0:ed8dc4531ac1 | 87 | { |
| acracan | 0:ed8dc4531ac1 | 88 | m_link->setAddress(addr); |
| acracan | 0:ed8dc4531ac1 | 89 | } |
| acracan | 0:ed8dc4531ac1 | 90 | |
| acracan | 0:ed8dc4531ac1 | 91 | void Pixy::setSerialOutput(Serial *pc) |
| acracan | 0:ed8dc4531ac1 | 92 | { |
| acracan | 0:ed8dc4531ac1 | 93 | this->pc = pc; |
| acracan | 0:ed8dc4531ac1 | 94 | } |
| acracan | 0:ed8dc4531ac1 | 95 | |
| acracan | 0:ed8dc4531ac1 | 96 | bool Pixy::getStart() |
| acracan | 0:ed8dc4531ac1 | 97 | { |
| acracan | 0:ed8dc4531ac1 | 98 | uint16_t w, lastw; |
| acracan | 0:ed8dc4531ac1 | 99 | |
| acracan | 0:ed8dc4531ac1 | 100 | lastw = 0xffff; |
| acracan | 0:ed8dc4531ac1 | 101 | while (true) { |
| acracan | 0:ed8dc4531ac1 | 102 | w = m_link->getWord(); |
| acracan | 0:ed8dc4531ac1 | 103 | if (w == 0 && lastw == 0) { |
| acracan | 0:ed8dc4531ac1 | 104 | wait_ms(10); |
| acracan | 0:ed8dc4531ac1 | 105 | return false; |
| acracan | 0:ed8dc4531ac1 | 106 | } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD) |
| acracan | 0:ed8dc4531ac1 | 107 | return true; |
| acracan | 0:ed8dc4531ac1 | 108 | else if (w == PIXY_START_WORDX) { |
| acracan | 0:ed8dc4531ac1 | 109 | if (pc) |
| acracan | 0:ed8dc4531ac1 | 110 | pc->printf("reorder\n\r"); |
| acracan | 0:ed8dc4531ac1 | 111 | m_link->getByte(); // resync |
| acracan | 0:ed8dc4531ac1 | 112 | } |
| acracan | 0:ed8dc4531ac1 | 113 | lastw = w; |
| acracan | 0:ed8dc4531ac1 | 114 | } |
| acracan | 0:ed8dc4531ac1 | 115 | } |
| acracan | 0:ed8dc4531ac1 | 116 | |
| acracan | 0:ed8dc4531ac1 | 117 | void Pixy::resize() |
| acracan | 0:ed8dc4531ac1 | 118 | { |
| acracan | 0:ed8dc4531ac1 | 119 | Block *newBlocks; |
| acracan | 0:ed8dc4531ac1 | 120 | blockArraySize += PIXY_INITIAL_ARRAYSIZE; |
| acracan | 0:ed8dc4531ac1 | 121 | newBlocks = new Block[blockArraySize]; |
| acracan | 0:ed8dc4531ac1 | 122 | memcpy(newBlocks, blocks, sizeof(Block) * blockCount); |
| acracan | 0:ed8dc4531ac1 | 123 | delete[] blocks; |
| acracan | 0:ed8dc4531ac1 | 124 | blocks = newBlocks; |
| acracan | 0:ed8dc4531ac1 | 125 | } |