jetfishteam / pixy_cam

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?

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 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 }