AiRDrum project pixy tracking module main program
Dependencies: StereoTriagulation mbed pixy
Diff: main.cpp
- Revision:
- 0:cb6825644f06
diff -r 000000000000 -r cb6825644f06 main.cpp --- /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))); + } + } + } + } + +}