Erik Hatfield / pixylib

Port of Arduino Pixy Cmucam5 library

Dependents:   Robot_Control

Fork of pixy by Arcadie Cracan

Pixy.cpp@5:23e98079fb75, 2016-03-04 (annotated)

Committer:
balsamfir
Date:
Fri Mar 04 17:20:22 2016 +0000
Revision:
5:23e98079fb75
Parent:
0:ed8dc4531ac1 
commit to backup

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