Model-Based Team / Mbed 2 deprecated CubicHand

Cubic Hand project for EECS 249A course.

Dependencies:   MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811

Correction.cpp@36:4a58639da6cf, 2014-12-11 (annotated)

Committer:
naren
Date:
Thu Dec 11 07:03:09 2014 +0000
Revision:
36:4a58639da6cf
Child:
40:bdd949fc3bc2
Created Correction class; ; This corrects and filters the sensor data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
naren 36:4a58639da6cf 1 #include "Correction.h"
naren 36:4a58639da6cf 2
naren 36:4a58639da6cf 3 Correction::Correction()
naren 36:4a58639da6cf 4 {
naren 36:4a58639da6cf 5 count = 0;
naren 36:4a58639da6cf 6 initTime = 100;
naren 36:4a58639da6cf 7 alpha = 0.1;
naren 36:4a58639da6cf 8 correction = new Glove;
naren 36:4a58639da6cf 9 corrected = new Glove;
naren 36:4a58639da6cf 10 //Initialize correction values to 0
naren 36:4a58639da6cf 11 correction->roll = 0.0;
naren 36:4a58639da6cf 12 correction->pitch = 0.0;
naren 36:4a58639da6cf 13 correction->yaw = 0.0;
naren 36:4a58639da6cf 14 for (int i = 0; i < 5; i++)
naren 36:4a58639da6cf 15 correction->fingers[i] = 0.0;
naren 36:4a58639da6cf 16 }
naren 36:4a58639da6cf 17
naren 36:4a58639da6cf 18 Correction::~Correction()
naren 36:4a58639da6cf 19 {
naren 36:4a58639da6cf 20 delete(correction);
naren 36:4a58639da6cf 21 delete(corrected);
naren 36:4a58639da6cf 22 }
naren 36:4a58639da6cf 23
naren 36:4a58639da6cf 24 Glove Correction::Correct(Glove gloveData)
naren 36:4a58639da6cf 25 {
naren 36:4a58639da6cf 26 // Normalize sensor data for initTime
naren 36:4a58639da6cf 27 if (count < initTime) {
naren 36:4a58639da6cf 28 count++;
naren 36:4a58639da6cf 29 Train(gloveData);
naren 36:4a58639da6cf 30 }
naren 36:4a58639da6cf 31 else {
naren 36:4a58639da6cf 32 corrected->roll = alpha*(gloveData.roll - correction->roll) + (1.0-alpha)*corrected->roll;
naren 36:4a58639da6cf 33 corrected->pitch = alpha*(gloveData.pitch - correction->pitch) + (1.0-alpha)*corrected->pitch;
naren 36:4a58639da6cf 34 corrected->yaw = alpha*(gloveData.yaw - correction->yaw) + (1.0-alpha)*corrected->yaw;
naren 36:4a58639da6cf 35 for (int iter = 0; iter < 5; iter++)
naren 36:4a58639da6cf 36 corrected->fingers[iter] = alpha*(gloveData.fingers[iter] - correction->fingers[iter]) + (1.0-alpha)*corrected->fingers[iter];
naren 36:4a58639da6cf 37 }
naren 36:4a58639da6cf 38 return *corrected;
naren 36:4a58639da6cf 39 }
naren 36:4a58639da6cf 40
naren 36:4a58639da6cf 41 void Correction::Train(Glove gloveData)
naren 36:4a58639da6cf 42 {
naren 36:4a58639da6cf 43 correction->roll = (correction->roll*count + gloveData.roll)/(count+1);
naren 36:4a58639da6cf 44 correction->pitch = (correction->pitch*count + gloveData.pitch)/(count+1);
naren 36:4a58639da6cf 45 correction->yaw = (correction->yaw*count + gloveData.yaw)/(count+1);
naren 36:4a58639da6cf 46 for (int iter = 0; iter < 5; iter++)
naren 36:4a58639da6cf 47 correction->fingers[iter] = (correction->fingers[iter]*count + gloveData.fingers[iter])/(count+1);
naren 36:4a58639da6cf 48 }