jetfishteam / pixy_cam

an SPI interface the cmu pixy camera

Dependents:   robotic_fish_ver_4_9_pixy UMO

pixy.cpp@9:89b9d1a6457c, 2014-07-11 (annotated)

Committer:
sandwich
Date:
Fri Jul 11 14:30:06 2014 +0000
Revision:
9:89b9d1a6457c
Parent:
8:a915c5eff55a 
..

Who changed what in which revision?

UserRevisionLine numberNew 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 9:89b9d1a6457c 12 bestX=CENTER_X;
sandwich 9:89b9d1a6457c 13 bestY=CENTER_Y;
sandwich 0:1b99a0bee9e0 14 }
sandwich 0:1b99a0bee9e0 15
sandwich 0:1b99a0bee9e0 16 pixySPI::~pixySPI()
sandwich 0:1b99a0bee9e0 17 {
sandwich 2:fc86438b206f 18 delete blocks;
sandwich 0:1b99a0bee9e0 19 }
sandwich 0:1b99a0bee9e0 20
sandwich 0:1b99a0bee9e0 21
sandwich 0:1b99a0bee9e0 22 short pixySPI::readTwoBytesLSB()
sandwich 0:1b99a0bee9e0 23 {
sandwich 0:1b99a0bee9e0 24 short out;
sandwich 2:fc86438b206f 25 char read[2]= {0,0};
sandwich 5:f54759b26096 26 read[0]=spi.write(0x00);
sandwich 5:f54759b26096 27 read[1]=spi.write(0x00);
sandwich 0:1b99a0bee9e0 28 out=(((short)read[0]) << 8) | read[1];
sandwich 0:1b99a0bee9e0 29 return out;
sandwich 0:1b99a0bee9e0 30 }
sandwich 0:1b99a0bee9e0 31
sandwich 0:1b99a0bee9e0 32
sandwich 0:1b99a0bee9e0 33 //this doesn't work
sandwich 0:1b99a0bee9e0 34 void pixySPI::readNBytes(char* buf, int num)
sandwich 0:1b99a0bee9e0 35 {
sandwich 0:1b99a0bee9e0 36 for (int i=0; i<num; i++) {
sandwich 5:f54759b26096 37 char byte=spi.write(0x00);
sandwich 0:1b99a0bee9e0 38 memcpy(buf+i, &byte, 1);
sandwich 0:1b99a0bee9e0 39 }
sandwich 0:1b99a0bee9e0 40 }
sandwich 0:1b99a0bee9e0 41
sandwich 8:a915c5eff55a 42 void pixySPI::capture()
sandwich 0:1b99a0bee9e0 43 {
sandwich 5:f54759b26096 44 memset(blocks, 0, numBlocks*sizeof(Block)); //destroy the old targets
sandwich 2:fc86438b206f 45 for (int i=0; i<numBlocks; ++i) {
sandwich 5:f54759b26096 46 // printf("block %d\n", i);
sandwich 2:fc86438b206f 47 Block* out=&blocks[i];
sandwich 2:fc86438b206f 48 out->signature=INVALID_BLOCK;
sandwich 2:fc86438b206f 49 uint16_t checksum=0;
sandwich 2:fc86438b206f 50 //first we need to detect the start of a block. They all start with 0xAA55
sandwich 2:fc86438b206f 51 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 52 //debug->printf("looking for valid signature\n");
sandwich 8:a915c5eff55a 53 Timer t;
sandwich 8:a915c5eff55a 54 t.start();
sandwich 2:fc86438b206f 55 while (memcmp((char*)&sync, frame, 2)!=0) {
sandwich 2:fc86438b206f 56 frame[0]=frame[1]; //move byte down
sandwich 5:f54759b26096 57 frame[1]=spi.write(0x00); //get next byte.
sandwich 8:a915c5eff55a 58 if (t.read_ms()>5)
sandwich 8:a915c5eff55a 59 {
sandwich 8:a915c5eff55a 60 t.stop();
sandwich 8:a915c5eff55a 61 return;
sandwich 8:a915c5eff55a 62 }
sandwich 2:fc86438b206f 63 }
sandwich 8:a915c5eff55a 64 //printf("target\n\n");
sandwich 5:f54759b26096 65 spi.write(0x00);
sandwich 2:fc86438b206f 66 //ok so we got a valid signature
sandwich 2:fc86438b206f 67 //these didn't end up working
sandwich 2:fc86438b206f 68 //readNBytes((char*)(&checksum), 2); //get the checksum
sandwich 2:fc86438b206f 69 //readNBytes((char*)(&out), sizeof(Block)); //get the rest of the data
sandwich 2:fc86438b206f 70 checksum=readTwoBytesLSB();
sandwich 2:fc86438b206f 71 out->signature=readTwoBytesLSB();
sandwich 2:fc86438b206f 72 out->x=readTwoBytesLSB();
sandwich 2:fc86438b206f 73 out->y=readTwoBytesLSB();
sandwich 2:fc86438b206f 74 out->width=readTwoBytesLSB();
sandwich 2:fc86438b206f 75 out->height=readTwoBytesLSB();
sandwich 2:fc86438b206f 76
sandwich 2:fc86438b206f 77 if (checksum!=(out->x+out->y+out->signature+out->width+out->height) || checksum==0) {
sandwich 2:fc86438b206f 78 //debug->printf("checksum doesn't add up %d\n", checksum);
sandwich 2:fc86438b206f 79 out->signature=INVALID_BLOCK; //used for invalid signatures
sandwich 2:fc86438b206f 80 }
sandwich 5:f54759b26096 81 //printf("found block\n");
sandwich 6:4bf8d39fc5ce 82 if (blocks[0].signature!=INVALID_BLOCK)
sandwich 6:4bf8d39fc5ce 83 {
sandwich 8:a915c5eff55a 84 //mutex->lock();
sandwich 6:4bf8d39fc5ce 85 bestX=blocks[0].x;
sandwich 6:4bf8d39fc5ce 86 bestY=blocks[0].y;
sandwich 8:a915c5eff55a 87 //mutex->unlock();
sandwich 6:4bf8d39fc5ce 88 }
sandwich 0:1b99a0bee9e0 89 }
sandwich 2:fc86438b206f 90 return;
sandwich 2:fc86438b206f 91 }
sandwich 2:fc86438b206f 92
sandwich 2:fc86438b206f 93 Block* pixySPI::getBlocks()
sandwich 2:fc86438b206f 94 {
sandwich 2:fc86438b206f 95 return blocks;
sandwich 2:fc86438b206f 96 }
sandwich 2:fc86438b206f 97
sandwich 2:fc86438b206f 98 int pixySPI::getNumBlocks()
sandwich 2:fc86438b206f 99 {
sandwich 2:fc86438b206f 100 return numBlocks;
sandwich 0:1b99a0bee9e0 101 }
sandwich 0:1b99a0bee9e0 102
sandwich 5:f54759b26096 103 int pixySPI::getBestX()
sandwich 5:f54759b26096 104 {
sandwich 5:f54759b26096 105 if (bestX>640)
sandwich 5:f54759b26096 106 return 640;
sandwich 5:f54759b26096 107 else if (bestX<0)
sandwich 5:f54759b26096 108 return 0;
sandwich 5:f54759b26096 109 else
sandwich 5:f54759b26096 110 return bestX;
sandwich 5:f54759b26096 111 }
sandwich 5:f54759b26096 112
sandwich 5:f54759b26096 113 int pixySPI::getBestY()
sandwich 5:f54759b26096 114 {
sandwich 5:f54759b26096 115 if (bestY>400)
sandwich 5:f54759b26096 116 return 400;
sandwich 5:f54759b26096 117 else if (bestY<0)
sandwich 5:f54759b26096 118 return 0;
sandwich 5:f54759b26096 119 else
sandwich 5:f54759b26096 120 return bestY;
sandwich 5:f54759b26096 121 }
sandwich 5:f54759b26096 122
sandwich 0:1b99a0bee9e0 123 char pixySPI::getRawData()
sandwich 0:1b99a0bee9e0 124 {
sandwich 5:f54759b26096 125 return spi.write(0x00);
sandwich 0:1b99a0bee9e0 126 }