Port of Arduino Pixy Cmucam5 library
Fork of pixy by
Pixy.cpp.orig@8:6cd36ded7c4e, 2016-03-13 (annotated)
- Committer:
- balsamfir
- Date:
- Sun Mar 13 14:23:33 2016 +0000
- Revision:
- 8:6cd36ded7c4e
- Parent:
- 6:f1c641be779e
Fixed block reading bug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
balsamfir | 6:f1c641be779e | 1 | #include "Pixy.h" |
balsamfir | 6:f1c641be779e | 2 | |
balsamfir | 6:f1c641be779e | 3 | Pixy::Pixy(Pixy::LinkType linkType, PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk) |
balsamfir | 6:f1c641be779e | 4 | { |
balsamfir | 6:f1c641be779e | 5 | switch (linkType) { |
balsamfir | 6:f1c641be779e | 6 | case SPI: |
balsamfir | 6:f1c641be779e | 7 | m_link = new PixyLinkSPI(mosi_sda_tx, miso_scl_rx, sclk); |
balsamfir | 6:f1c641be779e | 8 | break; |
balsamfir | 6:f1c641be779e | 9 | case I2C: |
balsamfir | 6:f1c641be779e | 10 | m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx); |
balsamfir | 6:f1c641be779e | 11 | break; |
balsamfir | 6:f1c641be779e | 12 | case UART: |
balsamfir | 6:f1c641be779e | 13 | m_link = new PixyLinkUART(mosi_sda_tx, miso_scl_rx); |
balsamfir | 6:f1c641be779e | 14 | break; |
balsamfir | 6:f1c641be779e | 15 | }; |
balsamfir | 6:f1c641be779e | 16 | pc = 0; |
balsamfir | 6:f1c641be779e | 17 | skipStart = false; |
balsamfir | 6:f1c641be779e | 18 | blockCount = 0; |
balsamfir | 6:f1c641be779e | 19 | blockArraySize = PIXY_INITIAL_ARRAYSIZE; |
balsamfir | 6:f1c641be779e | 20 | blocks = new Block[blockArraySize]; |
balsamfir | 6:f1c641be779e | 21 | } |
balsamfir | 6:f1c641be779e | 22 | |
balsamfir | 6:f1c641be779e | 23 | Pixy::~Pixy() |
balsamfir | 6:f1c641be779e | 24 | { |
balsamfir | 6:f1c641be779e | 25 | delete[] blocks; |
balsamfir | 6:f1c641be779e | 26 | delete m_link; |
balsamfir | 6:f1c641be779e | 27 | } |
balsamfir | 6:f1c641be779e | 28 | |
balsamfir | 6:f1c641be779e | 29 | uint16_t Pixy::getBlocks(uint16_t maxBlocks) |
balsamfir | 6:f1c641be779e | 30 | { |
balsamfir | 6:f1c641be779e | 31 | uint8_t i; |
balsamfir | 6:f1c641be779e | 32 | uint16_t w, checksum, sum, type; |
balsamfir | 6:f1c641be779e | 33 | Block *block;; |
balsamfir | 6:f1c641be779e | 34 | if (!skipStart) { |
balsamfir | 6:f1c641be779e | 35 | if (getStart(&type) == false) { |
balsamfir | 6:f1c641be779e | 36 | //pc->printf("fuck1 \n\r"); |
balsamfir | 6:f1c641be779e | 37 | return 0; |
balsamfir | 6:f1c641be779e | 38 | } |
balsamfir | 6:f1c641be779e | 39 | } else { |
balsamfir | 6:f1c641be779e | 40 | skipStart = false; |
balsamfir | 6:f1c641be779e | 41 | } |
balsamfir | 6:f1c641be779e | 42 | |
balsamfir | 6:f1c641be779e | 43 | for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) { |
balsamfir | 6:f1c641be779e | 44 | //pc->printf("tick \n\r"); |
balsamfir | 6:f1c641be779e | 45 | |
balsamfir | 6:f1c641be779e | 46 | checksum = m_link->getWord(); |
balsamfir | 6:f1c641be779e | 47 | if ((checksum == PIXY_START_WORD)||(checksum == PIXY_START_WORD_CC)) { |
balsamfir | 6:f1c641be779e | 48 | skipStart = true; |
balsamfir | 6:f1c641be779e | 49 | //pc->printf("fuck2 \n\r"); |
balsamfir | 6:f1c641be779e | 50 | return blockCount; |
balsamfir | 6:f1c641be779e | 51 | } else if (checksum == 0) { |
balsamfir | 6:f1c641be779e | 52 | //pc->printf("fuck3 \n\r"); |
balsamfir | 6:f1c641be779e | 53 | return blockCount; |
balsamfir | 6:f1c641be779e | 54 | } |
balsamfir | 6:f1c641be779e | 55 | |
balsamfir | 6:f1c641be779e | 56 | if (blockCount > blockArraySize) resize(); |
balsamfir | 6:f1c641be779e | 57 | |
balsamfir | 6:f1c641be779e | 58 | block = blocks + blockCount; |
balsamfir | 6:f1c641be779e | 59 | |
balsamfir | 6:f1c641be779e | 60 | *((uint16_t *)block) = type; |
balsamfir | 6:f1c641be779e | 61 | |
balsamfir | 6:f1c641be779e | 62 | for (i = 1, sum = 0; i < (sizeof(Block)/sizeof(uint16_t))-1; i++) { |
balsamfir | 6:f1c641be779e | 63 | w = m_link->getWord(); |
balsamfir | 6:f1c641be779e | 64 | sum += w; |
balsamfir | 6:f1c641be779e | 65 | *((int16_t *)block + i) = w; |
balsamfir | 6:f1c641be779e | 66 | } |
balsamfir | 6:f1c641be779e | 67 | |
balsamfir | 6:f1c641be779e | 68 | // Read in the extra word if its a color code |
balsamfir | 6:f1c641be779e | 69 | if (block->type == PIXY_START_WORD_CC) { |
balsamfir | 6:f1c641be779e | 70 | w = m_link->getWord(); |
balsamfir | 6:f1c641be779e | 71 | sum += w; |
balsamfir | 6:f1c641be779e | 72 | *((uint16_t *)block + i) = w; |
balsamfir | 6:f1c641be779e | 73 | } |
balsamfir | 6:f1c641be779e | 74 | |
balsamfir | 6:f1c641be779e | 75 | if (checksum == sum) blockCount++; |
balsamfir | 6:f1c641be779e | 76 | else if (pc) { |
balsamfir | 6:f1c641be779e | 77 | //pc->printf("cs error: csum: %d, sum: %d \n\r", checksum, sum); |
balsamfir | 6:f1c641be779e | 78 | //pc->printf("ERROR: "); |
balsamfir | 6:f1c641be779e | 79 | block->print(*pc); |
balsamfir | 6:f1c641be779e | 80 | } |
balsamfir | 6:f1c641be779e | 81 | |
balsamfir | 6:f1c641be779e | 82 | // Read in the next block type |
balsamfir | 6:f1c641be779e | 83 | w = m_link->getWord(); |
balsamfir | 6:f1c641be779e | 84 | if (w == PIXY_START_WORD) { |
balsamfir | 6:f1c641be779e | 85 | type = PIXY_START_WORD; |
balsamfir | 6:f1c641be779e | 86 | } else if (w == PIXY_START_WORD_CC) { |
balsamfir | 6:f1c641be779e | 87 | type = PIXY_START_WORD_CC; |
balsamfir | 6:f1c641be779e | 88 | } else { |
balsamfir | 6:f1c641be779e | 89 | return blockCount; |
balsamfir | 6:f1c641be779e | 90 | } |
balsamfir | 6:f1c641be779e | 91 | } |
balsamfir | 6:f1c641be779e | 92 | //pc->printf("fuck4 \n\r"); |
balsamfir | 6:f1c641be779e | 93 | return blockCount; |
balsamfir | 6:f1c641be779e | 94 | } |
balsamfir | 6:f1c641be779e | 95 | |
balsamfir | 6:f1c641be779e | 96 | int8_t Pixy::setServos(uint16_t s0, uint16_t s1) |
balsamfir | 6:f1c641be779e | 97 | { |
balsamfir | 6:f1c641be779e | 98 | uint8_t outBuf[6]; |
balsamfir | 6:f1c641be779e | 99 | |
balsamfir | 6:f1c641be779e | 100 | outBuf[0] = 0x00; |
balsamfir | 6:f1c641be779e | 101 | outBuf[1] = 0xff; |
balsamfir | 6:f1c641be779e | 102 | *(uint16_t *)(outBuf + 2) = s0; |
balsamfir | 6:f1c641be779e | 103 | *(uint16_t *)(outBuf + 4) = s1; |
balsamfir | 6:f1c641be779e | 104 | |
balsamfir | 6:f1c641be779e | 105 | return m_link->send(outBuf, 6); |
balsamfir | 6:f1c641be779e | 106 | } |
balsamfir | 6:f1c641be779e | 107 | |
balsamfir | 6:f1c641be779e | 108 | void Pixy::setAddress(uint8_t addr) |
balsamfir | 6:f1c641be779e | 109 | { |
balsamfir | 6:f1c641be779e | 110 | m_link->setAddress(addr); |
balsamfir | 6:f1c641be779e | 111 | } |
balsamfir | 6:f1c641be779e | 112 | |
balsamfir | 6:f1c641be779e | 113 | void Pixy::setSerialOutput(Serial *pc) |
balsamfir | 6:f1c641be779e | 114 | { |
balsamfir | 6:f1c641be779e | 115 | this->pc = pc; |
balsamfir | 6:f1c641be779e | 116 | } |
balsamfir | 6:f1c641be779e | 117 | |
balsamfir | 6:f1c641be779e | 118 | bool Pixy::getStart(uint16_t *type) |
balsamfir | 6:f1c641be779e | 119 | { |
balsamfir | 6:f1c641be779e | 120 | uint16_t w, lastw; |
balsamfir | 6:f1c641be779e | 121 | |
balsamfir | 6:f1c641be779e | 122 | lastw = 0xffff; |
balsamfir | 6:f1c641be779e | 123 | while (true) { |
balsamfir | 6:f1c641be779e | 124 | w = m_link->getWord(); |
balsamfir | 6:f1c641be779e | 125 | //pc->printf("word: %04x \r\n", w); |
balsamfir | 6:f1c641be779e | 126 | if (w == 0 && lastw == 0) { |
balsamfir | 6:f1c641be779e | 127 | wait_ms(10); |
balsamfir | 6:f1c641be779e | 128 | return false; |
balsamfir | 6:f1c641be779e | 129 | } else if ((w == PIXY_START_WORD || w == PIXY_START_WORD_CC) && lastw == PIXY_START_WORD) { |
balsamfir | 6:f1c641be779e | 130 | *type = w; |
balsamfir | 6:f1c641be779e | 131 | return true; |
balsamfir | 6:f1c641be779e | 132 | } else if (w == PIXY_START_WORDX) { |
balsamfir | 6:f1c641be779e | 133 | if (pc) pc->printf("reorder\n\r"); |
balsamfir | 6:f1c641be779e | 134 | m_link->getByte(); // resync |
balsamfir | 6:f1c641be779e | 135 | } |
balsamfir | 6:f1c641be779e | 136 | lastw = w; |
balsamfir | 6:f1c641be779e | 137 | } |
balsamfir | 6:f1c641be779e | 138 | } |
balsamfir | 6:f1c641be779e | 139 | |
balsamfir | 6:f1c641be779e | 140 | void Pixy::resize() |
balsamfir | 6:f1c641be779e | 141 | { |
balsamfir | 6:f1c641be779e | 142 | Block *newBlocks; |
balsamfir | 6:f1c641be779e | 143 | blockArraySize += PIXY_INITIAL_ARRAYSIZE; |
balsamfir | 6:f1c641be779e | 144 | newBlocks = new Block[blockArraySize]; |
balsamfir | 6:f1c641be779e | 145 | memcpy(newBlocks, blocks, sizeof(Block) * blockCount); |
balsamfir | 6:f1c641be779e | 146 | delete[] blocks; |
balsamfir | 6:f1c641be779e | 147 | blocks = newBlocks; |
balsamfir | 6:f1c641be779e | 148 | } |