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@49:361833355173, 2014-12-11 (annotated)

Committer:
robertbui
Date:
Thu Dec 11 17:49:21 2014 +0000
Revision:
49:361833355173
Parent:
48:2ba6321d79fc
Child:
52:0f7a239cdd09
Gesture parameters update.

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;
robertbui 49:361833355173 7 alpha = 0.25;
robertbui 41:367cab0162de 8 //correction = new Glove;
robertbui 41:367cab0162de 9 //corrected = new Glove;
naren 36:4a58639da6cf 10 //Initialize correction values to 0
robertbui 41:367cab0162de 11 correction.roll = 0.0;
robertbui 41:367cab0162de 12 correction.pitch = 0.0;
robertbui 41:367cab0162de 13 correction.yaw = 0.0;
naren 36:4a58639da6cf 14 for (int i = 0; i < 5; i++)
robertbui 41:367cab0162de 15 correction.fingers[i] = 0.0;
robertbui 41:367cab0162de 16 corrected.roll = 0.0;
robertbui 41:367cab0162de 17 corrected.pitch = 0.0;
robertbui 41:367cab0162de 18 corrected.yaw = 0.0;
robertbui 40:bdd949fc3bc2 19 for (int i = 0; i < 5; i++)
robertbui 41:367cab0162de 20 corrected.fingers[i] = 0.0;
naren 36:4a58639da6cf 21 }
naren 36:4a58639da6cf 22
naren 36:4a58639da6cf 23 Correction::~Correction()
naren 36:4a58639da6cf 24 {
robertbui 41:367cab0162de 25 //delete(correction);
robertbui 41:367cab0162de 26 //delete(corrected);
naren 36:4a58639da6cf 27 }
naren 36:4a58639da6cf 28
naren 36:4a58639da6cf 29 Glove Correction::Correct(Glove gloveData)
naren 36:4a58639da6cf 30 {
naren 36:4a58639da6cf 31 // Normalize sensor data for initTime
naren 36:4a58639da6cf 32 if (count < initTime) {
naren 36:4a58639da6cf 33 count++;
naren 36:4a58639da6cf 34 Train(gloveData);
naren 36:4a58639da6cf 35 }
robertbui 49:361833355173 36 else if (count < 2*initTime) {
robertbui 49:361833355173 37 count++;
robertbui 49:361833355173 38 }
naren 36:4a58639da6cf 39 else {
robertbui 41:367cab0162de 40 corrected.roll = alpha*(gloveData.roll - correction.roll) + (1.0-alpha)*corrected.roll;
robertbui 41:367cab0162de 41 corrected.pitch = alpha*(gloveData.pitch - correction.pitch) + (1.0-alpha)*corrected.pitch;
robertbui 41:367cab0162de 42 corrected.yaw = alpha*(gloveData.yaw - correction.yaw) + (1.0-alpha)*corrected.yaw;
naren 36:4a58639da6cf 43 for (int iter = 0; iter < 5; iter++)
robertbui 41:367cab0162de 44 corrected.fingers[iter] = alpha*(gloveData.fingers[iter] - correction.fingers[iter]) + (1.0-alpha)*corrected.fingers[iter];
naren 36:4a58639da6cf 45 }
robertbui 41:367cab0162de 46 return corrected;
naren 36:4a58639da6cf 47 }
naren 36:4a58639da6cf 48
naren 36:4a58639da6cf 49 void Correction::Train(Glove gloveData)
naren 36:4a58639da6cf 50 {
robertbui 41:367cab0162de 51 correction.roll = (correction.roll*count + gloveData.roll)/(count+1);
robertbui 41:367cab0162de 52 correction.pitch = (correction.pitch*count + gloveData.pitch)/(count+1);
robertbui 41:367cab0162de 53 correction.yaw = (correction.yaw*count + gloveData.yaw)/(count+1);
naren 36:4a58639da6cf 54 for (int iter = 0; iter < 5; iter++)
robertbui 41:367cab0162de 55 correction.fingers[iter] = (correction.fingers[iter]*count + gloveData.fingers[iter])/(count+1);
naren 36:4a58639da6cf 56 }