Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
Correction.cpp@48:2ba6321d79fc, 2014-12-11 (annotated)
- Committer:
- robertbui
- Date:
- Thu Dec 11 14:06:34 2014 +0000
- Revision:
- 48:2ba6321d79fc
- Parent:
- 44:9636cea17b3b
- Child:
- 49:361833355173
Updated alpha filter values and change position of cube conditions.
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 | 48:2ba6321d79fc | 7 | alpha = 0.4; |
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 | } |
naren | 36:4a58639da6cf | 36 | else { |
robertbui | 41:367cab0162de | 37 | corrected.roll = alpha*(gloveData.roll - correction.roll) + (1.0-alpha)*corrected.roll; |
robertbui | 41:367cab0162de | 38 | corrected.pitch = alpha*(gloveData.pitch - correction.pitch) + (1.0-alpha)*corrected.pitch; |
robertbui | 41:367cab0162de | 39 | corrected.yaw = alpha*(gloveData.yaw - correction.yaw) + (1.0-alpha)*corrected.yaw; |
naren | 36:4a58639da6cf | 40 | for (int iter = 0; iter < 5; iter++) |
robertbui | 41:367cab0162de | 41 | corrected.fingers[iter] = alpha*(gloveData.fingers[iter] - correction.fingers[iter]) + (1.0-alpha)*corrected.fingers[iter]; |
naren | 36:4a58639da6cf | 42 | } |
robertbui | 41:367cab0162de | 43 | return corrected; |
naren | 36:4a58639da6cf | 44 | } |
naren | 36:4a58639da6cf | 45 | |
naren | 36:4a58639da6cf | 46 | void Correction::Train(Glove gloveData) |
naren | 36:4a58639da6cf | 47 | { |
robertbui | 41:367cab0162de | 48 | correction.roll = (correction.roll*count + gloveData.roll)/(count+1); |
robertbui | 41:367cab0162de | 49 | correction.pitch = (correction.pitch*count + gloveData.pitch)/(count+1); |
robertbui | 41:367cab0162de | 50 | correction.yaw = (correction.yaw*count + gloveData.yaw)/(count+1); |
naren | 36:4a58639da6cf | 51 | for (int iter = 0; iter < 5; iter++) |
robertbui | 41:367cab0162de | 52 | correction.fingers[iter] = (correction.fingers[iter]*count + gloveData.fingers[iter])/(count+1); |
naren | 36:4a58639da6cf | 53 | } |