File content as of revision 48:2ba6321d79fc:

#include "Correction.h"

Correction::Correction()
{
    count = 0;
    initTime = 100;
    alpha = 0.4;
    //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;
    corrected.roll = 0.0;
    corrected.pitch = 0.0;
    corrected.yaw = 0.0;
    for (int i = 0; i < 5; i++)
        corrected.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);
}