Diff: main.cpp

Revision:

0:cb6825644f06

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 12 08:15:44 2015 +0000
@@ -0,0 +1,62 @@
+//#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)));
+                }
+                }
+            }
+            }
+    
+}