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@2:c3a866b20784, 2017-05-17 (annotated)

Committer:
ZHAW_Prometheus
Date:
Wed May 17 07:37:43 2017 +0000
Revision:
2:c3a866b20784
Parent:
0:a2603d7fa0ac 
Vers. 17.05.2017

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 2:c3a866b20784 3
ZHAW_Prometheus 2:c3a866b20784 4 Pixy::Pixy(PinName mosi_sda_tx, PinName miso_scl_rx)
ZHAW_Prometheus 0:a2603d7fa0ac 5 {
ZHAW_Prometheus 2:c3a866b20784 6 m_link = new PixyLinkI2C(mosi_sda_tx, miso_scl_rx);
ZHAW_Prometheus 0:a2603d7fa0ac 7 pc = 0;
ZHAW_Prometheus 0:a2603d7fa0ac 8 skipStart = false;
ZHAW_Prometheus 0:a2603d7fa0ac 9 blockCount = 0;
ZHAW_Prometheus 0:a2603d7fa0ac 10 blockArraySize = PIXY_INITIAL_ARRAYSIZE;
ZHAW_Prometheus 0:a2603d7fa0ac 11 blocks = new Block[blockArraySize];
ZHAW_Prometheus 2:c3a866b20784 12
ZHAW_Prometheus 0:a2603d7fa0ac 13 }
ZHAW_Prometheus 0:a2603d7fa0ac 14
ZHAW_Prometheus 0:a2603d7fa0ac 15 Pixy::~Pixy()
ZHAW_Prometheus 0:a2603d7fa0ac 16 {
ZHAW_Prometheus 0:a2603d7fa0ac 17 delete[] blocks;
ZHAW_Prometheus 0:a2603d7fa0ac 18 delete m_link;
ZHAW_Prometheus 0:a2603d7fa0ac 19 }
ZHAW_Prometheus 0:a2603d7fa0ac 20
ZHAW_Prometheus 0:a2603d7fa0ac 21 uint16_t Pixy::getBlocks(uint16_t maxBlocks)
ZHAW_Prometheus 0:a2603d7fa0ac 22 {
ZHAW_Prometheus 0:a2603d7fa0ac 23 uint8_t i;
ZHAW_Prometheus 0:a2603d7fa0ac 24 uint16_t w, checksum, sum;
ZHAW_Prometheus 0:a2603d7fa0ac 25 Block *block;
ZHAW_Prometheus 0:a2603d7fa0ac 26
ZHAW_Prometheus 0:a2603d7fa0ac 27 if (!skipStart) {
ZHAW_Prometheus 0:a2603d7fa0ac 28 if (getStart() == false)
ZHAW_Prometheus 0:a2603d7fa0ac 29 return 0;
ZHAW_Prometheus 0:a2603d7fa0ac 30 } else
ZHAW_Prometheus 0:a2603d7fa0ac 31 skipStart = false;
ZHAW_Prometheus 0:a2603d7fa0ac 32
ZHAW_Prometheus 0:a2603d7fa0ac 33 for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
ZHAW_Prometheus 0:a2603d7fa0ac 34 checksum = m_link->getWord();
ZHAW_Prometheus 0:a2603d7fa0ac 35 if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
ZHAW_Prometheus 0:a2603d7fa0ac 36 skipStart = true;
ZHAW_Prometheus 2:c3a866b20784 37 // if (pc)
ZHAW_Prometheus 2:c3a866b20784 38 // pc->printf("skip\n\r");
ZHAW_Prometheus 0:a2603d7fa0ac 39 return blockCount;
ZHAW_Prometheus 0:a2603d7fa0ac 40 } else if (checksum == 0)
ZHAW_Prometheus 2:c3a866b20784 41 return blockCount; // Made by Renske
ZHAW_Prometheus 0:a2603d7fa0ac 42
ZHAW_Prometheus 0:a2603d7fa0ac 43 if (blockCount > blockArraySize)
ZHAW_Prometheus 0:a2603d7fa0ac 44 resize();
ZHAW_Prometheus 0:a2603d7fa0ac 45
ZHAW_Prometheus 0:a2603d7fa0ac 46 block = blocks + blockCount;
ZHAW_Prometheus 0:a2603d7fa0ac 47
ZHAW_Prometheus 0:a2603d7fa0ac 48 for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) {
ZHAW_Prometheus 0:a2603d7fa0ac 49 w = m_link->getWord();
ZHAW_Prometheus 0:a2603d7fa0ac 50 sum += w;
ZHAW_Prometheus 0:a2603d7fa0ac 51 *((uint16_t *)block + i) = w;
ZHAW_Prometheus 0:a2603d7fa0ac 52 }
ZHAW_Prometheus 0:a2603d7fa0ac 53
ZHAW_Prometheus 2:c3a866b20784 54 if (checksum == sum){
ZHAW_Prometheus 0:a2603d7fa0ac 55 blockCount++;
ZHAW_Prometheus 2:c3a866b20784 56 //w = m_link->getWord();
ZHAW_Prometheus 2:c3a866b20784 57 }
ZHAW_Prometheus 2:c3a866b20784 58 else if (pc) {
ZHAW_Prometheus 2:c3a866b20784 59 pc->printf("cs error\n\r");}
ZHAW_Prometheus 0:a2603d7fa0ac 60
ZHAW_Prometheus 0:a2603d7fa0ac 61 w = m_link->getWord();
ZHAW_Prometheus 0:a2603d7fa0ac 62 if (w != PIXY_START_WORD)
ZHAW_Prometheus 0:a2603d7fa0ac 63 return blockCount;
ZHAW_Prometheus 0:a2603d7fa0ac 64 }
ZHAW_Prometheus 0:a2603d7fa0ac 65 return blockCount;
ZHAW_Prometheus 0:a2603d7fa0ac 66 }
ZHAW_Prometheus 0:a2603d7fa0ac 67
ZHAW_Prometheus 0:a2603d7fa0ac 68 void Pixy::setAddress(uint8_t addr)
ZHAW_Prometheus 0:a2603d7fa0ac 69 {
ZHAW_Prometheus 0:a2603d7fa0ac 70 m_link->setAddress(addr);
ZHAW_Prometheus 0:a2603d7fa0ac 71 }
ZHAW_Prometheus 0:a2603d7fa0ac 72
ZHAW_Prometheus 0:a2603d7fa0ac 73 void Pixy::setSerialOutput(Serial *pc)
ZHAW_Prometheus 0:a2603d7fa0ac 74 {
ZHAW_Prometheus 0:a2603d7fa0ac 75 this->pc = pc;
ZHAW_Prometheus 0:a2603d7fa0ac 76 }
ZHAW_Prometheus 0:a2603d7fa0ac 77
ZHAW_Prometheus 0:a2603d7fa0ac 78 bool Pixy::getStart()
ZHAW_Prometheus 0:a2603d7fa0ac 79 {
ZHAW_Prometheus 0:a2603d7fa0ac 80 uint16_t w, lastw;
ZHAW_Prometheus 0:a2603d7fa0ac 81
ZHAW_Prometheus 0:a2603d7fa0ac 82 lastw = 0xffff;
ZHAW_Prometheus 0:a2603d7fa0ac 83 while (true) {
ZHAW_Prometheus 0:a2603d7fa0ac 84 w = m_link->getWord();
ZHAW_Prometheus 0:a2603d7fa0ac 85 if (w == 0 && lastw == 0) {
ZHAW_Prometheus 0:a2603d7fa0ac 86 wait_ms(10);
ZHAW_Prometheus 0:a2603d7fa0ac 87 return false;
ZHAW_Prometheus 0:a2603d7fa0ac 88 } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
ZHAW_Prometheus 0:a2603d7fa0ac 89 return true;
ZHAW_Prometheus 0:a2603d7fa0ac 90 else if (w == PIXY_START_WORDX) {
ZHAW_Prometheus 0:a2603d7fa0ac 91 if (pc)
ZHAW_Prometheus 0:a2603d7fa0ac 92 pc->printf("reorder\n\r");
ZHAW_Prometheus 0:a2603d7fa0ac 93 m_link->getByte(); // resync
ZHAW_Prometheus 0:a2603d7fa0ac 94 }
ZHAW_Prometheus 0:a2603d7fa0ac 95 lastw = w;
ZHAW_Prometheus 0:a2603d7fa0ac 96 }
ZHAW_Prometheus 0:a2603d7fa0ac 97 }
ZHAW_Prometheus 0:a2603d7fa0ac 98
ZHAW_Prometheus 0:a2603d7fa0ac 99 void Pixy::resize()
ZHAW_Prometheus 0:a2603d7fa0ac 100 {
ZHAW_Prometheus 0:a2603d7fa0ac 101 Block *newBlocks;
ZHAW_Prometheus 0:a2603d7fa0ac 102 blockArraySize += PIXY_INITIAL_ARRAYSIZE;
ZHAW_Prometheus 0:a2603d7fa0ac 103 newBlocks = new Block[blockArraySize];
ZHAW_Prometheus 0:a2603d7fa0ac 104 memcpy(newBlocks, blocks, sizeof(Block) * blockCount);
ZHAW_Prometheus 0:a2603d7fa0ac 105 delete[] blocks;
ZHAW_Prometheus 0:a2603d7fa0ac 106 blocks = newBlocks;
ZHAW_Prometheus 2:c3a866b20784 107 }