Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
Correction.cpp
- Committer:
- naren
- Date:
- 2014-12-11
- Revision:
- 36:4a58639da6cf
- Child:
- 40:bdd949fc3bc2
File content as of revision 36:4a58639da6cf:
#include "Correction.h" Correction::Correction() { count = 0; initTime = 100; alpha = 0.1; correction = new Glove; corrected = new Glove; //Initialize correction values to 0 correction->roll = 0.0; correction->pitch = 0.0; correction->yaw = 0.0; for (int i = 0; i < 5; i++) correction->fingers[i] = 0.0; } Correction::~Correction() { delete(correction); delete(corrected); } Glove Correction::Correct(Glove gloveData) { // Normalize sensor data for initTime if (count < initTime) { count++; Train(gloveData); } else { corrected->roll = alpha*(gloveData.roll - correction->roll) + (1.0-alpha)*corrected->roll; corrected->pitch = alpha*(gloveData.pitch - correction->pitch) + (1.0-alpha)*corrected->pitch; corrected->yaw = alpha*(gloveData.yaw - correction->yaw) + (1.0-alpha)*corrected->yaw; for (int iter = 0; iter < 5; iter++) corrected->fingers[iter] = alpha*(gloveData.fingers[iter] - correction->fingers[iter]) + (1.0-alpha)*corrected->fingers[iter]; } return *corrected; } void Correction::Train(Glove gloveData) { correction->roll = (correction->roll*count + gloveData.roll)/(count+1); correction->pitch = (correction->pitch*count + gloveData.pitch)/(count+1); correction->yaw = (correction->yaw*count + gloveData.yaw)/(count+1); for (int iter = 0; iter < 5; iter++) correction->fingers[iter] = (correction->fingers[iter]*count + gloveData.fingers[iter])/(count+1); }