jetfishteam / pixy_cam

an SPI interface the cmu pixy camera

Dependents:   robotic_fish_ver_4_9_pixy UMO

pixy.cpp@5:f54759b26096, 2014-06-05 (annotated)

Committer:
sandwich
Date:
Thu Jun 05 00:12:48 2014 +0000
Revision:
5:f54759b26096
Parent:
2:fc86438b206f
Child:
6:4bf8d39fc5ce
-switched to rtos to facilitate better threading; -integrated pixycam and there are now 2 operating modes; -pixycam tracking feedback isn't tuned yet. Currently using proportional controller

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 2:fc86438b206f 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 2:fc86438b206f 51 while (memcmp((char*)&sync, frame, 2)!=0) {
sandwich 2:fc86438b206f 52 frame[0]=frame[1]; //move byte down
sandwich 5:f54759b26096 53 frame[1]=spi.write(0x00); //get next byte.
sandwich 2:fc86438b206f 54 }
sandwich 5:f54759b26096 55 spi.write(0x00);
sandwich 2:fc86438b206f 56 //ok so we got a valid signature
sandwich 2:fc86438b206f 57 //these didn't end up working
sandwich 2:fc86438b206f 58 //readNBytes((char*)(&checksum), 2); //get the checksum
sandwich 2:fc86438b206f 59 //readNBytes((char*)(&out), sizeof(Block)); //get the rest of the data
sandwich 2:fc86438b206f 60 checksum=readTwoBytesLSB();
sandwich 2:fc86438b206f 61 out->signature=readTwoBytesLSB();
sandwich 2:fc86438b206f 62 out->x=readTwoBytesLSB();
sandwich 2:fc86438b206f 63 out->y=readTwoBytesLSB();
sandwich 2:fc86438b206f 64 out->width=readTwoBytesLSB();
sandwich 2:fc86438b206f 65 out->height=readTwoBytesLSB();
sandwich 2:fc86438b206f 66
sandwich 2:fc86438b206f 67 if (checksum!=(out->x+out->y+out->signature+out->width+out->height) || checksum==0) {
sandwich 2:fc86438b206f 68 //debug->printf("checksum doesn't add up %d\n", checksum);
sandwich 2:fc86438b206f 69 out->signature=INVALID_BLOCK; //used for invalid signatures
sandwich 2:fc86438b206f 70 }
sandwich 5:f54759b26096 71 //printf("found block\n");
sandwich 5:f54759b26096 72 bestX=blocks[0].x;
sandwich 5:f54759b26096 73 bestY=blocks[0].y;
sandwich 0:1b99a0bee9e0 74 }
sandwich 2:fc86438b206f 75 return;
sandwich 2:fc86438b206f 76 }
sandwich 2:fc86438b206f 77
sandwich 2:fc86438b206f 78 Block* pixySPI::getBlocks()
sandwich 2:fc86438b206f 79 {
sandwich 2:fc86438b206f 80 return blocks;
sandwich 2:fc86438b206f 81 }
sandwich 2:fc86438b206f 82
sandwich 2:fc86438b206f 83 int pixySPI::getNumBlocks()
sandwich 2:fc86438b206f 84 {
sandwich 2:fc86438b206f 85 return numBlocks;
sandwich 0:1b99a0bee9e0 86 }
sandwich 0:1b99a0bee9e0 87
sandwich 5:f54759b26096 88 int pixySPI::getBestX()
sandwich 5:f54759b26096 89 {
sandwich 5:f54759b26096 90 if (bestX>640)
sandwich 5:f54759b26096 91 return 640;
sandwich 5:f54759b26096 92 else if (bestX<0)
sandwich 5:f54759b26096 93 return 0;
sandwich 5:f54759b26096 94 else
sandwich 5:f54759b26096 95 return bestX;
sandwich 5:f54759b26096 96 }
sandwich 5:f54759b26096 97
sandwich 5:f54759b26096 98 int pixySPI::getBestY()
sandwich 5:f54759b26096 99 {
sandwich 5:f54759b26096 100 if (bestY>400)
sandwich 5:f54759b26096 101 return 400;
sandwich 5:f54759b26096 102 else if (bestY<0)
sandwich 5:f54759b26096 103 return 0;
sandwich 5:f54759b26096 104 else
sandwich 5:f54759b26096 105 return bestY;
sandwich 5:f54759b26096 106 }
sandwich 5:f54759b26096 107
sandwich 0:1b99a0bee9e0 108 char pixySPI::getRawData()
sandwich 0:1b99a0bee9e0 109 {
sandwich 5:f54759b26096 110 return spi.write(0x00);
sandwich 0:1b99a0bee9e0 111 }