private Direction computeDirectionOfMotion(double currentPosition) {
Direction directionOfMotion;
if (currentPosition > finiteDifferenceVelocity.getPreviousPosition()) {
directionOfMotion = Direction.FORWARD;
} else if (currentPosition < finiteDifferenceVelocity.getPreviousPosition()) {
directionOfMotion = Direction.BACKWARD;
} else {
directionOfMotion = lastPosChangeDirection.getEnumValue();
}
return directionOfMotion;
}
public void update(double currentPosition) {
if (!updateHasBeenCalled) {
updateHasBeenCalled = true;
lastPosChangeDirection.set(Direction.NONE);
}
timeSinceLastPosChange.set(
time.getDoubleValue() - finiteDifferenceVelocity.getPreviousTimestamp());
double previousPositon = finiteDifferenceVelocity.getPreviousPosition();
velocityIfEncoderTicksNow.set(
(positionPredicted.getDoubleValue() - previousPositon)
/ timeSinceLastPosChange.getDoubleValue());
if (currentPosition != previousPositon) {
this.finiteDifferenceVelocity.update();
this.finiteDifferenceAccel.update();
if (determineIfDirectionChanged(currentPosition)) {
unfilteredVelocity.set(0.0);
positionPredicted.set(currentPosition - lastPositionIncrement.getDoubleValue());
} else {
unfilteredVelocity.set(finiteDifferenceVelocity.getDoubleValue());
positionPredicted.set(currentPosition + lastPositionIncrement.getDoubleValue());
}
lastPosChangeTimeInterval.set(timeSinceLastPosChange.getDoubleValue());
lastPositionIncrement.set(currentPosition - previousPositon);
} else {
velocityIfEncoderTicksNowConstantAccel.set(
finiteDifferenceVelocity.getDoubleValue()
+ finiteDifferenceAccel.getDoubleValue() * timeSinceLastPosChange.getDoubleValue());
boolean velSlowedDownSincePosTakingTooLongToChange =
timeSinceLastPosChange.getDoubleValue() > lastPosChangeTimeInterval.getDoubleValue();
if (velSlowedDownSincePosTakingTooLongToChange && useDecay.getBooleanValue()) {
this.unfilteredVelocity.set(velocityIfEncoderTicksNow.getDoubleValue());
}
}
/** ************ SET VALUE ************************ */
this.set(alphaFilter(velocityIfEncoderTicksNowConstantAccel.getDoubleValue()));
}