an SPI interface the cmu pixy camera
Dependents: robotic_fish_ver_4_9_pixy UMO
pixy.cpp@8:a915c5eff55a, 2014-06-06 (annotated)
- Committer:
- sandwich
- Date:
- Fri Jun 06 19:47:27 2014 +0000
- Revision:
- 8:a915c5eff55a
- Parent:
- 7:e57b18779bf1
- Child:
- 9:89b9d1a6457c
??
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sandwich | 0:1b99a0bee9e0 | 1 | #include "pixy.h" |
sandwich | 0:1b99a0bee9e0 | 2 | |
sandwich | 5:f54759b26096 | 3 | pixySPI::pixySPI(PinName mosi, PinName miso, PinName sclk, int nBlocks) |
sandwich | 5:f54759b26096 | 4 | :spi(mosi,miso,sclk) |
sandwich | 0:1b99a0bee9e0 | 5 | { |
sandwich | 5:f54759b26096 | 6 | spi.format(8,0); //8bits mode 0 |
sandwich | 5:f54759b26096 | 7 | spi.frequency(100000); //set the frequency in Hz |
sandwich | 2:fc86438b206f | 8 | sync=0xAA55; //start word of a frame coming from the pixy |
sandwich | 5:f54759b26096 | 9 | //blocks=new Block[nBlocks]; //allocate space for the blocks |
sandwich | 5:f54759b26096 | 10 | //numBlocks=nBlocks; |
sandwich | 5:f54759b26096 | 11 | numBlocks=NUM_BLOCKS; |
sandwich | 0:1b99a0bee9e0 | 12 | } |
sandwich | 0:1b99a0bee9e0 | 13 | |
sandwich | 0:1b99a0bee9e0 | 14 | pixySPI::~pixySPI() |
sandwich | 0:1b99a0bee9e0 | 15 | { |
sandwich | 2:fc86438b206f | 16 | delete blocks; |
sandwich | 0:1b99a0bee9e0 | 17 | } |
sandwich | 0:1b99a0bee9e0 | 18 | |
sandwich | 0:1b99a0bee9e0 | 19 | |
sandwich | 0:1b99a0bee9e0 | 20 | short pixySPI::readTwoBytesLSB() |
sandwich | 0:1b99a0bee9e0 | 21 | { |
sandwich | 0:1b99a0bee9e0 | 22 | short out; |
sandwich | 2:fc86438b206f | 23 | char read[2]= {0,0}; |
sandwich | 5:f54759b26096 | 24 | read[0]=spi.write(0x00); |
sandwich | 5:f54759b26096 | 25 | read[1]=spi.write(0x00); |
sandwich | 0:1b99a0bee9e0 | 26 | out=(((short)read[0]) << 8) | read[1]; |
sandwich | 0:1b99a0bee9e0 | 27 | return out; |
sandwich | 0:1b99a0bee9e0 | 28 | } |
sandwich | 0:1b99a0bee9e0 | 29 | |
sandwich | 0:1b99a0bee9e0 | 30 | |
sandwich | 0:1b99a0bee9e0 | 31 | //this doesn't work |
sandwich | 0:1b99a0bee9e0 | 32 | void pixySPI::readNBytes(char* buf, int num) |
sandwich | 0:1b99a0bee9e0 | 33 | { |
sandwich | 0:1b99a0bee9e0 | 34 | for (int i=0; i<num; i++) { |
sandwich | 5:f54759b26096 | 35 | char byte=spi.write(0x00); |
sandwich | 0:1b99a0bee9e0 | 36 | memcpy(buf+i, &byte, 1); |
sandwich | 0:1b99a0bee9e0 | 37 | } |
sandwich | 0:1b99a0bee9e0 | 38 | } |
sandwich | 0:1b99a0bee9e0 | 39 | |
sandwich | 8:a915c5eff55a | 40 | void pixySPI::capture() |
sandwich | 0:1b99a0bee9e0 | 41 | { |
sandwich | 5:f54759b26096 | 42 | memset(blocks, 0, numBlocks*sizeof(Block)); //destroy the old targets |
sandwich | 2:fc86438b206f | 43 | for (int i=0; i<numBlocks; ++i) { |
sandwich | 5:f54759b26096 | 44 | // printf("block %d\n", i); |
sandwich | 2:fc86438b206f | 45 | Block* out=&blocks[i]; |
sandwich | 2:fc86438b206f | 46 | out->signature=INVALID_BLOCK; |
sandwich | 2:fc86438b206f | 47 | uint16_t checksum=0; |
sandwich | 2:fc86438b206f | 48 | //first we need to detect the start of a block. They all start with 0xAA55 |
sandwich | 2:fc86438b206f | 49 | char frame[2]= {0,0}; //this is a 2 byte running frame of what is being recieved. It's like a first-in-last-out queue |
sandwich | 2:fc86438b206f | 50 | //debug->printf("looking for valid signature\n"); |
sandwich | 8:a915c5eff55a | 51 | Timer t; |
sandwich | 8:a915c5eff55a | 52 | t.start(); |
sandwich | 2:fc86438b206f | 53 | while (memcmp((char*)&sync, frame, 2)!=0) { |
sandwich | 2:fc86438b206f | 54 | frame[0]=frame[1]; //move byte down |
sandwich | 5:f54759b26096 | 55 | frame[1]=spi.write(0x00); //get next byte. |
sandwich | 8:a915c5eff55a | 56 | if (t.read_ms()>5) |
sandwich | 8:a915c5eff55a | 57 | { |
sandwich | 8:a915c5eff55a | 58 | t.stop(); |
sandwich | 8:a915c5eff55a | 59 | return; |
sandwich | 8:a915c5eff55a | 60 | } |
sandwich | 2:fc86438b206f | 61 | } |
sandwich | 8:a915c5eff55a | 62 | //printf("target\n\n"); |
sandwich | 5:f54759b26096 | 63 | spi.write(0x00); |
sandwich | 2:fc86438b206f | 64 | //ok so we got a valid signature |
sandwich | 2:fc86438b206f | 65 | //these didn't end up working |
sandwich | 2:fc86438b206f | 66 | //readNBytes((char*)(&checksum), 2); //get the checksum |
sandwich | 2:fc86438b206f | 67 | //readNBytes((char*)(&out), sizeof(Block)); //get the rest of the data |
sandwich | 2:fc86438b206f | 68 | checksum=readTwoBytesLSB(); |
sandwich | 2:fc86438b206f | 69 | out->signature=readTwoBytesLSB(); |
sandwich | 2:fc86438b206f | 70 | out->x=readTwoBytesLSB(); |
sandwich | 2:fc86438b206f | 71 | out->y=readTwoBytesLSB(); |
sandwich | 2:fc86438b206f | 72 | out->width=readTwoBytesLSB(); |
sandwich | 2:fc86438b206f | 73 | out->height=readTwoBytesLSB(); |
sandwich | 2:fc86438b206f | 74 | |
sandwich | 2:fc86438b206f | 75 | if (checksum!=(out->x+out->y+out->signature+out->width+out->height) || checksum==0) { |
sandwich | 2:fc86438b206f | 76 | //debug->printf("checksum doesn't add up %d\n", checksum); |
sandwich | 2:fc86438b206f | 77 | out->signature=INVALID_BLOCK; //used for invalid signatures |
sandwich | 2:fc86438b206f | 78 | } |
sandwich | 5:f54759b26096 | 79 | //printf("found block\n"); |
sandwich | 6:4bf8d39fc5ce | 80 | if (blocks[0].signature!=INVALID_BLOCK) |
sandwich | 6:4bf8d39fc5ce | 81 | { |
sandwich | 8:a915c5eff55a | 82 | //mutex->lock(); |
sandwich | 6:4bf8d39fc5ce | 83 | bestX=blocks[0].x; |
sandwich | 6:4bf8d39fc5ce | 84 | bestY=blocks[0].y; |
sandwich | 8:a915c5eff55a | 85 | //mutex->unlock(); |
sandwich | 6:4bf8d39fc5ce | 86 | } |
sandwich | 0:1b99a0bee9e0 | 87 | } |
sandwich | 2:fc86438b206f | 88 | return; |
sandwich | 2:fc86438b206f | 89 | } |
sandwich | 2:fc86438b206f | 90 | |
sandwich | 2:fc86438b206f | 91 | Block* pixySPI::getBlocks() |
sandwich | 2:fc86438b206f | 92 | { |
sandwich | 2:fc86438b206f | 93 | return blocks; |
sandwich | 2:fc86438b206f | 94 | } |
sandwich | 2:fc86438b206f | 95 | |
sandwich | 2:fc86438b206f | 96 | int pixySPI::getNumBlocks() |
sandwich | 2:fc86438b206f | 97 | { |
sandwich | 2:fc86438b206f | 98 | return numBlocks; |
sandwich | 0:1b99a0bee9e0 | 99 | } |
sandwich | 0:1b99a0bee9e0 | 100 | |
sandwich | 5:f54759b26096 | 101 | int pixySPI::getBestX() |
sandwich | 5:f54759b26096 | 102 | { |
sandwich | 5:f54759b26096 | 103 | if (bestX>640) |
sandwich | 5:f54759b26096 | 104 | return 640; |
sandwich | 5:f54759b26096 | 105 | else if (bestX<0) |
sandwich | 5:f54759b26096 | 106 | return 0; |
sandwich | 5:f54759b26096 | 107 | else |
sandwich | 5:f54759b26096 | 108 | return bestX; |
sandwich | 5:f54759b26096 | 109 | } |
sandwich | 5:f54759b26096 | 110 | |
sandwich | 5:f54759b26096 | 111 | int pixySPI::getBestY() |
sandwich | 5:f54759b26096 | 112 | { |
sandwich | 5:f54759b26096 | 113 | if (bestY>400) |
sandwich | 5:f54759b26096 | 114 | return 400; |
sandwich | 5:f54759b26096 | 115 | else if (bestY<0) |
sandwich | 5:f54759b26096 | 116 | return 0; |
sandwich | 5:f54759b26096 | 117 | else |
sandwich | 5:f54759b26096 | 118 | return bestY; |
sandwich | 5:f54759b26096 | 119 | } |
sandwich | 5:f54759b26096 | 120 | |
sandwich | 0:1b99a0bee9e0 | 121 | char pixySPI::getRawData() |
sandwich | 0:1b99a0bee9e0 | 122 | { |
sandwich | 5:f54759b26096 | 123 | return spi.write(0x00); |
sandwich | 0:1b99a0bee9e0 | 124 | } |