diff --git a/src/main/java/com/team766/controllers/PIDRunner.java b/src/main/java/com/team766/controllers/PIDRunner.java new file mode 100644 index 000000000..74efff25c --- /dev/null +++ b/src/main/java/com/team766/controllers/PIDRunner.java @@ -0,0 +1,113 @@ +package com.team766.controllers; + +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.library.ValueProvider; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.function.Supplier; + +/** + * Helper class that can set a setpoint in a PID loop, eg within {@link Mechanism.run}. + * + * Uses functional interfaces for getting the setpoint, output, PID slot, and feedforward. + * Can use with lambdas and/or method references, eg: + * + * PIDRunner pidRunner = new PIDRunner("SHOULDER", leftMotor, ControlMode.Position, + * this::getTargetRotations, this::getRotations); + */ +public class PIDRunner { + + public static final Supplier DEFAULT_SLOT_PICKER = () -> 0; + public static final Supplier NO_ARBITRARY_FEED_FORWARD = () -> 0.0; + + /** + * Returns a fixed arbitrary FeedForward supplier, simply returning the latest input ffGain from a config file. + * @param arbitraryFFGain Input FeedForward Gain, read from a config file. + * @return Fixed FeedForward Gain from the current value of ffGain. + */ + public static final Supplier fixedArbitraryFeedForward( + ValueProvider arbitraryFFGain) { + return () -> arbitraryFFGain.valueOr(0.0); + } + + /** + * Returns a FeedForward supplier that gets the latest input ffGain from a config file and returns a proportional + * FeedForward based on the cosine of the supplied angle. Useful for arm type mechanisms, where we want to + * counteract gravity proportionally to the arm's current angle, where 0 is parallel to the ground and 90 + * is perpendicular & up. + * + * @param arbitraryFFGain Input FeedForward Gain, read from a config file. + * @param angle Current angle of the mechanism. 0 is parallel to the ground, 90 is perpendicular & up. + * @return Proportional FeedForward Gain based on the angle. + */ + public static final Supplier cosineArbitraryFeedForward( + ValueProvider arbitraryFFGain, Supplier angle) { + return () -> arbitraryFFGain.valueOr(0.0) * Math.cos(Math.toRadians(angle.get())); + } + + private final String label; + private final MotorController motor; + private final MotorController.ControlMode mode; + private final Supplier setPoint; + private final Supplier output; + private final Supplier slot; + private final Supplier arbitraryFeedForward; + private boolean first = true; + private double prevSetPoint = 0.0; + private double prevArbitraryFeedForward = 0.0; + private int prevSlot = 0; + + public PIDRunner( + String label, + MotorController motor, + MotorController.ControlMode mode, + Supplier setPoint, + Supplier output) { + this(label, motor, mode, setPoint, output, DEFAULT_SLOT_PICKER, NO_ARBITRARY_FEED_FORWARD); + } + + public PIDRunner( + String label, + MotorController motor, + MotorController.ControlMode mode, + Supplier setPoint, + Supplier output, + Supplier slot, + Supplier arbitraryFeedForward) { + this.label = label; + this.motor = motor; + this.mode = mode; + this.setPoint = setPoint; + this.output = output; + this.slot = slot; + this.arbitraryFeedForward = arbitraryFeedForward; + } + + public void run() { + double currentSetPoint = setPoint.get(); + double currentOutput = output.get(); + double currentArbitraryFeedForward = arbitraryFeedForward.get(); + int currentSlot = slot.get(); + + // if we haven't set the setpoint yet, or if the setpoint, feedforward, or slot have + // changed, set the setpoint. + if (first + || prevSetPoint != currentSetPoint + || prevArbitraryFeedForward != currentArbitraryFeedForward + || currentSlot != prevSlot) { + first = false; + prevSetPoint = currentSetPoint; + prevArbitraryFeedForward = currentArbitraryFeedForward; + prevSlot = currentSlot; + + // log to SmartDashboard - useful for PID tuning. + SmartDashboard.putNumber(label + " setpoint", currentSetPoint); + SmartDashboard.putNumber(label + " output", currentOutput); + SmartDashboard.putNumber(label + " PID slot", currentSlot); + + // FIXME: switch to supporting slot, feedForward once that PR is integrated + // motor.set(mode, currentSetPoint, slot.get(), arbitraryFeedForward.get()); + motor.set(mode, currentSetPoint); + } + } +} diff --git a/src/main/java/com/team766/robot/reva/constants/ConfigConstants.java b/src/main/java/com/team766/robot/reva/constants/ConfigConstants.java index c16c1a949..b8c1bf419 100644 --- a/src/main/java/com/team766/robot/reva/constants/ConfigConstants.java +++ b/src/main/java/com/team766/robot/reva/constants/ConfigConstants.java @@ -10,6 +10,8 @@ private ConfigConstants() {} public static final String SHOULDER_RIGHT = "shoulder.rightMotor"; public static final String SHOULDER_LEFT = "shoulder.leftMotor"; + // TODO: change this to shoulder.arbitraryFFGain + public static final String SHOULDER_ARBITRARY_FFGAIN = "shoulder.leftMotor.ffGain"; // intake config values public static final String INTAKE_MOTOR = "intake.motor"; diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index f5941e036..8721e1496 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -1,12 +1,12 @@ package com.team766.robot.reva.mechanisms; +import static com.team766.robot.reva.constants.ConfigConstants.SHOULDER_ARBITRARY_FFGAIN; import static com.team766.robot.reva.constants.ConfigConstants.SHOULDER_LEFT; import static com.team766.robot.reva.constants.ConfigConstants.SHOULDER_RIGHT; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; import com.team766.config.ConfigFileReader; +import com.team766.controllers.PIDRunner; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; @@ -36,10 +36,11 @@ public double getAngle() { private static final double NUDGE_AMOUNT = 30; // degrees - private MotorController leftMotor; - private MotorController rightMotor; + private final MotorController leftMotor; + private final MotorController rightMotor; + private final PIDRunner pidRunner; - private ValueProvider ffGain; + private ValueProvider arbitraryFFGain; private double targetRotations = 0.0; public Shoulder() { @@ -48,8 +49,18 @@ public Shoulder() { rightMotor = RobotProvider.instance.getMotor(SHOULDER_RIGHT); rightMotor.follow(leftMotor); leftMotor.setNeutralMode(NeutralMode.Brake); - ffGain = ConfigFileReader.getInstance().getDouble("shoulder.leftMotor.ffGain"); + arbitraryFFGain = ConfigFileReader.getInstance().getDouble(SHOULDER_ARBITRARY_FFGAIN); leftMotor.setSensorPosition(0); + + pidRunner = + new PIDRunner( + "SHOULDER", + leftMotor, + MotorController.ControlMode.Position, + this::getTargetRotations, + this::getRotations, + PIDRunner.DEFAULT_SLOT_PICKER, + PIDRunner.cosineArbitraryFeedForward(arbitraryFFGain, this::getAngle)); } public void stop() { @@ -73,6 +84,10 @@ public double getRotations() { return leftMotor.getSensorPosition(); } + public double getTargetRotations() { + return targetRotations; + } + public double getAngle() { return rotationsToDegrees(leftMotor.getSensorPosition()); } @@ -103,17 +118,10 @@ public void rotate(double angle) { @Override public void run() { - SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); - SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations()); - SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations); - - TalonFX leftTalon = (TalonFX) leftMotor; - SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get()); - double ff = ffGain.valueOr(0.0) * Math.cos(Math.toRadians(getAngle())); - SmartDashboard.putNumber("[SHOULDER] FF", ff); + pidRunner.run(); + // also log velocity, for PID tuning + // TODO: consider moving this into PIDRunner SmartDashboard.putNumber("[SHOULDER VELOCITY]", Math.abs(leftMotor.getSensorVelocity())); - PositionDutyCycle positionRequest = new PositionDutyCycle(targetRotations); - positionRequest.FeedForward = ff; - leftTalon.setControl(positionRequest); + SmartDashboard.putNumber("[SHOULDER ANGLE]", getAngle()); } }