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);
      }
    }
  }
}
Esempio n. 2
0
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++;
        }
        
    }
}