Alexandre Pirotte / pixy_ok

programme ok

Fork of pixy by Arcadie Cracan

Pixy.cpp@1:a4c25c179e7a, 2017-06-23 (annotated)

Committer:
pirottealex
Date:
Fri Jun 23 11:18:38 2017 +0000
Revision:
1:a4c25c179e7a
Parent:
0:ed8dc4531ac1 
version ok;

Who changed what in which revision?

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