void updateState() {
for (int i = 0; i < NUM_FINGERS; ++i) {
if (finger[i].reachedPos()) {
states[i] = kIdleState;
} else if (states[i] == kWaitMoveState && fabs(avgSpeed(samples[i], SAMPLE_SIZE)) > 0.8) {
states[i] = kMovingState;
errors[i] = 0;
} else if (states[i] == kMovingState && fabs(avgSpeed(samples[i], SAMPLE_SIZE)) < 0.6) {
states[i] = kErrorState;
} else if (states[i] == kErrorState) {
errors[i]++;
if (errors[i] > 5) {
finger[i].writePos(finger[i].readPos() - avgSpeed(samples[i], SAMPLE_SIZE) / 2);
}
}
}
}
void updateSpeeds(void) {
for(unsigned char i = 0; i < 4; ++i) {
Wheels[i]->Buffer[Wheels[i]->Time] = Wheels[i]->Counter;
Wheels[i]->Counter = 0;
if(Wheels[i]->Time == (BUFSIZE -1)) {
Wheels[i]->Speed = avgSpeed(Wheels[i]);
Wheels[i]->Acceleration = updateAcc(Wheels[i]);
Wheels[i]->Time = 0;
}
else {
Wheels[i]->Time++;
}
}
}