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);
            }
        }
    }  
}