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?
User | Revision | Line number | New 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 | } |