Skip to content

Commit

Permalink
merge staging into elevator
Browse files Browse the repository at this point in the history
  • Loading branch information
Brandon-Harad committed Feb 2, 2024
2 parents 1862263 + 48618e7 commit 9fe27ba
Showing 1 changed file with 3 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,15 +28,15 @@ public class ShooterPivotIOSim implements ShooterPivotIO {
private final ArmFeedforward feedforward = new ArmFeedforward(KS, KG, KV);

private boolean isClosedLoop;
private TrapezoidProfile.State setpoint;
private TrapezoidProfile.State goalState;

private double appliedVolts;

@Override
public void updateInputs(ShooterPivotIOInputs inputs) {
if (isClosedLoop) {
appliedVolts =
controller.calculate(Units.radiansToRotations(sim.getAngleRads()), setpoint)
controller.calculate(Units.radiansToRotations(sim.getAngleRads()), goalState)
+ feedforward.calculate(
Units.rotationsToRadians(controller.getSetpoint().position),
controller.getSetpoint().velocity);
Expand All @@ -58,7 +58,7 @@ public void setPosition(Rotation2d position) {
Units.radiansToRotations(sim.getVelocityRadPerSec())
);
isClosedLoop = true;
setpoint = new TrapezoidProfile.State(position.getRadians(), 0);
goalState = new TrapezoidProfile.State(position.getRadians(), 0);
}

@Override
Expand Down

0 comments on commit 9fe27ba

Please sign in to comment.