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?
User | Revision | Line number | New 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 | } |