blob: e03d22d735879589f08826a582a1e0cef3c9a536 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
|
#include <iostream>
#include <iomanip>
#include "wiimote.h"
#include "Vect3D.h"
#define _USE_MATH_DEFINES
#include <math.h>
#define USING_WIIYOURSELF
#include "C_3DPointSmoother.h"
#define SPACES " "
#define BLANKLINE " "
using namespace std;
int main (int argc, char **argv)
{
HANDLE console = GetStdHandle(STD_OUTPUT_HANDLE);
COORD origin = {0, 0};
Vect3D_t l_Dot[2];
C_3DPointSmoother l_smoother0, l_smoother1;
//l_smoother0.SetExponentialMovingAverage(0.2);
//l_smoother1.SetExponentialMovingAverage(0.2);
l_smoother0.SetSimpleMovingAverage(10);
l_smoother1.SetSimpleMovingAverage(10);
wiimote l_wm = wiimote();
l_wm.ChangedCallback = NULL; //no callbacks, we just poll...
l_wm.CallbackTriggerFlags = NO_CHANGE;
cout << "Connecting";
while (!l_wm.Connect())
{
cout << ".";
Sleep(500);
}
cout << "\nConnected!\n";
//enable ir
l_wm.SetReportType(wiimote::IN_BUTTONS_ACCEL_IR, true);
Sleep(100);
cout.fill(' ');
cout.setf(ios::left, ios::adjustfield);
do
{
l_wm.RefreshState();
SetConsoleCursorPosition(console, origin);
if (FindSensorBarDots(l_wm.IR, l_Dot))
{
l_Dot[0] = l_smoother0.Smooth(l_Dot[0]);
l_Dot[1] = l_smoother1.Smooth(l_Dot[1]);
cout << "Smoothed Dots:" << endl;
cout << setw(32) << l_Dot[0] ;
cout << setw(32) << l_Dot[1] << endl << endl;
cout << setw(32) << "Dot distance:" << endl;
Vect3D_t dif = l_Dot[0] - l_Dot[1];
double dist2 = dif.x*dif.x + dif.y*dif.y + dif.z*dif.z;
double dist = sqrt(dist2);
cout << setw(32) << dist << endl << endl;
double radPerPixel = ((M_PI / 180)*40.0)/1016.0,
angle = dist * radPerPixel,
z = (0.5 * 205.0) / tan(0.5 * angle);
cout << setw(32) << "Relative coordinates:" << endl;
cout << "x (px): " << setw(10) << fixed << setprecision(2)
<< (l_Dot[0].x + l_Dot[1].x)/2.0 << endl;
cout << "y (px): " << setw(10) << fixed << setprecision(2)
<< (l_Dot[0].y + l_Dot[1].y)/2.0 << endl;
cout << "z (mm): " << setw(10) << fixed << setprecision(2) << z << endl;
}
else
{
cout << "no dots...";
for (int i=0 ; i<20; i++)
cout << BLANKLINE << endl;
}
Sleep(10);
}
while (!l_wm.Button.Home());//!l_wm.Button.Home());
cout << "Exiting...\n";
//l_wm.Disconnect();
return 0;
}
|
