Prometheus / Pixy

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.

Dependents:   PixyTest PixyTest

Pixy.cpp@0:a2603d7fa0ac, 2017-05-05 (annotated)

Committer:
ZHAW_Prometheus
Date:
Fri May 05 12:03:13 2017 +0000
Revision:
0:a2603d7fa0ac
Child:
2:c3a866b20784
Vers-0.1

Who changed what in which revision?

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