Uses multiple Pixy coordinates, communicated through SPI, to estimate the distance between the object and the parallel cameras.
Diff: PixyStereoCam.cpp
- Revision:
- 0:cac27bf8a28c
diff -r 000000000000 -r cac27bf8a28c PixyStereoCam.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PixyStereoCam.cpp Tue Aug 12 11:02:24 2014 +0000 @@ -0,0 +1,119 @@ +//-------------------------------------------------------------------------------------------- +//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); + } + } + } +} + \ No newline at end of file