File content as of revision 1:bd16d28c221c:

//#include "mbed.h"
#include "StereoCamera.h"
#include "Pixy.h"

Pixy pixyR(Pixy::SPI, p5, p6, p7);
Pixy pixyL(Pixy::SPI, p11, p12, p13);
Serial pc(USBTX, USBRX);

void printPoint(Vec3D<double> p)
{
    pc.printf("x%fy%fz%f\n\r", p.x,p.y,p.z);   
};

int main() {
    pc.baud(115200);
    //int i = 0;
    const int sigN = 2;
    /*double R[9] =   {0.9998,    0.0047,    0.0172,
                    -0.0045,    1.0000,   -0.0073,
                    -0.0173,    0.0072,    0.9998};
    Camera left(fcX,   fcY, ccX,   ccY, k1,  k2 ,  k3,   k4);
    Camera right(237.58194  , 239.97612, 166.13773 ,  118.99173, -0.44335 ,  0.22962 ,  -0.00675 ,  -0.00106);
    StereoCamera pixyS(left, right, R, Vec3D<double>(56.38457,   -1.12954,  -1.77026));*/
    
    double R[9] =  { 0.9999,    0.0134,    0.0075,
                    -0.0133,    0.9999,   -0.0090,
                    -0.0076,    0.0089,    0.9999};
    Camera left(243.76014,   241.61370, 170.94012,   91.58828,  -0.42595,   0.18390,   0.00160,   -0.00186);
    Camera right(240.05098,   238.91093, 158.97058,   89.72589, -0.42929,   0.16953,   -0.00193,   -0.00057);
    //Vec3D<double> T( -52.89272,   -0.31508,  -0.64784);
    StereoCamera pixyS(left, right, R,Vec3D<double>( -52.89272,   -0.31508,  -0.64784));
    
    uint16_t blockL;
    uint16_t blockR;
    int count;
        while (1)
        {
            count = 0;
    blockL = pixyL.getBlocks();
    blockR = pixyR.getBlocks();
    //pc.printf("%d\n\r",i++);
    if (blockL && blockR)
    {
        pc.printf("f");
         for (int j = 0; j < sigN; j++) {
                     if (pixyR.blocks[j].width &&pixyL.blocks[j].width)
                     {
                       count++;
                }
                }
                pc.printf("%d\n\r", count);
    for (int j = 0; j < sigN; j++) {
                     if (pixyR.blocks[j].width &&pixyL.blocks[j].width)
                     {
                         pc.printf("s%d",j);
                     printPoint(pixyS.triangulation(Vec2D<int>(pixyL.blocks[j].x,pixyL.blocks[j].y), Vec2D<int>(pixyR.blocks[j].x,pixyR.blocks[j].y)));
                }
                }
            }
            }
    
}