Port of Arduino Pixy Cmucam5 library
Fork of pixy by
Pixy.cpp@3:cc3240a789e9, 2016-02-29 (annotated)
- Committer:
- balsamfir
- Date:
- Mon Feb 29 17:32:35 2016 +0000
- Revision:
- 3:cc3240a789e9
- Parent:
- 2:be2c893fb5d5
- Child:
- 4:6aa0f748a636
Prototype library
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; |
balsamfir | 3:cc3240a789e9 | 32 | uint16_t w, checksum, sum, type; |
acracan | 0:ed8dc4531ac1 | 33 | Block *block; |
acracan | 0:ed8dc4531ac1 | 34 | |
acracan | 0:ed8dc4531ac1 | 35 | if (!skipStart) { |
balsamfir | 2:be2c893fb5d5 | 36 | if (getStart() == false) { |
balsamfir | 2:be2c893fb5d5 | 37 | pc->printf("fuck1 \n\r"); |
acracan | 0:ed8dc4531ac1 | 38 | return 0; |
balsamfir | 2:be2c893fb5d5 | 39 | } |
balsamfir | 2:be2c893fb5d5 | 40 | } else { |
acracan | 0:ed8dc4531ac1 | 41 | skipStart = false; |
balsamfir | 2:be2c893fb5d5 | 42 | } |
acracan | 0:ed8dc4531ac1 | 43 | |
acracan | 0:ed8dc4531ac1 | 44 | for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) { |
balsamfir | 2:be2c893fb5d5 | 45 | pc->printf("tick \n\r"); |
acracan | 0:ed8dc4531ac1 | 46 | checksum = m_link->getWord(); |
balsamfir | 2:be2c893fb5d5 | 47 | if ((checksum == PIXY_START_WORD)||(checksum == PIXY_START_WORD_CC)) { |
balsamfir | 2:be2c893fb5d5 | 48 | *((uint16_t *)block) = checksum; // Set the type to the checksum |
acracan | 0:ed8dc4531ac1 | 49 | skipStart = true; |
balsamfir | 2:be2c893fb5d5 | 50 | pc->printf("fuck2 \n\r"); |
acracan | 0:ed8dc4531ac1 | 51 | return blockCount; |
balsamfir | 2:be2c893fb5d5 | 52 | } else if (checksum == 0) { |
balsamfir | 2:be2c893fb5d5 | 53 | pc->printf("fuck3 \n\r"); |
acracan | 0:ed8dc4531ac1 | 54 | return blockCount; |
balsamfir | 2:be2c893fb5d5 | 55 | } |
acracan | 0:ed8dc4531ac1 | 56 | |
balsamfir | 2:be2c893fb5d5 | 57 | if (blockCount > blockArraySize) resize(); |
acracan | 0:ed8dc4531ac1 | 58 | |
acracan | 0:ed8dc4531ac1 | 59 | block = blocks + blockCount; |
balsamfir | 2:be2c893fb5d5 | 60 | |
balsamfir | 2:be2c893fb5d5 | 61 | for (i = 1, sum = 0; i < (sizeof(Block)/sizeof(uint16_t))-1; i++) { |
balsamfir | 2:be2c893fb5d5 | 62 | w = m_link->getWord(); |
balsamfir | 2:be2c893fb5d5 | 63 | sum += w; |
balsamfir | 2:be2c893fb5d5 | 64 | *((uint16_t *)block + i) = w; |
balsamfir | 2:be2c893fb5d5 | 65 | } |
balsamfir | 2:be2c893fb5d5 | 66 | |
balsamfir | 2:be2c893fb5d5 | 67 | // Read in the extra word if its a color code |
balsamfir | 2:be2c893fb5d5 | 68 | if (checksum == PIXY_START_WORD_CC) { |
acracan | 0:ed8dc4531ac1 | 69 | w = m_link->getWord(); |
acracan | 0:ed8dc4531ac1 | 70 | sum += w; |
acracan | 0:ed8dc4531ac1 | 71 | *((uint16_t *)block + i) = w; |
acracan | 0:ed8dc4531ac1 | 72 | } |
acracan | 0:ed8dc4531ac1 | 73 | |
balsamfir | 2:be2c893fb5d5 | 74 | if (checksum == sum) blockCount++; |
balsamfir | 2:be2c893fb5d5 | 75 | else if (pc) pc->printf("cs error: csum: %04x, sum: %04x \n\r", checksum, sum); |
balsamfir | 2:be2c893fb5d5 | 76 | |
acracan | 0:ed8dc4531ac1 | 77 | } |
balsamfir | 2:be2c893fb5d5 | 78 | pc->printf("fuck4 \n\r"); |
acracan | 0:ed8dc4531ac1 | 79 | return blockCount; |
acracan | 0:ed8dc4531ac1 | 80 | } |
acracan | 0:ed8dc4531ac1 | 81 | |
acracan | 0:ed8dc4531ac1 | 82 | int8_t Pixy::setServos(uint16_t s0, uint16_t s1) |
acracan | 0:ed8dc4531ac1 | 83 | { |
acracan | 0:ed8dc4531ac1 | 84 | uint8_t outBuf[6]; |
acracan | 0:ed8dc4531ac1 | 85 | |
acracan | 0:ed8dc4531ac1 | 86 | outBuf[0] = 0x00; |
acracan | 0:ed8dc4531ac1 | 87 | outBuf[1] = 0xff; |
acracan | 0:ed8dc4531ac1 | 88 | *(uint16_t *)(outBuf + 2) = s0; |
acracan | 0:ed8dc4531ac1 | 89 | *(uint16_t *)(outBuf + 4) = s1; |
acracan | 0:ed8dc4531ac1 | 90 | |
acracan | 0:ed8dc4531ac1 | 91 | return m_link->send(outBuf, 6); |
acracan | 0:ed8dc4531ac1 | 92 | } |
acracan | 0:ed8dc4531ac1 | 93 | |
acracan | 0:ed8dc4531ac1 | 94 | void Pixy::setAddress(uint8_t addr) |
acracan | 0:ed8dc4531ac1 | 95 | { |
acracan | 0:ed8dc4531ac1 | 96 | m_link->setAddress(addr); |
acracan | 0:ed8dc4531ac1 | 97 | } |
acracan | 0:ed8dc4531ac1 | 98 | |
acracan | 0:ed8dc4531ac1 | 99 | void Pixy::setSerialOutput(Serial *pc) |
acracan | 0:ed8dc4531ac1 | 100 | { |
acracan | 0:ed8dc4531ac1 | 101 | this->pc = pc; |
acracan | 0:ed8dc4531ac1 | 102 | } |
acracan | 0:ed8dc4531ac1 | 103 | |
acracan | 0:ed8dc4531ac1 | 104 | bool Pixy::getStart() |
acracan | 0:ed8dc4531ac1 | 105 | { |
acracan | 0:ed8dc4531ac1 | 106 | uint16_t w, lastw; |
acracan | 0:ed8dc4531ac1 | 107 | |
acracan | 0:ed8dc4531ac1 | 108 | lastw = 0xffff; |
acracan | 0:ed8dc4531ac1 | 109 | while (true) { |
acracan | 0:ed8dc4531ac1 | 110 | w = m_link->getWord(); |
balsamfir | 2:be2c893fb5d5 | 111 | //pc->printf("word: %04x \r\n", w); |
acracan | 0:ed8dc4531ac1 | 112 | if (w == 0 && lastw == 0) { |
acracan | 0:ed8dc4531ac1 | 113 | wait_ms(10); |
acracan | 0:ed8dc4531ac1 | 114 | return false; |
balsamfir | 2:be2c893fb5d5 | 115 | } else if ((w == PIXY_START_WORD || w == PIXY_START_WORD_CC) && lastw == PIXY_START_WORD) { |
acracan | 0:ed8dc4531ac1 | 116 | return true; |
balsamfir | 2:be2c893fb5d5 | 117 | } else if (w == PIXY_START_WORDX) { |
balsamfir | 2:be2c893fb5d5 | 118 | if (pc) pc->printf("reorder\n\r"); |
acracan | 0:ed8dc4531ac1 | 119 | m_link->getByte(); // resync |
acracan | 0:ed8dc4531ac1 | 120 | } |
acracan | 0:ed8dc4531ac1 | 121 | lastw = w; |
acracan | 0:ed8dc4531ac1 | 122 | } |
acracan | 0:ed8dc4531ac1 | 123 | } |
acracan | 0:ed8dc4531ac1 | 124 | |
acracan | 0:ed8dc4531ac1 | 125 | void Pixy::resize() |
acracan | 0:ed8dc4531ac1 | 126 | { |
acracan | 0:ed8dc4531ac1 | 127 | Block *newBlocks; |
acracan | 0:ed8dc4531ac1 | 128 | blockArraySize += PIXY_INITIAL_ARRAYSIZE; |
acracan | 0:ed8dc4531ac1 | 129 | newBlocks = new Block[blockArraySize]; |
acracan | 0:ed8dc4531ac1 | 130 | memcpy(newBlocks, blocks, sizeof(Block) * blockCount); |
acracan | 0:ed8dc4531ac1 | 131 | delete[] blocks; |
acracan | 0:ed8dc4531ac1 | 132 | blocks = newBlocks; |
acracan | 0:ed8dc4531ac1 | 133 | } |