double beforeValue = processed.getDoubleValue(); processed.update(); double afterValue = processed.getDoubleValue(); assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertTrue(Math.abs(processed.getDoubleValue()) <= amplitude || isValueWithinMarginOfError(amplitude, processed.getDoubleValue(), permissibleErrorRatio)); assertTrue(Math.abs(processed.getSmoothedRate().getDoubleValue()) <= maxRate); assertTrue(Math.abs(processed.getSmoothedAcceleration().getDoubleValue()) <= maxAcceleration);
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clipToMinMax(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clipToMinMax(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }
assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertTrue(Math.abs(processed.getDoubleValue()) <= amplitude); assertTrue(Math.abs(processed.getSmoothedRate().getDoubleValue()) <= maxRate); assertTrue(Math.abs(processed.getSmoothedAcceleration().getDoubleValue()) <= maxAcceleration);
assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertTrue(Math.abs(processed.getDoubleValue()) <= amplitude); assertTrue(Math.abs(processed.getSmoothedRate().getDoubleValue()) <= maxRate); assertTrue(Math.abs(processed.getSmoothedAcceleration().getDoubleValue()) <= maxAcceleration);
assertEquals(3.0, processed.getDoubleValue(), EPSILON); assertTrue(processed.hasBeenInitialized()); assertEquals(0.0, processed.getSmoothedRate().getDoubleValue(), EPSILON);