Uses multiple Pixy coordinates, communicated through SPI, to estimate the distance between the object and the parallel cameras.
PixyStereoCam.cpp
- Committer:
- MBM
- Date:
- 2014-08-12
- Revision:
- 1:b742e6df453e
- Parent:
- 0:cac27bf8a28c
File content as of revision 1:b742e6df453e:
//-------------------------------------------------------------------------------------------- //Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip // //Modifications made by: Mathieu Malone //Modifications: Modified Arduino code to function with mbed development platform //Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication // between the mbed platform and putty terminal through USB. // //Purpose: This program uses two CMUcam5 Pixy cameras in parallel, and allows the coordinates // gathered from each camera to be used to estimate a distance 'z' from the cameras. // //Latest update by: Mathieu Malone //Date of last update: August 12th, 2014 //-------------------------------------------------------------------------------------------- double Xp1; double Xp2; //Global variable keeping track of the X coordinates in TPixy#.h double Yp1; double Yp2; //Global variable keeping track of the Y coordinates in TPixy#.h double sig1; double sig2; //Global variable to keep track of object in view in TPixy#.h #include "mbed.h" #include "Pixy1.h" #include "Pixy2.h" #include "Motor.h" #define pixel 1.2e-5 //pixel dimensions #define f 2.8e-3 //focal length #define s 7.75e-2 //distance between lenses #define xc 160 //center coordinates for x-axis in pixels #define pi 3.141592653589793 // Distance Calculation Variables double X1; double X2; double Theta1; double Theta2; double s1; double s2; double z; int main(){ //Right PIXY Serial pc1(USBTX, USBRX); Pixy1 pixy1; SPI spi1(p5, p6, p7); // mosi, miso, sclk spi1.format(16,0); //16 bits mode 0 spi1.frequency(100000);//set the frequency in Hz pc1.printf("\r Starting #1...\n"); //Left PIXY Serial pc2(USBTX, USBRX); Pixy2 pixy2; SPI spi2(p11, p12, p13);// mosi, miso, sclk spi2.format(16,0); //16 bits mode 0 spi2.frequency(100000); //set the frequency in Hz pc2.printf("\r Starting #2...\n"); while(1){ static int i1 = 0; int j1; uint16_t blocks1; char buf1[32]; blocks1 = pixy1.getBlocks1(); if (blocks1) { i1++; if (i1%50==0) { sprintf(buf1, "\r Detected #1 %d:\r\n", blocks1); pc1.printf(buf1); for (j1=0; j1<blocks1; j1++) { sprintf(buf1, " block #1 %d: ", j1); printf(buf1); pixy1.blocks1[j1].print1(); } } } static int i2 = 0; int j2; uint16_t blocks2; char buf2[32]; blocks2 = pixy2.getBlocks2(); if (blocks2) { i2++; if (i2%50==0) { sprintf(buf2, "\r Detected #2 %d:\r\n", blocks2); pc2.printf(buf2); for (j2=0; j2<blocks2; j2++) { sprintf(buf2, " block #2 %d: ", j2); printf(buf2); pixy2.blocks2[j2].print2(); } } } if (blocks2 || blocks1){ if ((Xp1 != 0) && (Xp2 != 0) && ((i1%50==0) || (i2%50==0))){ //pc1.printf("\r Xp1 = %f\r\n",Xp1); pc2.printf(" Xp2 = %f\r\n",Xp2); X1 = (Xp1-xc)*pixel; X2 = (Xp2-xc)*pixel; //pc1.printf(" X1 = %f\r\n",X1); pc2.printf(" X2 = %f\r\n",X2); Theta1 = atan((f/X1)); Theta2 = atan((f/X2)); //pc1.printf(" Theta1 = %f\r\n",Theta1); pc2.printf(" Theta2 = %f\r\n",Theta2); s1 = abs(s*(sin(pi-Theta2)/sin(Theta2-Theta1))); s2 = abs(s*(sin(Theta1)/sin(Theta2-Theta1))); //pc1.printf(" s1 = %f\r\n",s1); pc2.printf(" s2 = %f\r\n",s2); z = (s1+s2)/2; pc1.printf("\r Distances is: %f meters \r\n", z); } } } }