public double calculate(TorqueTMP tmProfile, double currentPosition, double currentVelocity) { double voltageAdjustment = tunedVoltage / ds.getBatteryVoltage(); profile = tmProfile; setPoint = profile.getCurrentVelocity(); currentValue = currentVelocity; double output = 0.0; // Position P double error = profile.getCurrentPosition() - currentPosition; output += (error * kP * voltageAdjustment); // Velocity P double velocityError = profile.getCurrentVelocity() - currentVelocity; output += (velocityError * kV * voltageAdjustment); // Velocity FeedForward output += (profile.getCurrentVelocity() * kFFV * voltageAdjustment); // Acceleration FeedForward output += (profile.getCurrentAcceleration() * kFFA * voltageAdjustment); return output; }
public boolean isDone() { if ((Math.abs(profile.getCurrentPosition() - actualPosition) < positionDoneRange) && Math.abs(profile.getCurrentVelocity() - actualVelocity) < doneRange) { doneCyclesCount++; } else { doneCyclesCount = 0; } return (doneCyclesCount > minDoneCycles); }
public boolean onTrack() { return Math.abs(profile.getCurrentVelocity() - actualVelocity) < doneRange; }