Diff: Correction.cpp

Revision:

36:4a58639da6cf

Child:

40:bdd949fc3bc2

diff -r 50be685a3f54 -r 4a58639da6cf Correction.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Correction.cpp	Thu Dec 11 07:03:09 2014 +0000
@@ -0,0 +1,48 @@
+#include "Correction.h"
+
+Correction::Correction()
+{
+    count = 0;
+    initTime = 100;
+    alpha = 0.1;
+    correction = new Glove;
+    corrected = new Glove;
+    //Initialize correction values to 0
+    correction->roll = 0.0;
+    correction->pitch = 0.0;
+    correction->yaw = 0.0;
+    for (int i = 0; i < 5; i++)
+        correction->fingers[i] = 0.0;
+}
+
+Correction::~Correction()
+{
+    delete(correction);
+    delete(corrected);
+}
+
+Glove Correction::Correct(Glove gloveData)
+{
+    // Normalize sensor data for initTime
+    if (count < initTime) {
+        count++;
+        Train(gloveData);
+    }
+    else {
+        corrected->roll = alpha*(gloveData.roll - correction->roll) + (1.0-alpha)*corrected->roll;
+        corrected->pitch = alpha*(gloveData.pitch - correction->pitch) + (1.0-alpha)*corrected->pitch;
+        corrected->yaw = alpha*(gloveData.yaw - correction->yaw) + (1.0-alpha)*corrected->yaw;
+        for (int iter = 0; iter < 5; iter++)
+            corrected->fingers[iter] = alpha*(gloveData.fingers[iter] - correction->fingers[iter]) + (1.0-alpha)*corrected->fingers[iter];
+    }
+    return *corrected;
+}
+
+void Correction::Train(Glove gloveData)
+{
+    correction->roll = (correction->roll*count + gloveData.roll)/(count+1);
+    correction->pitch = (correction->pitch*count + gloveData.pitch)/(count+1);
+    correction->yaw = (correction->yaw*count + gloveData.yaw)/(count+1);
+    for (int iter = 0; iter < 5; iter++)
+        correction->fingers[iter] = (correction->fingers[iter]*count + gloveData.fingers[iter])/(count+1);
+}
\ No newline at end of file