AiRDrum project pixy tracking module main program
Dependencies: StereoTriagulation mbed pixy
main.cpp
- Committer:
- CheeseW
- Date:
- 2015-03-12
- Revision:
- 0:cb6825644f06
File content as of revision 0:cb6825644f06:
//#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))); } } } } }