Diff: PixyStereoCam.cpp

Revision:

0:cac27bf8a28c

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