Erik Hatfield / pixylib

Port of Arduino Pixy Cmucam5 library

Dependents:   Robot_Control

Fork of pixy by Arcadie Cracan

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

Committer:
balsamfir
Date:
Fri Mar 04 17:33:49 2016 +0000
Revision:
6:f1c641be779e
Backup before cleanup;

Who changed what in which revision?

UserRevisionLine numberNew 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 }