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@56:f95ec9baa4cb, 2014-12-15 (annotated)

Committer:
joseoyola
Date:
Mon Dec 15 23:55:15 2014 +0000
Revision:
56:f95ec9baa4cb
Parent:
52:0f7a239cdd09 
Changes to LedCube.h and LedCube.cpp; ; Renamed ledCube.h and ledCube.cpp to LedCube.h and LedCube.cpp, added documentation to both, added logic to move cube to allow changing size regardless of current position.

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++)
naren 52:0f7a239cdd09 15 correction.fingers[i] = 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++)
naren 52:0f7a239cdd09 20 corrected.fingers[i] = 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 }