Qisi Wang / Mbed 2 deprecated AiRDrummer_Visual

AiRDrum project pixy tracking module main program

Dependencies:   StereoTriagulation mbed pixy

main.cpp@1:bd16d28c221c, 2015-03-12 (annotated)

Committer:
CheeseW
Date:
Thu Mar 12 08:20:41 2015 +0000
Revision:
1:bd16d28c221c
Parent:
0:cb6825644f06 
AiRDrums project pixy tracking module main program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CheeseW 0:cb6825644f06 1 //#include "mbed.h"
CheeseW 0:cb6825644f06 2 #include "StereoCamera.h"
CheeseW 0:cb6825644f06 3 #include "Pixy.h"
CheeseW 0:cb6825644f06 4
CheeseW 0:cb6825644f06 5 Pixy pixyR(Pixy::SPI, p5, p6, p7);
CheeseW 0:cb6825644f06 6 Pixy pixyL(Pixy::SPI, p11, p12, p13);
CheeseW 0:cb6825644f06 7 Serial pc(USBTX, USBRX);
CheeseW 0:cb6825644f06 8
CheeseW 0:cb6825644f06 9 void printPoint(Vec3D<double> p)
CheeseW 0:cb6825644f06 10 {
CheeseW 0:cb6825644f06 11 pc.printf("x%fy%fz%f\n\r", p.x,p.y,p.z);
CheeseW 0:cb6825644f06 12 };
CheeseW 0:cb6825644f06 13
CheeseW 0:cb6825644f06 14 int main() {
CheeseW 0:cb6825644f06 15 pc.baud(115200);
CheeseW 0:cb6825644f06 16 //int i = 0;
CheeseW 0:cb6825644f06 17 const int sigN = 2;
CheeseW 0:cb6825644f06 18 /*double R[9] = {0.9998, 0.0047, 0.0172,
CheeseW 0:cb6825644f06 19 -0.0045, 1.0000, -0.0073,
CheeseW 0:cb6825644f06 20 -0.0173, 0.0072, 0.9998};
CheeseW 0:cb6825644f06 21 Camera left(fcX, fcY, ccX, ccY, k1, k2 , k3, k4);
CheeseW 0:cb6825644f06 22 Camera right(237.58194 , 239.97612, 166.13773 , 118.99173, -0.44335 , 0.22962 , -0.00675 , -0.00106);
CheeseW 0:cb6825644f06 23 StereoCamera pixyS(left, right, R, Vec3D<double>(56.38457, -1.12954, -1.77026));*/
CheeseW 0:cb6825644f06 24
CheeseW 0:cb6825644f06 25 double R[9] = { 0.9999, 0.0134, 0.0075,
CheeseW 0:cb6825644f06 26 -0.0133, 0.9999, -0.0090,
CheeseW 0:cb6825644f06 27 -0.0076, 0.0089, 0.9999};
CheeseW 0:cb6825644f06 28 Camera left(243.76014, 241.61370, 170.94012, 91.58828, -0.42595, 0.18390, 0.00160, -0.00186);
CheeseW 0:cb6825644f06 29 Camera right(240.05098, 238.91093, 158.97058, 89.72589, -0.42929, 0.16953, -0.00193, -0.00057);
CheeseW 0:cb6825644f06 30 //Vec3D<double> T( -52.89272, -0.31508, -0.64784);
CheeseW 0:cb6825644f06 31 StereoCamera pixyS(left, right, R,Vec3D<double>( -52.89272, -0.31508, -0.64784));
CheeseW 0:cb6825644f06 32
CheeseW 0:cb6825644f06 33 uint16_t blockL;
CheeseW 0:cb6825644f06 34 uint16_t blockR;
CheeseW 0:cb6825644f06 35 int count;
CheeseW 0:cb6825644f06 36 while (1)
CheeseW 0:cb6825644f06 37 {
CheeseW 0:cb6825644f06 38 count = 0;
CheeseW 0:cb6825644f06 39 blockL = pixyL.getBlocks();
CheeseW 0:cb6825644f06 40 blockR = pixyR.getBlocks();
CheeseW 0:cb6825644f06 41 //pc.printf("%d\n\r",i++);
CheeseW 0:cb6825644f06 42 if (blockL && blockR)
CheeseW 0:cb6825644f06 43 {
CheeseW 0:cb6825644f06 44 pc.printf("f");
CheeseW 0:cb6825644f06 45 for (int j = 0; j < sigN; j++) {
CheeseW 0:cb6825644f06 46 if (pixyR.blocks[j].width &&pixyL.blocks[j].width)
CheeseW 0:cb6825644f06 47 {
CheeseW 0:cb6825644f06 48 count++;
CheeseW 0:cb6825644f06 49 }
CheeseW 0:cb6825644f06 50 }
CheeseW 0:cb6825644f06 51 pc.printf("%d\n\r", count);
CheeseW 0:cb6825644f06 52 for (int j = 0; j < sigN; j++) {
CheeseW 0:cb6825644f06 53 if (pixyR.blocks[j].width &&pixyL.blocks[j].width)
CheeseW 0:cb6825644f06 54 {
CheeseW 0:cb6825644f06 55 pc.printf("s%d",j);
CheeseW 0:cb6825644f06 56 printPoint(pixyS.triangulation(Vec2D<int>(pixyL.blocks[j].x,pixyL.blocks[j].y), Vec2D<int>(pixyR.blocks[j].x,pixyR.blocks[j].y)));
CheeseW 0:cb6825644f06 57 }
CheeseW 0:cb6825644f06 58 }
CheeseW 0:cb6825644f06 59 }
CheeseW 0:cb6825644f06 60 }
CheeseW 0:cb6825644f06 61
CheeseW 0:cb6825644f06 62 }