PixyStereoCam.cpp

00001 //--------------------------------------------------------------------------------------------
00002 //Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip 
00003 //
00004 //Modifications made by: Mathieu Malone
00005 //Modifications: Modified Arduino code to function with mbed development platform
00006 //Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication
00007 //               between the mbed platform and putty terminal through USB.
00008 //
00009 //Purpose: This program uses two CMUcam5 Pixy cameras in parallel, and allows the coordinates 
00010 //         gathered from each camera to be used to estimate a distance 'z' from the cameras.
00011 //
00012 //Latest update by: Mathieu Malone
00013 //Date of last update: August 12th, 2014
00014 //--------------------------------------------------------------------------------------------
00015 
00016 double Xp1; double Xp2; //Global variable keeping track of the X coordinates in TPixy#.h
00017 double Yp1; double Yp2; //Global variable keeping track of the Y coordinates in TPixy#.h
00018 double sig1; double sig2; //Global variable to keep track of object in view in TPixy#.h
00019 
00020 #include "mbed.h"
00021 #include "Pixy1.h"
00022 #include "Pixy2.h"
00023 #include "Motor.h"
00024 
00025 #define     pixel   1.2e-5  //pixel dimensions
00026 #define     f       2.8e-3  //focal length
00027 #define     s       7.75e-2    //distance between lenses
00028 #define     xc      160     //center coordinates for x-axis in pixels
00029 #define     pi      3.141592653589793
00030 
00031 // Distance Calculation Variables
00032 double X1; double X2;
00033 double Theta1; double Theta2;
00034 double s1; double s2; double z;
00035 
00036 int main(){
00037 
00038     //Right PIXY
00039     Serial pc1(USBTX, USBRX);
00040     Pixy1 pixy1;
00041     SPI spi1(p5, p6, p7);  // mosi, miso, sclk
00042     spi1.format(16,0);     //16 bits mode 0
00043     spi1.frequency(100000);//set the frequency in Hz
00044     pc1.printf("\r Starting #1...\n");
00045         
00046     //Left PIXY
00047     Serial pc2(USBTX, USBRX);
00048     Pixy2 pixy2;
00049     SPI spi2(p11, p12, p13);// mosi, miso, sclk
00050     spi2.format(16,0);      //16 bits mode 0
00051     spi2.frequency(100000); //set the frequency in Hz
00052     pc2.printf("\r Starting #2...\n");
00053           
00054     while(1){ 
00055     
00056         static int i1 = 0;
00057         int j1;
00058         uint16_t blocks1;
00059         char buf1[32]; 
00060           
00061         blocks1 = pixy1.getBlocks1();
00062           
00063         if (blocks1)
00064         {
00065             i1++;
00066             
00067             if (i1%50==0)
00068             {
00069             sprintf(buf1, "\r Detected #1 %d:\r\n", blocks1);
00070             pc1.printf(buf1);
00071                 for (j1=0; j1<blocks1; j1++)
00072                 {
00073                     sprintf(buf1, " block #1 %d: ", j1);
00074                     printf(buf1); 
00075                     pixy1.blocks1[j1].print1();
00076                 }
00077             }
00078         }  
00079         
00080         static int i2 = 0;
00081         int j2;
00082         uint16_t blocks2;
00083         char buf2[32]; 
00084           
00085         blocks2 = pixy2.getBlocks2();
00086           
00087         if (blocks2)
00088         {
00089             i2++;
00090             
00091                 if (i2%50==0)
00092                 {
00093                     sprintf(buf2, "\r Detected #2 %d:\r\n", blocks2);
00094                     pc2.printf(buf2);
00095                     for (j2=0; j2<blocks2; j2++)
00096                     {
00097                         sprintf(buf2, " block #2 %d: ", j2);
00098                         printf(buf2); 
00099                         pixy2.blocks2[j2].print2();
00100                     }
00101                 }
00102             }
00103           
00104         if (blocks2 || blocks1){
00105             if ((Xp1 != 0) && (Xp2 != 0) && ((i1%50==0) || (i2%50==0))){
00106                     //pc1.printf("\r Xp1 = %f\r\n",Xp1); pc2.printf(" Xp2 = %f\r\n",Xp2);
00107                     X1 = (Xp1-xc)*pixel; X2 = (Xp2-xc)*pixel;
00108                     //pc1.printf(" X1 = %f\r\n",X1); pc2.printf(" X2 = %f\r\n",X2);
00109                     Theta1 = atan((f/X1)); Theta2 = atan((f/X2));
00110                     //pc1.printf(" Theta1 = %f\r\n",Theta1); pc2.printf(" Theta2 = %f\r\n",Theta2);
00111                     s1 = abs(s*(sin(pi-Theta2)/sin(Theta2-Theta1))); s2 = abs(s*(sin(Theta1)/sin(Theta2-Theta1)));
00112                     //pc1.printf(" s1 = %f\r\n",s1); pc2.printf(" s2 = %f\r\n",s2);
00113                     z = (s1+s2)/2;
00114                     pc1.printf("\r Distances is: %f meters \r\n", z);
00115             }
00116         }
00117     }  
00118 }
00119