Prometheus / Mbed 2 deprecated PixyTest2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Pixy.cpp@0:4a23b84b733e, 2017-05-17 (annotated)

Committer:
ZHAW_Prometheus
Date:
Wed May 17 13:08:18 2017 +0000
Revision:
0:4a23b84b733e
PixyTest2 Vers. 17.05.2017

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ZHAW_Prometheus 0:4a23b84b733e 1 #include "Pixy.h"
ZHAW_Prometheus 0:4a23b84b733e 2
ZHAW_Prometheus 0:4a23b84b733e 3 Pixy::Pixy(Pixy::LinkType linkType, PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk)
ZHAW_Prometheus 0:4a23b84b733e 4 {
ZHAW_Prometheus 0:4a23b84b733e 5 switch (linkType) {
ZHAW_Prometheus 0:4a23b84b733e 6 case SPI:
ZHAW_Prometheus 0:4a23b84b733e 7 m_link = new PixyLinkSPI(mosi_sda_tx, miso_scl_rx, sclk);
ZHAW_Prometheus 0:4a23b84b733e 8 break;
ZHAW_Prometheus 0:4a23b84b733e 9 case I2C:
ZHAW_Prometheus 0:4a23b84b733e 10 m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx);
ZHAW_Prometheus 0:4a23b84b733e 11 break;
ZHAW_Prometheus 0:4a23b84b733e 12 case UART:
ZHAW_Prometheus 0:4a23b84b733e 13 m_link = new PixyLinkUART(mosi_sda_tx, miso_scl_rx);
ZHAW_Prometheus 0:4a23b84b733e 14 break;
ZHAW_Prometheus 0:4a23b84b733e 15 };
ZHAW_Prometheus 0:4a23b84b733e 16 pc = 0;
ZHAW_Prometheus 0:4a23b84b733e 17 skipStart = false;
ZHAW_Prometheus 0:4a23b84b733e 18 blockCount = 0;
ZHAW_Prometheus 0:4a23b84b733e 19 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
ZHAW_Prometheus 0:4a23b84b733e 20 blocks = new Block[blockArraySize];
ZHAW_Prometheus 0:4a23b84b733e 21 }
ZHAW_Prometheus 0:4a23b84b733e 22
ZHAW_Prometheus 0:4a23b84b733e 23 Pixy::~Pixy()
ZHAW_Prometheus 0:4a23b84b733e 24 {
ZHAW_Prometheus 0:4a23b84b733e 25 delete[] blocks;
ZHAW_Prometheus 0:4a23b84b733e 26 delete m_link;
ZHAW_Prometheus 0:4a23b84b733e 27 }
ZHAW_Prometheus 0:4a23b84b733e 28
ZHAW_Prometheus 0:4a23b84b733e 29 uint16_t Pixy::getBlocks(uint16_t maxBlocks)
ZHAW_Prometheus 0:4a23b84b733e 30 {
ZHAW_Prometheus 0:4a23b84b733e 31 uint8_t i;
ZHAW_Prometheus 0:4a23b84b733e 32 uint16_t w, checksum, sum;
ZHAW_Prometheus 0:4a23b84b733e 33 Block *block;
ZHAW_Prometheus 0:4a23b84b733e 34
ZHAW_Prometheus 0:4a23b84b733e 35 if (!skipStart) {
ZHAW_Prometheus 0:4a23b84b733e 36 if (getStart() == false)
ZHAW_Prometheus 0:4a23b84b733e 37 return 0;
ZHAW_Prometheus 0:4a23b84b733e 38 } else
ZHAW_Prometheus 0:4a23b84b733e 39 skipStart = false;
ZHAW_Prometheus 0:4a23b84b733e 40
ZHAW_Prometheus 0:4a23b84b733e 41 for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
ZHAW_Prometheus 0:4a23b84b733e 42 checksum = m_link->getWord();
ZHAW_Prometheus 0:4a23b84b733e 43 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
ZHAW_Prometheus 0:4a23b84b733e 44 skipStart = true;
ZHAW_Prometheus 0:4a23b84b733e 45 //if (pc)
ZHAW_Prometheus 0:4a23b84b733e 46 // pc->printf("skip\n\r");
ZHAW_Prometheus 0:4a23b84b733e 47 return blockCount;
ZHAW_Prometheus 0:4a23b84b733e 48 } else if (checksum == 0)
ZHAW_Prometheus 0:4a23b84b733e 49 return blockCount;
ZHAW_Prometheus 0:4a23b84b733e 50
ZHAW_Prometheus 0:4a23b84b733e 51 if (blockCount > blockArraySize)
ZHAW_Prometheus 0:4a23b84b733e 52 resize();
ZHAW_Prometheus 0:4a23b84b733e 53
ZHAW_Prometheus 0:4a23b84b733e 54 block = blocks + blockCount;
ZHAW_Prometheus 0:4a23b84b733e 55
ZHAW_Prometheus 0:4a23b84b733e 56 for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) {
ZHAW_Prometheus 0:4a23b84b733e 57 w = m_link->getWord();
ZHAW_Prometheus 0:4a23b84b733e 58 sum += w;
ZHAW_Prometheus 0:4a23b84b733e 59 *((uint16_t *)block + i) = w;
ZHAW_Prometheus 0:4a23b84b733e 60 }
ZHAW_Prometheus 0:4a23b84b733e 61
ZHAW_Prometheus 0:4a23b84b733e 62 if (checksum == sum)
ZHAW_Prometheus 0:4a23b84b733e 63 blockCount++;
ZHAW_Prometheus 0:4a23b84b733e 64 else if (pc)
ZHAW_Prometheus 0:4a23b84b733e 65 pc->printf("cs error\n\r");
ZHAW_Prometheus 0:4a23b84b733e 66
ZHAW_Prometheus 0:4a23b84b733e 67 w = m_link->getWord();
ZHAW_Prometheus 0:4a23b84b733e 68 if (w != PIXY_START_WORD)
ZHAW_Prometheus 0:4a23b84b733e 69 return blockCount;
ZHAW_Prometheus 0:4a23b84b733e 70 }
ZHAW_Prometheus 0:4a23b84b733e 71 return blockCount;
ZHAW_Prometheus 0:4a23b84b733e 72 }
ZHAW_Prometheus 0:4a23b84b733e 73
ZHAW_Prometheus 0:4a23b84b733e 74 int8_t Pixy::setServos(uint16_t s0, uint16_t s1)
ZHAW_Prometheus 0:4a23b84b733e 75 {
ZHAW_Prometheus 0:4a23b84b733e 76 uint8_t outBuf[6];
ZHAW_Prometheus 0:4a23b84b733e 77
ZHAW_Prometheus 0:4a23b84b733e 78 outBuf[0] = 0x00;
ZHAW_Prometheus 0:4a23b84b733e 79 outBuf[1] = 0xff;
ZHAW_Prometheus 0:4a23b84b733e 80 *(uint16_t *)(outBuf + 2) = s0;
ZHAW_Prometheus 0:4a23b84b733e 81 *(uint16_t *)(outBuf + 4) = s1;
ZHAW_Prometheus 0:4a23b84b733e 82
ZHAW_Prometheus 0:4a23b84b733e 83 return m_link->send(outBuf, 6);
ZHAW_Prometheus 0:4a23b84b733e 84 }
ZHAW_Prometheus 0:4a23b84b733e 85
ZHAW_Prometheus 0:4a23b84b733e 86 void Pixy::setAddress(uint8_t addr)
ZHAW_Prometheus 0:4a23b84b733e 87 {
ZHAW_Prometheus 0:4a23b84b733e 88 m_link->setAddress(addr);
ZHAW_Prometheus 0:4a23b84b733e 89 }
ZHAW_Prometheus 0:4a23b84b733e 90
ZHAW_Prometheus 0:4a23b84b733e 91 void Pixy::setSerialOutput(Serial *pc)
ZHAW_Prometheus 0:4a23b84b733e 92 {
ZHAW_Prometheus 0:4a23b84b733e 93 this->pc = pc;
ZHAW_Prometheus 0:4a23b84b733e 94 }
ZHAW_Prometheus 0:4a23b84b733e 95
ZHAW_Prometheus 0:4a23b84b733e 96 bool Pixy::getStart()
ZHAW_Prometheus 0:4a23b84b733e 97 {
ZHAW_Prometheus 0:4a23b84b733e 98 uint16_t w, lastw;
ZHAW_Prometheus 0:4a23b84b733e 99
ZHAW_Prometheus 0:4a23b84b733e 100 lastw = 0xffff;
ZHAW_Prometheus 0:4a23b84b733e 101 while (true) {
ZHAW_Prometheus 0:4a23b84b733e 102 w = m_link->getWord();
ZHAW_Prometheus 0:4a23b84b733e 103 if (w == 0 && lastw == 0) {
ZHAW_Prometheus 0:4a23b84b733e 104 wait_ms(10);
ZHAW_Prometheus 0:4a23b84b733e 105 return false;
ZHAW_Prometheus 0:4a23b84b733e 106 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
ZHAW_Prometheus 0:4a23b84b733e 107 return true;
ZHAW_Prometheus 0:4a23b84b733e 108 else if (w == PIXY_START_WORDX) {
ZHAW_Prometheus 0:4a23b84b733e 109 if (pc)
ZHAW_Prometheus 0:4a23b84b733e 110 pc->printf("reorder\n\r");
ZHAW_Prometheus 0:4a23b84b733e 111 m_link->getByte(); // resync
ZHAW_Prometheus 0:4a23b84b733e 112 }
ZHAW_Prometheus 0:4a23b84b733e 113 lastw = w;
ZHAW_Prometheus 0:4a23b84b733e 114 }
ZHAW_Prometheus 0:4a23b84b733e 115 }
ZHAW_Prometheus 0:4a23b84b733e 116
ZHAW_Prometheus 0:4a23b84b733e 117 void Pixy::resize()
ZHAW_Prometheus 0:4a23b84b733e 118 {
ZHAW_Prometheus 0:4a23b84b733e 119 Block *newBlocks;
ZHAW_Prometheus 0:4a23b84b733e 120 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
ZHAW_Prometheus 0:4a23b84b733e 121 newBlocks = new Block[blockArraySize];
ZHAW_Prometheus 0:4a23b84b733e 122 memcpy(newBlocks, blocks, sizeof(Block) * blockCount);
ZHAW_Prometheus 0:4a23b84b733e 123 delete[] blocks;
ZHAW_Prometheus 0:4a23b84b733e 124 blocks = newBlocks;
ZHAW_Prometheus 0:4a23b84b733e 125 }