Correction.cpp

00001 #include "Correction.h"
00002 
00003 Correction::Correction()
00004 {
00005     count = 0;
00006     initTime = 100;
00007     alpha = 0.25;
00008     //correction = new Glove;
00009     //corrected = new Glove;
00010     //Initialize correction values to 0
00011     correction.roll = 0.0;
00012     correction.pitch = 0.0;
00013     correction.yaw = 0.0;
00014     for (int i = 0; i < 5; i++)
00015         correction.fingers[i] = 0;
00016     corrected.roll = 0.0;
00017     corrected.pitch = 0.0;
00018     corrected.yaw = 0.0;
00019     for (int i = 0; i < 5; i++)
00020         corrected.fingers[i] = 0;
00021 }
00022 
00023 Correction::~Correction()
00024 {
00025     //delete(correction);
00026     //delete(corrected);
00027 }
00028 
00029 Glove Correction::Correct(Glove gloveData)
00030 {
00031     // Normalize sensor data for initTime
00032     if (count < initTime) {
00033         count++;
00034         Train(gloveData);
00035     }
00036     else if (count < 2*initTime) {
00037         count++;
00038     }
00039     else {
00040         corrected.roll = alpha*(gloveData.roll - correction.roll) + (1.0-alpha)*corrected.roll;
00041         corrected.pitch = alpha*(gloveData.pitch - correction.pitch) + (1.0-alpha)*corrected.pitch;
00042         corrected.yaw = alpha*(gloveData.yaw - correction.yaw) + (1.0-alpha)*corrected.yaw;
00043         for (int iter = 0; iter < 5; iter++)
00044             corrected.fingers[iter] = alpha*(gloveData.fingers[iter] - correction.fingers[iter]) + (1.0-alpha)*corrected.fingers[iter];
00045     }
00046     return corrected;
00047 }
00048 
00049 void Correction::Train(Glove gloveData)
00050 {
00051     correction.roll = (correction.roll*count + gloveData.roll)/(count+1);
00052     correction.pitch = (correction.pitch*count + gloveData.pitch)/(count+1);
00053     correction.yaw = (correction.yaw*count + gloveData.yaw)/(count+1);
00054     for (int iter = 0; iter < 5; iter++)
00055         correction.fingers[iter] = (correction.fingers[iter]*count + gloveData.fingers[iter])/(count+1);
00056 }