1 files changed, 103 insertions, 0 deletions
diff --git a/wiimote_ir_smoothing/main.cpp b/wiimote_ir_smoothing/main.cpp
new file mode 100644
index 0000000..e03d22d
--- /dev/null
+++ b/wiimote_ir_smoothing/main.cpp
@@ -0,0 +1,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;
+}
+\ No newline at end of file