Erik Hatfield / pixylib

Port of Arduino Pixy Cmucam5 library

Dependents:   Robot_Control

Fork of pixy by Arcadie Cracan

Pixy.cpp@6:f1c641be779e, 2016-03-04 (annotated)

Committer:
balsamfir
Date:
Fri Mar 04 17:33:49 2016 +0000
Revision:
6:f1c641be779e
Parent:
4:6aa0f748a636
Child:
7:57cbf91c2d46
Backup before cleanup;

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