Example #1
0
  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;
  }
Example #2
0
  public boolean isDone() {
    if ((Math.abs(profile.getCurrentPosition() - actualPosition) < positionDoneRange)
        && Math.abs(profile.getCurrentVelocity() - actualVelocity) < doneRange) {
      doneCyclesCount++;
    } else {
      doneCyclesCount = 0;
    }

    return (doneCyclesCount > minDoneCycles);
  }
Example #3
0
 public boolean onTrack() {
   return Math.abs(profile.getCurrentVelocity() - actualVelocity) < doneRange;
 }