Gruppe 3 / pixy

pixy Libari 2

Dependents:   PES PES1 PES4 PES3

Pixy.cpp@2:b0a6783fb615, 2017-04-19 (annotated)

Committer:
Shukle
Date:
Wed Apr 19 10:13:33 2017 +0000
Revision:
2:b0a6783fb615
Parent:
1:a57585d5e81b 
read Camera hinzugef?gt

Who changed what in which revision?

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