Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
Correction.cpp
- Committer:
- joseoyola
- Date:
- 2014-12-15
- Revision:
- 56:f95ec9baa4cb
- Parent:
- 52:0f7a239cdd09
File content as of revision 56:f95ec9baa4cb:
#include "Correction.h" Correction::Correction() { count = 0; initTime = 100; alpha = 0.25; //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; corrected.roll = 0.0; corrected.pitch = 0.0; corrected.yaw = 0.0; for (int i = 0; i < 5; i++) corrected.fingers[i] = 0; } Correction::~Correction() { //delete(correction); //delete(corrected); } Glove Correction::Correct(Glove gloveData) { // Normalize sensor data for initTime if (count < initTime) { count++; Train(gloveData); } else if (count < 2*initTime) { count++; } 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); }