diff --git a/.vscode/settings.json b/.vscode/settings.json index cdcae9a..21d7841 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -27,5 +27,6 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", - "java.debug.settings.onBuildFailureProceed": true + "java.debug.settings.onBuildFailureProceed": true, + "wpilib.skipTests": true } diff --git a/simgui.json b/simgui.json index 721b509..fe5ac4f 100644 --- a/simgui.json +++ b/simgui.json @@ -51,6 +51,9 @@ }, "transitory": { "Shuffleboard": { + ".recording": { + "open": true + }, "Arm": { "Feedforward": { "open": true @@ -58,6 +61,9 @@ "Position": { "open": true }, + "Shoulder": { + "open": true + }, "Velocities": { "open": true } @@ -99,7 +105,7 @@ "Measurement": { "open": true }, - "Setpoint": { + "State": { "open": true }, "open": true, @@ -214,7 +220,10 @@ } ] } - ] + ], + "window": { + "visible": false + } } } } diff --git a/src/main/java/frc/lib/AccelerationCalculator.java b/src/main/java/frc/lib/AccelerationCalculator.java deleted file mode 100644 index 2a74b0c..0000000 --- a/src/main/java/frc/lib/AccelerationCalculator.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.lib; - -import edu.wpi.first.wpilibj.Timer; - -public class AccelerationCalculator { - - private double previousVelocity; - private double previousTime; - - public double calculate(double velocity) { - double time = Timer.getFPGATimestamp(); - - double acceleration = (velocity - previousVelocity) / (time - previousTime); - - previousVelocity = velocity; - previousTime = time; - - return acceleration; - } -} diff --git a/src/main/java/frc/lib/DoubleJointedArmFeedforward.java b/src/main/java/frc/lib/DoubleJointedArmFeedforward.java deleted file mode 100644 index 340296a..0000000 --- a/src/main/java/frc/lib/DoubleJointedArmFeedforward.java +++ /dev/null @@ -1,154 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.ejml.data.MatrixType; -import org.ejml.simple.SimpleMatrix; - -public class DoubleJointedArmFeedforward { - - private final double m1; - private final double m2; - private final double l1; - private final double r1; - private final double r2; - private final double I1; - private final double I2; - private final double g1; - private final double g2; - - private final SimpleMatrix M_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - private final SimpleMatrix C_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - private final SimpleMatrix Tg_VECTOR = new SimpleMatrix(2, 1, MatrixType.DDRM); - private final SimpleMatrix Kb_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - private final SimpleMatrix B_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - - public DoubleJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) { - this.m1 = shoulder.massKg(); - this.m2 = elbow.massKg(); - this.l1 = shoulder.lengthMeters(); - this.r1 = shoulder.radiusMeters(); - this.r2 = elbow.radiusMeters(); - this.I1 = shoulder.moiKgMetersSquared(); - this.I2 = elbow.moiKgMetersSquared(); - this.g1 = shoulder.gearing(); - this.g2 = elbow.gearing(); - - B_MATRIX.set(0, 0, shoulder.torqueNm()); - B_MATRIX.set(1, 1, elbow.torqueNm()); - B_MATRIX.set(1, 0, 0); - B_MATRIX.set(0, 1, 0); - - Kb_MATRIX.set(0, 0, shoulder.torqueLossNm()); - Kb_MATRIX.set(1, 1, elbow.torqueLossNm()); - Kb_MATRIX.set(1, 0, 0); - Kb_MATRIX.set(0, 1, 0); - } - - /** - * Calculates the M matrix. - * - * @param elbowPosition the position of the elbow joint. - */ - public void updateMMatrix(Rotation2d elbowPosition) { - double c2 = elbowPosition.getCos(); - double diagonal = m2 * (r2 * r2 + l1 * r2 * c2) + I2; - - M_MATRIX.set(0, 0, m1 * r1 * r1 + m2 * (l1 * l1 + r2 * r2 + 2 * l1 * r2 * c2) + I1 + I2); - M_MATRIX.set(0, 1, diagonal); - M_MATRIX.set(1, 0, diagonal); - M_MATRIX.set(1, 1, m2 * r2 * r2 + I2); - } - - /** - * Calculates the C matrix. - * - * @param elbowPosition the position of the elbow joint. - * @param shoulderVelocity the velocity of the shoulder joint. - * @param elbowVelocity the velocity of the elbow joint. - */ - public void updateCMatrix( - Rotation2d elbowPosition, Rotation2d shoulderVelocity, Rotation2d elbowVelocity) { - double s2 = elbowPosition.getSin(); - - C_MATRIX.set(0, 0, -m2 * l1 * r2 * s2 * elbowVelocity.getRadians()); - C_MATRIX.set( - 0, 1, -m2 * l1 * r2 * s2 * (shoulderVelocity.getRadians() + elbowVelocity.getRadians())); - C_MATRIX.set(1, 0, m2 * l1 * r2 * s2 * shoulderVelocity.getRadians()); - C_MATRIX.set(1, 1, 0); - } - - /** - * Calculates the Tg vector. - * - * @param shoulderPosition the position of the shoulder joint. - * @param elbowPosition the position of the elbow joint. - */ - public void updateTgVector(Rotation2d shoulderPosition, Rotation2d elbowPosition) { - double c1 = shoulderPosition.getCos(); - double c12 = shoulderPosition.plus(elbowPosition).getCos(); - - Tg_VECTOR.set(0, 0, (m1 * r1 + m2 * l1) * g1 * c1 + m2 * r2 * g2 * c12); - Tg_VECTOR.set(1, 0, m2 * r2 * g2 * c12); - } - - /** - * Calculates the voltage feedforward required to move the arm in a certain state. - * - * @param shoulderPosition the angle of the shoulder joint. - * @param elbowPosition the angle of the elbow joint. - * @param shoulderVelocity the angular velocity of the shoulder joint. - * @param elbowVelocity the angular velocity of the elbow joint. - * @param shoulderAcceleration the angular acceleration of the shoulder joint. - * @param elbowAcceleration the angular acceleration of the elbow joint. - * @return the voltage feedforward required to move the arm in the given state. - */ - public TwoJointedArmFeedforwardResult calculateFeedforward( - Rotation2d shoulderPosition, - Rotation2d elbowPosition, - Rotation2d shoulderVelocity, - Rotation2d elbowVelocity, - Rotation2d shoulderAcceleration, - Rotation2d elbowAcceleration) { - updateMMatrix(elbowPosition); - updateCMatrix(elbowPosition, shoulderVelocity, elbowVelocity); - updateTgVector(shoulderPosition, elbowPosition); - - var thetaDotVector = new SimpleMatrix(2, 1, MatrixType.DDRM); - thetaDotVector.set(0, 0, shoulderVelocity.getRadians()); - thetaDotVector.set(1, 0, elbowVelocity.getRadians()); - - var thetaDotDotVector = new SimpleMatrix(2, 1, MatrixType.DDRM); - thetaDotDotVector.set(0, 0, shoulderAcceleration.getRadians()); - thetaDotDotVector.set(1, 0, elbowAcceleration.getRadians()); - - var M = M_MATRIX.mult(thetaDotDotVector); - var C = C_MATRIX.mult(thetaDotVector); - var Kb = Kb_MATRIX.mult(thetaDotVector); - var B_INV = B_MATRIX.invert(); - - var u = B_INV.mult(M.plus(C).plus(Kb).plus(Tg_VECTOR)); - - return new TwoJointedArmFeedforwardResult(u.get(0, 0), u.get(1, 0)); - } - - /** - * Calculates the voltage feedforward required to move the arm in a certain (static) state. - * - * @param shoulderPosition the angle of the shoulder joint. - * @param elbowPosition the angle of the elbow joint. - * @return the voltage feedforward required to move the arm in the given (static) state. - */ - public TwoJointedArmFeedforwardResult calculateFeedforward( - Rotation2d shoulderPosition, Rotation2d elbowPosition) { - return calculateFeedforward( - shoulderPosition, - elbowPosition, - new Rotation2d(), - new Rotation2d(), - new Rotation2d(), - new Rotation2d()); - } - - /** Calculates the voltage feedforward required to move the arm in a certain state. */ - public static record TwoJointedArmFeedforwardResult(double shoulderVolts, double elbowVolts) {} -} diff --git a/src/main/java/frc/lib/JointConstants.java b/src/main/java/frc/lib/JointConstants.java deleted file mode 100644 index 005b4a8..0000000 --- a/src/main/java/frc/lib/JointConstants.java +++ /dev/null @@ -1,73 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.system.plant.DCMotor; -import java.util.Objects; - -public record JointConstants( - double massKg, - double lengthMeters, - double radiusMeters, - double moiKgMetersSquared, - double gearing, - DCMotor motor, - int motorCount) { - - /** - * Creates joint constants for the joint of an arm. - * - * @param massKg the joint's mass in kilograms. - * @param lengthMeters the joint's length in meters. - * @param radius the distance between the joint's pivot and the joint's center of mass in meters. - * @param moiKgMetersSquared the joint's moment of inertia in kilograms meters squared. - * @param gearing the gearing between the joint's motor and the joint's pivot. - * @param motor the type of the joint's driving motor. - * @param motorCount the number of the driving motors. - */ - public JointConstants { - Objects.requireNonNull(massKg); - Objects.requireNonNull(lengthMeters); - Objects.requireNonNull(radiusMeters); - Objects.requireNonNull(moiKgMetersSquared); - Objects.requireNonNull(gearing); - Objects.requireNonNull(motor); - Objects.requireNonNull(motorCount); - } - - /** - * Creates joint constants for the joint of an arm. - * - * @param massKg the joint's mass in kilograms. - * @param length the joint's length in meters. - * @param radius the distance between the joint's pivot and the joint's center of mass in meters. - * @param moi the joint's moment of inertia in kilograms meters squared. - * @param gearing the gearing between the joint's motor and the joint's pivot. - * @param motor the type of the joint's driving motor. - */ - public JointConstants( - double massKg, double length, double radius, double moi, double gearing, DCMotor motor) { - this(massKg, length, radius, moi, gearing, motor, 1); - } - - /** - * Calculates the torque generated by the joint's gearbox element for the feedforward matrix. - * - * @return the torque generated by the joint's gearbox element for the feedforward matrix. - */ - public double torqueNm() { - return gearing * motorCount * motor.KtNMPerAmp / motor.rOhms; - } - - /** - * Calculates the torque loss due to back-emf for the feedforward matrix. - * - * @return the torque loss due to back-emf for the feedforward matrix. - */ - public double torqueLossNm() { - return gearing - * gearing - * motorCount - * motor.KtNMPerAmp - / motor.rOhms - / motor.KvRadPerSecPerVolt; - } -} diff --git a/src/main/java/frc/lib/LinearServo.java b/src/main/java/frc/lib/LinearServo.java deleted file mode 100644 index 1dc42a5..0000000 --- a/src/main/java/frc/lib/LinearServo.java +++ /dev/null @@ -1,69 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.Timer; - -public class LinearServo extends Servo { - - private final double speedMetersPerSecond; - private final double maximumLengthMeters; - - private double position; - private double setpointMeters; - private double previousTimeSeconds = 0; - - public LinearServo( - int channel, double maxiumumSpeedMetersPerSecond, double maximumLengthMetersPerSecond) { - super(channel); - - this.speedMetersPerSecond = maxiumumSpeedMetersPerSecond; - this.maximumLengthMeters = maximumLengthMetersPerSecond; - - setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - } - - private void setBounds( - double maxMilliseconds, - double deadbandMaxMilliseconds, - double centerMilliseconds, - double deadbandMinMilliseconds, - double minMilliseconds) { - int maxMicroseconds = (int) (1000 * maxMilliseconds); - int deadbandMaxMicroseconds = (int) (1000 * deadbandMaxMilliseconds); - int centerMicroseconds = (int) (1000 * centerMilliseconds); - int deadbandMinMicroseconds = (int) (1000 * deadbandMinMilliseconds); - int minMicroseconds = (int) (1000 * minMilliseconds); - - setBoundsMicroseconds( - maxMicroseconds, - deadbandMaxMicroseconds, - centerMicroseconds, - deadbandMinMicroseconds, - minMicroseconds); - } - - public void setSetpoint(double setpointMeters) { - setpointMeters = MathUtil.clamp(setpointMeters, 0, maximumLengthMeters); - - this.setpointMeters = setpointMeters; - - double percentage = setpointMeters / maximumLengthMeters; - - double speed = 2 * percentage - 1; - - setSpeed(speed); - } - - public void updateCurPos() { - double currentTimeSeconds = Timer.getFPGATimestamp(); - double dt = currentTimeSeconds - previousTimeSeconds; - if (position > setpointMeters + speedMetersPerSecond * dt) { - position -= speedMetersPerSecond * dt; - } else if (position < setpointMeters - speedMetersPerSecond * dt) { - position += speedMetersPerSecond * dt; - } else { - position = setpointMeters; - } - } -} diff --git a/src/main/java/frc/lib/PIDFConstants.java b/src/main/java/frc/lib/PIDFConstants.java index df95c41..bf2f7a6 100644 --- a/src/main/java/frc/lib/PIDFConstants.java +++ b/src/main/java/frc/lib/PIDFConstants.java @@ -17,18 +17,21 @@ public class PIDFConstants { /** Feedback controller position tolerance. */ public double kPositionTolerance = 0.0; - /** Feedback controller velocity constraint. */ - public double kVelocityConstraint = 0.0; - - /** Feedback controller acceleration constraint. */ - public double kAccelerationConstraint = 0.0; + /** Feedback controller velocity tolerance. */ + public double kVelocityTolerance = 0.0; /** Feedforward controller static gain. */ public double kS = 0.0; + /** Feedforward controller gravity gain. */ + public double kG = 0.0; + /** Feedforward controller velocity gain. */ public double kV = 0.0; + /** Feedforward controller acceleration gain. */ + public double kA = 0.0; + /** * Creates a Phoenix PIDF configuration using the PIDF constants. * @@ -41,7 +44,9 @@ public Slot0Configs asSlot0Configs() { slot0Configs.kI = kI; slot0Configs.kD = kD; slot0Configs.kS = kS; + slot0Configs.kG = kG; slot0Configs.kV = kV; + slot0Configs.kA = kA; return slot0Configs; } diff --git a/src/main/java/frc/lib/SingleJointedArmFeedforward.java b/src/main/java/frc/lib/SingleJointedArmFeedforward.java deleted file mode 100644 index 2d3798a..0000000 --- a/src/main/java/frc/lib/SingleJointedArmFeedforward.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SingleJointedArmFeedforward { - - public static class SingleJointedArmFeedforwardConstants { - public double kS; - public double kG; - public double kV; - public double kA; - - public SingleJointedArmFeedforwardConstants() { - this.kS = 0.0; - this.kG = 0.0; - this.kV = 0.0; - this.kA = 0.0; - } - - public SingleJointedArmFeedforwardConstants withKs(double kS) { - this.kS = kS; - return this; - } - - public SingleJointedArmFeedforwardConstants withKg(double kG) { - this.kG = kG; - return this; - } - - /** - * Calculates the gravity compensation constant for an arm given the voltage applied at an - * equilibrium position. - * - * @param angle the equilibrium position of the arm. - * @param volts the voltage applied to the arm to hold it the equilibrium position. - * @return the gravity compensation constant for the arm. - */ - public SingleJointedArmFeedforwardConstants withKg(Rotation2d angle, double volts) { - double kG = volts / angle.getCos(); - - return this.withKg(kG); - } - - public SingleJointedArmFeedforwardConstants withKv(double kV) { - this.kV = kV; - return this; - } - - public SingleJointedArmFeedforwardConstants withKa(double kA) { - this.kA = kA; - return this; - } - } - - private final SingleJointedArmFeedforwardConstants constants; - - public SingleJointedArmFeedforward() { - this.constants = new SingleJointedArmFeedforwardConstants(); - } - - public SingleJointedArmFeedforward(SingleJointedArmFeedforwardConstants constants) { - this.constants = constants; - } - - public double calculate(Rotation2d position) { - return calculate(position, new Rotation2d()); - } - - public double calculate(Rotation2d position, Rotation2d velocity) { - return calculate(position, new Rotation2d(), new Rotation2d()); - } - - public double calculate(Rotation2d position, Rotation2d velocity, Rotation2d acceleration) { - return constants.kS * Math.signum(velocity.getRotations()) - + constants.kG * position.getCos() - + constants.kV * velocity.getRotations() - + constants.kA * acceleration.getRotations(); - } -} diff --git a/src/main/java/frc/lib/SnapRotation.java b/src/main/java/frc/lib/SnapRotation.java deleted file mode 100644 index db2373d..0000000 --- a/src/main/java/frc/lib/SnapRotation.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SnapRotation { - - private final Rotation2d multiple; - - private SnapRotation(Rotation2d multiple) { - this.multiple = multiple; - } - - public static SnapRotation to(Rotation2d multiple) { - return new SnapRotation(multiple); - } - - private double snapToNearest(double n, double multiple) { - return Math.round(n / multiple) * multiple; - } - - public Rotation2d snap(Rotation2d angle) { - double snapped = snapToNearest(angle.getRotations(), multiple.getRotations()); - - return Rotation2d.fromRotations(snapped); - } -} diff --git a/src/main/java/frc/lib/TrapezoidProfileTelemetry.java b/src/main/java/frc/lib/TrapezoidProfileTelemetry.java deleted file mode 100644 index 8691e7f..0000000 --- a/src/main/java/frc/lib/TrapezoidProfileTelemetry.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.networktables.DoubleEntry; - -public class TrapezoidProfileTelemetry { - - private final DoubleEntry measuredPosition, measuredVelocity; - private final DoubleEntry setpointPosition, setpointVelocity; - private final DoubleEntry goalPosition, goalVelocity; - - public TrapezoidProfileTelemetry(String table) { - measuredPosition = Telemetry.addDoubleEntry(table, "measuredPosition"); - measuredVelocity = Telemetry.addDoubleEntry(table, "measuredVelocity"); - setpointPosition = Telemetry.addDoubleEntry(table, "setpointPosition"); - setpointVelocity = Telemetry.addDoubleEntry(table, "setpointVelocity"); - goalPosition = Telemetry.addDoubleEntry(table, "goalPosition"); - goalVelocity = Telemetry.addDoubleEntry(table, "goalVelocity"); - } - - public void updateMeasurement(double position, double velocity) { - measuredPosition.set(position); - measuredVelocity.set(velocity); - } - - public void updateMeasurement(State state) { - updateMeasurement(state.position, state.velocity); - } - - public void updateSetpoint(double position, double velocity) { - setpointPosition.set(position); - setpointVelocity.set(velocity); - } - - public void updateSetpoint(State state) { - updateSetpoint(state.position, state.velocity); - } - - public void updateGoal(double position, double velocity) { - goalPosition.set(position); - goalVelocity.set(velocity); - } - - public void updateGoal(State state) { - updateGoal(state.position, state.velocity); - } - - public void update(State measurement, State setpoint, State goal) { - updateMeasurement(measurement); - updateSetpoint(setpoint); - updateGoal(goal); - } -} diff --git a/src/main/java/frc/lib/controller/PositionControllerIO.java b/src/main/java/frc/lib/controller/PositionControllerIO.java new file mode 100644 index 0000000..b6787c8 --- /dev/null +++ b/src/main/java/frc/lib/controller/PositionControllerIO.java @@ -0,0 +1,106 @@ +package frc.lib.controller; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.lib.Telemetry; + +/** Position controller interface. */ +public interface PositionControllerIO { + + /** Position controller constants. */ + public static class PositionControllerIOConstants { + /** + * Interpret counterclockwise rotation on the motor face as having positive velocity, if set to + * true. + */ + public boolean ccwPositiveMotor = true; + + /** + * Interpret counterclockwise rotation on the encoder as having positive velocity, if set to + * true. + */ + public boolean ccwPositiveAbsoluteEncoder = true; + + /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */ + public boolean neutralBrake = false; + + /** Maximum amount of current, in amps, to provide to the motor. */ + public double currentLimitAmps = 40.0; + + /** Ratio between the position sensor and the controlled mechanism. */ + public double sensorToMechanismRatio = 1.0; + + /** Offset between absolute encoder reading and controlled mechanism position in rotations. */ + public double absoluteEncoderOffsetRotations = 0.0; + } + + /** Position controller values. */ + public static class PositionControllerIOValues { + /** Position in rotations. */ + public double positionRotations = 0.0; + + /** Velocity in rotations per second. */ + public double velocityRotationsPerSecond = 0.0; + + /** Acceleration in rotations per second per second. */ + public double accelerationRotationsPerSecondPerSecond = 0.0; + + /** Motor voltage in volts. */ + public double motorVolts = 0.0; + + /** Motor current in amps. */ + public double motorAmps = 0.0; + } + + /** + * Adds position controller values to Shuffleboard. + * + * @param tab + * @param name + * @param values + */ + public static void addToShuffleboard( + ShuffleboardTab tab, String name, PositionControllerIOValues values) { + ShuffleboardLayout positionController = Telemetry.addColumn(tab, name); + + positionController.addDouble( + "Position (deg)", () -> Units.rotationsToDegrees(values.positionRotations)); + positionController.addDouble( + "Velocity (dps)", () -> Units.rotationsToDegrees(values.velocityRotationsPerSecond)); + positionController.addDouble( + "Acceleration (dpsps)", + () -> Units.rotationsToDegrees(values.accelerationRotationsPerSecondPerSecond)); + positionController.addDouble("Voltage (V)", () -> values.motorVolts); + positionController.addDouble("Current (A)", () -> values.motorAmps); + } + + /** + * Configures the position controller. + * + * @param constants + */ + public void configure(PositionControllerIOConstants constants); + + /** + * Updates the position controller's values. + * + * @param values + */ + public void update(PositionControllerIOValues values); + + /** + * Sets the mechanism position. + * + * @param positionRotations + */ + public void setPosition(double positionRotations); + + /** + * Sets the position setpoint. + * + * @param positionRotations + * @param velocityRotationsPerSecond + */ + public void setSetpoint(double positionRotations, double velocityRotationsPerSecond); +} diff --git a/src/main/java/frc/lib/controller/PositionControllerIOSim.java b/src/main/java/frc/lib/controller/PositionControllerIOSim.java new file mode 100644 index 0000000..e4f2bef --- /dev/null +++ b/src/main/java/frc/lib/controller/PositionControllerIOSim.java @@ -0,0 +1,29 @@ +package frc.lib.controller; + +/** Simulated position controller. */ +public class PositionControllerIOSim implements PositionControllerIO { + + private double positionRotations = 0.0; + + private double velocityRotationsPerSecond = 0.0; + + @Override + public void configure(PositionControllerIOConstants constants) {} + + @Override + public void update(PositionControllerIOValues values) { + values.positionRotations = positionRotations; + values.velocityRotationsPerSecond = velocityRotationsPerSecond; + } + + @Override + public void setPosition(double positionRotations) { + this.positionRotations = positionRotations; + } + + @Override + public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { + this.positionRotations = positionRotations; + this.velocityRotationsPerSecond = velocityRotationsPerSecond; + } +} diff --git a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java new file mode 100644 index 0000000..6882cf8 --- /dev/null +++ b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java @@ -0,0 +1,149 @@ +package frc.lib.controller; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; +import frc.lib.CAN; +import frc.lib.Configurator; +import frc.lib.PIDFConstants; + +/** Position controller using two TalonFXs and a CANcoder and an external PIDF for an arm. */ +public class PositionControllerIOTalonFX2 implements PositionControllerIO { + + private final TalonFX leaderMotor, followerMotor; + + private final CANcoder encoder; + + private final StatusSignal position, velocity, acceleration, volts, amps; + + private final ArmFeedforward feedforward; + + private final PIDController feedback; + + private final VoltageOut voltage; + + /** + * Creates a new position controller using two TalonFXs and a CANcoder and an external PIDF for an + * arm. + * + * @param leaderCAN + * @param followerCAN + * @param encoderCAN + * @param pidf + * @param enableFOC + * @param invertFollower + */ + public PositionControllerIOTalonFX2( + CAN leaderCAN, + CAN followerCAN, + CAN encoderCAN, + PIDFConstants pidf, + boolean enableFOC, + boolean invertFollower) { + leaderMotor = new TalonFX(leaderCAN.id(), leaderCAN.bus()); + followerMotor = new TalonFX(followerCAN.id(), followerCAN.bus()); + + encoder = new CANcoder(encoderCAN.id(), encoderCAN.bus()); + + position = encoder.getAbsolutePosition(); + + velocity = leaderMotor.getVelocity(); + acceleration = leaderMotor.getAcceleration(); + + volts = leaderMotor.getMotorVoltage(); + amps = leaderMotor.getStatorCurrent(); + + feedforward = new ArmFeedforward(pidf.kS, pidf.kG, pidf.kV, pidf.kA); + + feedback = new PIDController(pidf.kP, pidf.kI, pidf.kD); + + followerMotor.setControl(new Follower(leaderMotor.getDeviceID(), invertFollower)); + + voltage = new VoltageOut(0.0).withEnableFOC(enableFOC); + } + + @Override + public void configure(PositionControllerIOConstants constants) { + BaseStatusSignal.setUpdateFrequencyForAll(100.0, position, velocity, acceleration, volts, amps); + + ParentDevice.optimizeBusUtilizationForAll(leaderMotor, followerMotor, encoder); + + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + + motorConfig.MotorOutput.Inverted = + constants.ccwPositiveMotor + ? InvertedValue.CounterClockwise_Positive + : InvertedValue.Clockwise_Positive; + motorConfig.MotorOutput.NeutralMode = + constants.neutralBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast; + + // Stator current is a measure of the current inside of the motor and is typically higher than + // supply (breaker) current + motorConfig.CurrentLimits.StatorCurrentLimit = constants.currentLimitAmps * 2.0; + motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + motorConfig.CurrentLimits.SupplyCurrentLimit = constants.currentLimitAmps; + // Allow higher current spikes (150%) for a brief duration (one second) + // REV 40A auto-resetting breakers typically trip when current exceeds 300% for one second + motorConfig.CurrentLimits.SupplyCurrentThreshold = constants.currentLimitAmps * 1.5; + motorConfig.CurrentLimits.SupplyTimeThreshold = 1; + motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + motorConfig.Feedback.SensorToMechanismRatio = constants.sensorToMechanismRatio; + + Configurator.configureTalonFX(leaderMotor.getConfigurator(), motorConfig); + Configurator.configureTalonFX(followerMotor.getConfigurator(), motorConfig); + + CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); + + encoderConfig.MagnetSensor.MagnetOffset = constants.absoluteEncoderOffsetRotations; + encoderConfig.MagnetSensor.SensorDirection = + constants.ccwPositiveAbsoluteEncoder + ? SensorDirectionValue.CounterClockwise_Positive + : SensorDirectionValue.Clockwise_Positive; + + Configurator.configureCANcoder(encoder.getConfigurator(), encoderConfig); + } + + @Override + public void update(PositionControllerIOValues values) { + BaseStatusSignal.refreshAll(position, velocity, acceleration, volts, amps); + + values.positionRotations = position.getValue(); + values.velocityRotationsPerSecond = velocity.getValue(); + values.accelerationRotationsPerSecondPerSecond = acceleration.getValue(); + values.motorVolts = volts.getValue(); + values.motorAmps = amps.getValue(); + } + + @Override + public void setPosition(double positionRotations) { + leaderMotor.setPosition(positionRotations); + followerMotor.setPosition(positionRotations); + } + + @Override + public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { + double measuredPositionRotations = position.getValue(); + + double feedforwardVolts = + feedforward.calculate( + Units.rotationsToRadians(measuredPositionRotations), velocityRotationsPerSecond); + + double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); + + leaderMotor.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts)); + } +} diff --git a/src/main/java/frc/lib/controller/VelocityControllerIO.java b/src/main/java/frc/lib/controller/VelocityControllerIO.java new file mode 100644 index 0000000..6ed9200 --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIO.java @@ -0,0 +1,94 @@ +package frc.lib.controller; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.lib.Telemetry; + +/** Velocity controller interface. */ +public interface VelocityControllerIO { + + /** Velocity controller constants. */ + public static class VelocityControllerIOConstants { + /** + * Interpret counterclockwise rotation on the motor face as having positive velocity, if set to + * true. + */ + public boolean ccwPositive = true; + + /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */ + public boolean neutralBrake = false; + + /** Maximum amount of current, in amps, to provide to the motor. */ + public double currentLimitAmps = 40.0; + + /** Ratio between the velocity sensor and the controlled mechanism. */ + public double sensorToMechanismRatio = 1.0; + } + + /** Velocity controller values. */ + public static class VelocityControllerIOValues { + /** Velocity in rotations per second. */ + public double velocityRotationsPerSecond = 0.0; + + /** Acceleration in rotations per second per second. */ + public double accelerationRotationsPerSecondPerSecond = 0.0; + + /** Motor voltage in volts. */ + public double motorVolts = 0.0; + + /** Motor current in amps. */ + public double motorAmps = 0.0; + } + + /** + * Adds velocity controller values to Shuffleboard. + * + * @param tab + * @param name + * @param values + */ + public static void addToShuffleboard( + ShuffleboardTab tab, String name, VelocityControllerIOValues values) { + ShuffleboardLayout velocityController = Telemetry.addColumn(tab, name); + + velocityController.addDouble( + "Velocity (dps)", () -> Units.rotationsToDegrees(values.velocityRotationsPerSecond)); + velocityController.addDouble( + "Acceleration (dpsps)", + () -> Units.rotationsToDegrees(values.accelerationRotationsPerSecondPerSecond)); + velocityController.addDouble( + "Velocity (rps)", () -> values.velocityRotationsPerSecond); + velocityController.addDouble( + "Acceleration (rpsps)", + () -> values.accelerationRotationsPerSecondPerSecond); + velocityController.addDouble( + "Velocity (rpm)", () -> values.velocityRotationsPerSecond * 60); + velocityController.addDouble( + "Acceleration (rpmpm)", + () -> values.accelerationRotationsPerSecondPerSecond * 60); + velocityController.addDouble("Voltage (V)", () -> values.motorVolts); + velocityController.addDouble("Current (A)", () -> values.motorAmps); + } + + /** + * Configures the velocity controller. + * + * @param constants + */ + public void configure(VelocityControllerIOConstants constants); + + /** + * Updates the velocity controller's values. + * + * @param values + */ + public void update(VelocityControllerIOValues values); + + /** + * Sets the velocity setpoint. + * + * @param velocityRotationsPerSecond + */ + public void setSetpoint(double velocityRotationsPerSecond); +} diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOSim.java b/src/main/java/frc/lib/controller/VelocityControllerIOSim.java new file mode 100644 index 0000000..b5c1820 --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIOSim.java @@ -0,0 +1,20 @@ +package frc.lib.controller; + +/** Simulated velocity controller. */ +public class VelocityControllerIOSim implements VelocityControllerIO { + + private double velocityRotationsPerSecond = 0.0; + + @Override + public void configure(VelocityControllerIOConstants constants) {} + + @Override + public void update(VelocityControllerIOValues values) { + values.velocityRotationsPerSecond = velocityRotationsPerSecond; + } + + @Override + public void setSetpoint(double velocityRotationsPerSecond) { + this.velocityRotationsPerSecond = velocityRotationsPerSecond; + } +} diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java new file mode 100644 index 0000000..ade36ab --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java @@ -0,0 +1,78 @@ +package frc.lib.controller; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.lib.CAN; +import frc.lib.Configurator; + +/** Velocity controller using TalonFX. */ +public abstract class VelocityControllerIOTalonFX implements VelocityControllerIO { + + protected final TalonFX motor; + + protected final StatusSignal velocity, acceleration, volts, amps; + + /** + * Creates a new velocity controller using TalonFX. + * + * @param can + */ + protected VelocityControllerIOTalonFX(CAN can) { + motor = new TalonFX(can.id(), can.bus()); + + velocity = motor.getVelocity(); + acceleration = motor.getAcceleration(); + volts = motor.getMotorVoltage(); + amps = motor.getStatorCurrent(); + } + + @Override + public void configure(VelocityControllerIOConstants constants) { + BaseStatusSignal.setUpdateFrequencyForAll(100.0, velocity, acceleration, volts, amps); + + ParentDevice.optimizeBusUtilizationForAll(motor); + + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.Inverted = + constants.ccwPositive + ? InvertedValue.CounterClockwise_Positive + : InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = + constants.neutralBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast; + + // Stator current is a measure of the current inside of the motor and is typically higher than + // supply (breaker) current + config.CurrentLimits.StatorCurrentLimit = constants.currentLimitAmps * 2.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + + config.CurrentLimits.SupplyCurrentLimit = constants.currentLimitAmps; + // Allow higher current spikes (150%) for a brief duration (one second) + // REV 40A auto-resetting breakers typically trip when current exceeds 300% for one second + config.CurrentLimits.SupplyCurrentThreshold = constants.currentLimitAmps * 1.5; + config.CurrentLimits.SupplyTimeThreshold = 1; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + config.Feedback.SensorToMechanismRatio = constants.sensorToMechanismRatio; + + Configurator.configureTalonFX(motor.getConfigurator(), config); + } + + @Override + public void update(VelocityControllerIOValues values) { + BaseStatusSignal.refreshAll(velocity, acceleration, volts, amps); + + values.velocityRotationsPerSecond = velocity.getValue(); + values.accelerationRotationsPerSecondPerSecond = acceleration.getValue(); + values.motorVolts = volts.getValue(); + values.motorAmps = amps.getValue(); + } + + @Override + public abstract void setSetpoint(double velocityRotationsPerSecond); +} diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java new file mode 100644 index 0000000..38354e6 --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java @@ -0,0 +1,56 @@ +package frc.lib.controller; + +import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import frc.lib.CAN; +import frc.lib.PIDFConstants; + +/** Velocity controller using TalonFX and external PIDF. */ +public class VelocityControllerIOTalonFXPIDF extends VelocityControllerIOTalonFX { + + private final SimpleMotorFeedforward feedforward; + + private final PIDController feedback; + + private final VoltageOut voltage; + + /** + * Creates a new velocity controller using TalonFX and external PIDF. + * + * @param can + * @param pidf + * @param enableFOC + */ + public VelocityControllerIOTalonFXPIDF(CAN can, PIDFConstants pidf, boolean enableFOC) { + super(can); + + feedforward = new SimpleMotorFeedforward(pidf.kS, pidf.kV, pidf.kA); + + feedback = new PIDController(pidf.kP, pidf.kI, pidf.kD); + + voltage = new VoltageOut(0.0).withEnableFOC(enableFOC); + } + + /** + * Creates a new velocity controller using TalonFX and external PIDF. + * + * @param can + * @param pidf + */ + public VelocityControllerIOTalonFXPIDF(CAN can, PIDFConstants pidf) { + this(can, pidf, false); + } + + @Override + public void setSetpoint(double velocityRotationsPerSecond) { + double feedforwardVolts = feedforward.calculate(velocityRotationsPerSecond); + + double measuredVelocityRotationsPerSecond = velocity.getValue(); + + double feedbackVolts = + feedback.calculate(measuredVelocityRotationsPerSecond, velocityRotationsPerSecond); + + motor.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts)); + } +} diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 7f64a20..18eb7f0 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -45,11 +45,7 @@ public enum Subsystem { public static final Set ALL_SUBSYSTEMS = EnumSet.of( - Subsystem.ARM, - Subsystem.INTAKE, - Subsystem.ODOMETRY, - Subsystem.SHOOTER, - Subsystem.SWERVE); + Subsystem.ARM, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE); public static final Set REAL_SUBSYSTEMS = ALL_SUBSYSTEMS; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2a6c728..f630321 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,12 +1,13 @@ package frc.robot; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Telemetry; import frc.robot.arm.Arm; -import frc.robot.arm.ManualCommand; import frc.robot.auto.Auto; import frc.robot.intake.Intake; import frc.robot.odometry.Odometry; @@ -31,6 +32,8 @@ public class RobotContainer { private final CommandXboxController driverController, operatorController; + private final XboxController rumbleController; + /** Creates a new instance of the robot container. */ private RobotContainer() { arm = Arm.getInstance(); @@ -43,6 +46,7 @@ private RobotContainer() { driverController = new CommandXboxController(0); operatorController = new CommandXboxController(1); + rumbleController = new XboxController(1); initializeTelemetry(); configureDefaultCommands(); @@ -75,19 +79,33 @@ private void configureDefaultCommands() { /** Configures controller bindings. */ private void configureBindings() { + driverController.a().whileTrue(swerve.forwards()); + driverController.b().whileTrue(swerve.sideways()); + driverController.x().whileTrue(swerve.cross()); + driverController.y().onTrue(odometry.tare()); operatorController.leftBumper().onTrue(superstructure.eject()); operatorController.leftTrigger().onTrue(superstructure.intake()); - operatorController.rightBumper().onTrue(superstructure.pass()); - operatorController.rightTrigger().onTrue(superstructure.shoot()); + operatorController.rightBumper().onTrue(superstructure.podium()); + operatorController.rightTrigger().onTrue(superstructure.subwoofer()); - // operatorController.a().onTrue(superstructure.ampPosition()); - // operatorController.b().onTrue(superstructure.ampShoot()); + operatorController.a().onTrue(superstructure.amp()); + operatorController.b().onTrue(superstructure.lob()); operatorController.x().onTrue(superstructure.stow()); + operatorController.y().onTrue(superstructure.skim()); + + intake.noteStuck().whileTrue(rumble(RumbleType.kLeftRumble)); + + shooter.serializedNote().whileTrue(rumble(RumbleType.kRightRumble)); + } - operatorController.y().whileTrue(superstructure.manualControl().andThen(new ManualCommand(operatorController::getLeftY))).onFalse(Commands.runOnce(() -> superstructure.setSetpoint(superstructure.getState()))); + public Command rumble(RumbleType side) { + return Commands.startEnd( + () -> rumbleController.setRumble(side, 1), + () -> rumbleController.setRumble(side, 0) + ); } /** diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 1894a1f..4054d58 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -1,13 +1,14 @@ package frc.robot.arm; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues; +import frc.lib.controller.PositionControllerIO; +import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; +import frc.robot.arm.ArmConstants.ShoulderConstants; /** Subsystem class for the arm subsystem. */ public class Arm extends Subsystem { @@ -15,17 +16,28 @@ public class Arm extends Subsystem { /** Instance variable for the arm subsystem singleton. */ private static Arm instance = null; - /** Shoulder motor. */ - private final ShoulderMotorIO shoulderMotor; + /** Shoulder. */ + private final PositionControllerIO shoulder; - /** Shoulder motor values. */ - private final ShoulderMotorIOValues shoulderMotorValues = new ShoulderMotorIOValues(); + /** Shoulder values. */ + private final PositionControllerIOValues shoulderValues; + + private double lastTimeSeconds; + + /** Arm goal. */ + private ArmState setpoint, goal; /** Creates a new instance of the arm subsystem. */ private Arm() { - shoulderMotor = ArmFactory.createShoulderMotor(); + shoulder = ArmFactory.createShoulder(); + shoulderValues = new PositionControllerIOValues(); + shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS); + + lastTimeSeconds = Timer.getFPGATimestamp(); - shoulderMotor.configure(); + setPosition(ArmState.INITIAL); + setpoint = ArmState.INITIAL; + goal = ArmState.INITIAL; } /** @@ -43,44 +55,47 @@ public static Arm getInstance() { @Override public void periodic() { - shoulderMotor.update(shoulderMotorValues); - } + shoulder.update(shoulderValues); - @Override - public void addToShuffleboard(ShuffleboardTab tab) { - ShuffleboardLayout position = Telemetry.addColumn(tab, "Position"); + double timeSeconds = Timer.getFPGATimestamp(); + + double dt = timeSeconds - lastTimeSeconds; - position.addDouble( - "Shoulder Position (deg)", - () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations)); + lastTimeSeconds = timeSeconds; - ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages"); + setpoint = + new ArmState( + ShoulderConstants.MOTION_PROFILE.calculate( + dt, + setpoint.shoulderRotations(), + goal.shoulderRotations())); - voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.inputVoltage); + shoulder.setSetpoint( + setpoint.shoulderRotations().position, setpoint.shoulderRotations().velocity); + } - ShuffleboardLayout derivatives = Telemetry.addColumn(tab, "Derivatives"); + @Override + public void addToShuffleboard(ShuffleboardTab tab) { + PositionControllerIO.addToShuffleboard(tab, "Shoulder", shoulderValues); - derivatives.addDouble( - "Shoulder Velocity (rps)", () -> shoulderMotorValues.velocityRotationsPerSecond); - derivatives.addDouble( - "Shoulder Acceleration (rpsps)", - () -> shoulderMotorValues.accelerationRotationsPerSecondPerSecond); + Telemetry.addColumn(tab, "Setpoint").addDouble("Setpoint (deg)", () -> Units.rotationsToDegrees(setpoint.shoulderRotations().position)); } - public State getMeasuredShoulderState() { - return new TrapezoidProfile.State( - shoulderMotorValues.positionRotations, shoulderMotorValues.velocityRotationsPerSecond); + public ArmState getState() { + return new ArmState( + new TrapezoidProfile.State( + shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond)); } - public void setShoulderPosition(double positionRotations) { - shoulderMotor.setPosition(positionRotations); + public void setGoal(ArmState goal) { + this.goal = goal; } - public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond); + public boolean atGoal() { + return getState().at(goal); } - public void setVoltage(double volts) { - shoulderMotor.setVoltage(volts); + public void setPosition(ArmState armState) { + shoulder.setPosition(armState.shoulderRotations().position); } } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index d8c7f14..61d85bc 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -1,4 +1,74 @@ package frc.robot.arm; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.lib.CAN; +import frc.lib.MotionProfileCalculator; +import frc.lib.PIDFConstants; +import frc.lib.controller.PositionControllerIO.PositionControllerIOConstants; + /** Constants for the arm subsystem. */ -public class ArmConstants {} +public class ArmConstants { + /** Constants for the shoulder. */ + public static class ShoulderConstants { + /** Shoulder's leader CAN. */ + public static final CAN LEADER_CAN = new CAN(48); + + /** Shoulder's follower CAN. */ + public static final CAN FOLLOWER_CAN = new CAN(46); + + /** Shoulder's encoder CAN. */ + public static final CAN ENCODER_CAN = new CAN(52); + + /** Shoulder's PIDF constants. */ + public static final PIDFConstants PIDF = new PIDFConstants(); + + static { + PIDF.kS = 0.14; + PIDF.kG = 0.5125; + PIDF.kV = 4.0; + PIDF.kP = 4.0; + } + + /** Shoulder's controller constants. */ + public static final PositionControllerIOConstants CONTROLLER_CONSTANTS = + new PositionControllerIOConstants(); + + static { + CONTROLLER_CONSTANTS.ccwPositiveMotor = true; + CONTROLLER_CONSTANTS.ccwPositiveAbsoluteEncoder = false; + CONTROLLER_CONSTANTS.neutralBrake = true; + CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571; + CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-173.135); + } + + /** Maximum speed of the shoulder in rotations per second. */ + public static final double MAXIMUM_SPEED = Units.degreesToRotations(240.0); + + /** Maximum acceleration of the shoulder in rotations per second per second. */ + public static final double MAXIMUM_ACCELERATION = + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25); + + /** Maximum speed and acceleration of the shoulder . */ + public static final TrapezoidProfile.Constraints CONSTRAINTS = + new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); + + /** Motion profile of the shoulder. */ + public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); + + public static final Rotation2d STOW = Rotation2d.fromDegrees(-26); + + public static final Rotation2d LOB = Rotation2d.fromDegrees(-26); + + public static final Rotation2d SUBWOOFER = Rotation2d.fromDegrees(-15); + + public static final Rotation2d PODIUM = Rotation2d.fromDegrees(-10); + + public static final Rotation2d EJECT = Rotation2d.fromDegrees(30); + + public static final Rotation2d SKIM = Rotation2d.fromDegrees(30); + + public static final Rotation2d AMP = Rotation2d.fromDegrees(80); + } +} diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java index 6bade0d..847b7a1 100644 --- a/src/main/java/frc/robot/arm/ArmFactory.java +++ b/src/main/java/frc/robot/arm/ArmFactory.java @@ -1,8 +1,12 @@ package frc.robot.arm; +import frc.lib.controller.PositionControllerIO; +import frc.lib.controller.PositionControllerIOSim; +import frc.lib.controller.PositionControllerIOTalonFX2; import frc.robot.Robot; import frc.robot.RobotConstants; import frc.robot.RobotConstants.Subsystem; +import frc.robot.arm.ArmConstants.ShoulderConstants; /** Helper class for creating hardware for the arm subsystem. */ public class ArmFactory { @@ -12,10 +16,17 @@ public class ArmFactory { * * @return a shoulder motor. */ - public static ShoulderMotorIO createShoulderMotor() { - if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) - return new ShoulderMotorIOTalonFX(); + public static PositionControllerIO createShoulder() { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) { + return new PositionControllerIOTalonFX2( + ShoulderConstants.LEADER_CAN, + ShoulderConstants.FOLLOWER_CAN, + ShoulderConstants.ENCODER_CAN, + ShoulderConstants.PIDF, + false, + true); + } - return new ShoulderMotorIOSim(); + return new PositionControllerIOSim(); } } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java new file mode 100644 index 0000000..5ba562c --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -0,0 +1,41 @@ +package frc.robot.arm; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.math.util.Units; +import frc.robot.arm.ArmConstants.ShoulderConstants; + +import java.util.Objects; + +public record ArmState(State shoulderRotations) { + + public static final ArmState INITIAL = new ArmState(ShoulderConstants.STOW); + + public static final ArmState STOW = new ArmState(ShoulderConstants.STOW); + + public static final ArmState SUBWOOFER = new ArmState(ShoulderConstants.SUBWOOFER); + + public static final ArmState PODIUM = new ArmState(ShoulderConstants.PODIUM); + + public static final ArmState EJECT = new ArmState(ShoulderConstants.EJECT); + + public static final ArmState SKIM = new ArmState(ShoulderConstants.SKIM); + + public static final ArmState LOB = new ArmState(ShoulderConstants.LOB); + + public static final ArmState AMP = new ArmState(ShoulderConstants.AMP); + + public ArmState { + Objects.requireNonNull(shoulderRotations); + } + + public ArmState(Rotation2d shoulder) { + this(new State(shoulder.getRotations(), 0)); + } + + public boolean at(ArmState other) { + return MathUtil.isNear( + shoulderRotations.position, other.shoulderRotations.position, Units.degreesToRotations(2.0)); + } +} diff --git a/src/main/java/frc/robot/arm/ManualCommand.java b/src/main/java/frc/robot/arm/ManualCommand.java deleted file mode 100644 index b5d3461..0000000 --- a/src/main/java/frc/robot/arm/ManualCommand.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.arm; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; - -public class ManualCommand extends Command { - - private final Arm arm; - - private final DoubleSupplier joystick; - - public ManualCommand(DoubleSupplier joystick) { - arm = Arm.getInstance(); - - this.joystick = joystick; - - addRequirements(arm); - } - - @Override - public void execute() { - double voltageScalar = 12.0; - - double percent = -joystick.getAsDouble(); - - double volts = percent * voltageScalar; - - double positionRotations = arm.getMeasuredShoulderState().position; - - if (positionRotations > ShoulderAngleConstants.AMP.getRotations() && volts > 0) { - volts = 0; - } - - if (positionRotations < ShoulderAngleConstants.STOW.getRotations() && volts < 0) { - volts = 0; - } - - volts = MathUtil.clamp(volts, -12.0, 6.0); - - arm.setVoltage(volts); - } - - @Override - public void end(boolean interrupted) { - arm.setVoltage(0); - } - -} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIO.java b/src/main/java/frc/robot/arm/ShoulderMotorIO.java deleted file mode 100644 index 01c8a25..0000000 --- a/src/main/java/frc/robot/arm/ShoulderMotorIO.java +++ /dev/null @@ -1,55 +0,0 @@ -package frc.robot.arm; - -/** Shoulder motor hardware interface. */ -public interface ShoulderMotorIO { - - /** Values for the shoulder motor hardware interface. */ - public static class ShoulderMotorIOValues { - /** Position of the shoulder motor in rotations. */ - public double positionRotations = 0.0; - - /** Velocity of the shoulder motor in rotations per second. */ - public double velocityRotationsPerSecond = 0.0; - - /** Acceleration of the shoulder motor in rotations per second per second. */ - public double accelerationRotationsPerSecondPerSecond = 0.0; - - /** Current drawn by the shoulder motor in amps. */ - public double currentAmps = 0.0; - - /** Voltage applied to the shoulder motor in volts. */ - public double inputVoltage = 0.0; - } - - /** Configures the shoulder motor. */ - public void configure(); - - /** - * Updates the shoulder motor's values. - * - * @param values - */ - public void update(ShoulderMotorIOValues values); - - /** - * Sets the shoulder motor's position. - * - * @param positionRotations the shoulder motor's position. - */ - public void setPosition(double positionRotations); - - /** - * Runs the shoulder motor's setpoint. - * - * @param positionRotations the shoulder motor's position setpoint. - * @param velocityRotationsPerSecond the shoulder motor's velocity setpoint. - */ - public void setSetpoint(double positionRotations, double velocityRotationsPerSecond); - - /** - * Runs the shoulder motor with a specified voltage. - * - * @param volts the shoulder motor's voltage. - */ - public void setVoltage(double volts); -} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java deleted file mode 100644 index 5a05447..0000000 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.arm; - -import edu.wpi.first.wpilibj.DriverStation; -import frc.lib.AccelerationCalculator; - -/** Simulated shoulder motor. */ -public class ShoulderMotorIOSim implements ShoulderMotorIO { - - private double positionRotations; - private double velocityRotationsPerSecond; - private double inputVoltage; - - private final AccelerationCalculator accelerationCalculator; - - /** Creates a new simulated shoulder motor. */ - public ShoulderMotorIOSim() { - accelerationCalculator = new AccelerationCalculator(); - } - - @Override - public void configure() {} - - @Override - public void update(ShoulderMotorIOValues values) { - values.positionRotations = this.positionRotations; - values.velocityRotationsPerSecond = this.velocityRotationsPerSecond; - values.accelerationRotationsPerSecondPerSecond = - accelerationCalculator.calculate(velocityRotationsPerSecond); - - values.inputVoltage = inputVoltage; - } - - @Override - public void setPosition(double positionRotations) { - this.positionRotations = positionRotations; - } - - @Override - public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - if (DriverStation.isEnabled()) { - this.positionRotations = positionRotations; - this.velocityRotationsPerSecond = velocityRotationsPerSecond; - } - } - - @Override - public void setVoltage(double volts) { - this.inputVoltage = volts; - this.positionRotations += volts / 1000; - } -} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java b/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java deleted file mode 100644 index 8ef544d..0000000 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java +++ /dev/null @@ -1,111 +0,0 @@ -package frc.robot.arm; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; -import frc.lib.Configurator; - -/** Shoulder motor using a TalonFX. */ -public class ShoulderMotorIOTalonFX implements ShoulderMotorIO { - - private final TalonFX eastTalonFX, westTalonFX; - - private final CANcoder cancoder; - - /** Feedback controller for shoulder position. */ - private final PIDController feedback; - - /** Feedforward controller for shoulder position. */ - private final ArmFeedforward feedforward; - - /** Creates a new shoulder motor using a Spark Max. */ - public ShoulderMotorIOTalonFX() { - eastTalonFX = new TalonFX(46); - westTalonFX = new TalonFX(48); - - cancoder = new CANcoder(52); - - feedback = new PIDController(20, 0, 0); - - feedforward = new ArmFeedforward(0.14, 0.45, 4.0); - // feedforward = new ArmFeedforward(0.14, 0, 0); - } - - @Override - public void configure() { - final TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - - talonFXConfig.Feedback.SensorToMechanismRatio = 39.771428571; - - talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - talonFXConfig.CurrentLimits.StatorCurrentLimit = 40; - talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - - talonFXConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - Configurator.configureTalonFX(eastTalonFX.getConfigurator(), talonFXConfig); - - talonFXConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - Configurator.configureTalonFX(westTalonFX.getConfigurator(), talonFXConfig); - - final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - - cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - - cancoderConfig.MagnetSensor.MagnetOffset = Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07); - - Configurator.configureCANcoder(cancoder.getConfigurator(), cancoderConfig); - } - - @Override - public void update(ShoulderMotorIOValues values) { - values.positionRotations = getAbsolutePositionRotations(); - values.velocityRotationsPerSecond = westTalonFX.getVelocity().refresh().getValue(); - values.accelerationRotationsPerSecondPerSecond = - westTalonFX.getAcceleration().refresh().getValue(); - - values.currentAmps = westTalonFX.getStatorCurrent().refresh().getValue(); - values.inputVoltage = westTalonFX.getMotorVoltage().refresh().getValue(); - } - - @Override - public void setPosition(double positionRotations) { - eastTalonFX.setPosition(positionRotations); - westTalonFX.setPosition(positionRotations); - } - - @Override - public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - double measuredPositionRotations = getAbsolutePositionRotations(); - - double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); - - double feedforwardVolts = - feedforward.calculate( - Units.rotationsToRadians(measuredPositionRotations), velocityRotationsPerSecond); - - setVoltage(feedbackVolts + feedforwardVolts); - } - - /** - * Gets the absolute position of the shoulder in rotations. - * - * @return the absolute position of the shoulder in rotations. - */ - private double getAbsolutePositionRotations() { - return cancoder.getAbsolutePosition().refresh().getValue(); - } - - @Override - public void setVoltage(double volts) { - eastTalonFX.setVoltage(volts); - westTalonFX.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 18cef07..df7c696 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.AllianceFlipHelper; import frc.lib.Subsystem; import frc.lib.Telemetry; @@ -72,8 +71,8 @@ private Auto() { swerve); NamedCommands.registerCommand("stow", superstructure.stow()); - NamedCommands.registerCommand("shoot", Commands.deadline(Commands.waitSeconds(3.0), superstructure.shoot())); - NamedCommands.registerCommand("intake", superstructure.intake()); + NamedCommands.registerCommand("shoot", superstructure.subwoofer().withTimeout(1.5)); + NamedCommands.registerCommand("intake", superstructure.intake().withTimeout(1.5)); autoChooser = AutoBuilder.buildAutoChooser(); } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 3c4e813..f54af9e 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -1,10 +1,12 @@ package frc.robot.intake; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; -import frc.lib.Telemetry; -import frc.robot.intake.RollerMotorIO.RollerMotorIOValues; +import frc.lib.controller.VelocityControllerIO; +import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; +import frc.robot.intake.IntakeConstants.BackRollerConstants; +import frc.robot.intake.IntakeConstants.FrontRollerConstants; /** Subsystem class for the intake subsystem. */ public class Intake extends Subsystem { @@ -12,17 +14,26 @@ public class Intake extends Subsystem { /** Instance variable for the intake subsystem singleton. */ private static Intake instance = null; - /** Roller motor. */ - private final RollerMotorIO rollerMotor; + /** Rollers. */ + private final VelocityControllerIO frontRoller, backRoller; - /** Roller motor values. */ - private final RollerMotorIOValues rollerMotorValues = new RollerMotorIOValues(); + /** Roller values. */ + private final VelocityControllerIOValues frontRollerValues, backRollerValues; + + private IntakeState setpoint, goal; /** Creates a new instance of the intake subsystem. */ private Intake() { - rollerMotor = IntakeFactory.createRollerMotor(); + frontRoller = IntakeFactory.createFrontRoller(); + frontRollerValues = new VelocityControllerIOValues(); + frontRoller.configure(FrontRollerConstants.CONTROLLER_CONSTANTS); + + backRoller = IntakeFactory.createBackRoller(); + backRollerValues = new VelocityControllerIOValues(); + backRoller.configure(BackRollerConstants.CONTROLLER_CONSTANTS); - rollerMotor.configure(); + setpoint = IntakeState.IDLE; + goal = IntakeState.IDLE; } /** @@ -40,24 +51,43 @@ public static Intake getInstance() { @Override public void periodic() { - rollerMotor.update(rollerMotorValues); + frontRoller.update(frontRollerValues); + backRoller.update(backRollerValues); + + setpoint = goal; + + frontRoller.setSetpoint(setpoint.frontRollerVelocityRotationsPerSecond()); + backRoller.setSetpoint(setpoint.backRollerVelocityRotationsPerSecond()); } @Override public void addToShuffleboard(ShuffleboardTab tab) { - ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller"); + VelocityControllerIO.addToShuffleboard(tab, "Front Roller", frontRollerValues); + VelocityControllerIO.addToShuffleboard(tab, "Back Roller", backRollerValues); + } - roller.addDouble("Current (A)", () -> rollerMotorValues.currentAmps); - roller.addDouble("Velocity (rps)", () -> rollerMotorValues.velocityRotationsPerSecond); + public Trigger noteStuck() { + return new Trigger(() -> frontRollerStuck() || backRollerStuck()); } - public double getRollerVelocity() { - rollerMotor.update(rollerMotorValues); + private boolean frontRollerStuck() { + return frontRollerValues.motorAmps > FrontRollerConstants.NOTE_AMPS; + } + + private boolean backRollerStuck() { + return backRollerValues.motorAmps > BackRollerConstants.NOTE_AMPS; + } + + public IntakeState getState() { + return new IntakeState( + frontRollerValues.velocityRotationsPerSecond, backRollerValues.velocityRotationsPerSecond); + } - return rollerMotorValues.velocityRotationsPerSecond; + public void setGoal(IntakeState goal) { + this.goal = goal; } - public void setSetpoint(double rollerVelocityRotationsPerSecond) { - rollerMotor.setSetpoint(rollerVelocityRotationsPerSecond); + public boolean atGoal() { + return getState().at(goal); } } diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index ae38f76..07d5677 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -1,16 +1,76 @@ package frc.robot.intake; +import frc.lib.CAN; +import frc.lib.PIDFConstants; +import frc.lib.controller.VelocityControllerIO.VelocityControllerIOConstants; + /** Constants for the intake subsystem. */ public class IntakeConstants { - /** Constants for the roller motor. */ - public static class RollerConstants { - /** Velocity to apply when intaking in rotations per second. */ - public static final double INTAKE_VELOCITY = 34; + /** Constants for the front roller. */ + public static class FrontRollerConstants { + /** Front roller's CAN. */ + public static final CAN CAN = new CAN(50); + + /** Front roller's PIDF constants. */ + public static final PIDFConstants PIDF = new PIDFConstants(); + + static { + PIDF.kS = 0.13; + PIDF.kV = 0.1683; + PIDF.kP = 0.1; + } + + /** Front roller's controller constants. */ + public static final VelocityControllerIOConstants CONTROLLER_CONSTANTS = + new VelocityControllerIOConstants(); + + static { + CONTROLLER_CONSTANTS.ccwPositive = false; + CONTROLLER_CONSTANTS.neutralBrake = false; + CONTROLLER_CONSTANTS.sensorToMechanismRatio = 24.0 / 16.0; + } + + public static final double INTAKE_SPEED = 34; + + public static final double EJECT_SPEED = -34; + + /** Maximum speed of the roller in rotations per second. */ + public static final double MAXIMUM_SPEED = 67; + + public static final double NOTE_AMPS = 40; + } + + /** Constants for the back roller. */ + public static class BackRollerConstants { + /** Back roller's CAN. */ + public static final CAN CAN = new CAN(40); + + /** Back roller's PIDF constants. */ + public static final PIDFConstants PIDF = new PIDFConstants(); + + static { + PIDF.kS = 0.13; + PIDF.kV = 0.1759; + PIDF.kP = 0.1; + } + + /** Back roller's controller constants. */ + public static final VelocityControllerIOConstants CONTROLLER_CONSTANTS = + new VelocityControllerIOConstants(); + + static { + CONTROLLER_CONSTANTS.ccwPositive = false; + CONTROLLER_CONSTANTS.neutralBrake = false; + CONTROLLER_CONSTANTS.sensorToMechanismRatio = 24.0 / 16.0; + } + + public static final double INTAKE_SPEED = 34; + + public static final double EJECT_SPEED = -34; /** Maximum speed of the roller in rotations per second. */ public static final double MAXIMUM_SPEED = 67; - /** Speed tolerance of the roller in rotations per second. */ - public static final double SPEED_TOLERANCE = 2.5; + public static final double NOTE_AMPS = 40; } } diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java index f710e31..72e42a2 100644 --- a/src/main/java/frc/robot/intake/IntakeFactory.java +++ b/src/main/java/frc/robot/intake/IntakeFactory.java @@ -1,21 +1,31 @@ package frc.robot.intake; +import frc.lib.controller.VelocityControllerIO; +import frc.lib.controller.VelocityControllerIOSim; +import frc.lib.controller.VelocityControllerIOTalonFXPIDF; import frc.robot.Robot; import frc.robot.RobotConstants; import frc.robot.RobotConstants.Subsystem; +import frc.robot.intake.IntakeConstants.BackRollerConstants; +import frc.robot.intake.IntakeConstants.FrontRollerConstants; /** Helper class for creating hardware for the intake subsystem. */ public class IntakeFactory { - /** - * Creates a roller motor. - * - * @return a roller motor. - */ - public static RollerMotorIO createRollerMotor() { - if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) - return new RollerMotorIOTalonFX(); + public static VelocityControllerIO createFrontRoller() { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) { + return new VelocityControllerIOTalonFXPIDF( + FrontRollerConstants.CAN, FrontRollerConstants.PIDF); + } - return new RollerMotorIOSim(); + return new VelocityControllerIOSim(); + } + + public static VelocityControllerIO createBackRoller() { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) { + return new VelocityControllerIOTalonFXPIDF(BackRollerConstants.CAN, BackRollerConstants.PIDF); + } + + return new VelocityControllerIOSim(); } } diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java new file mode 100644 index 0000000..2b1f650 --- /dev/null +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -0,0 +1,35 @@ +package frc.robot.intake; + +import edu.wpi.first.math.MathUtil; +import frc.robot.intake.IntakeConstants.BackRollerConstants; +import frc.robot.intake.IntakeConstants.FrontRollerConstants; + +import java.util.Objects; + +public record IntakeState( + double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) { + + public static final IntakeState IDLE = new IntakeState(0, 0); + + public static final IntakeState INTAKE = new IntakeState(FrontRollerConstants.INTAKE_SPEED, BackRollerConstants.INTAKE_SPEED); + + public static final IntakeState EJECT = new IntakeState(FrontRollerConstants.EJECT_SPEED, BackRollerConstants.EJECT_SPEED); + + public IntakeState { + Objects.requireNonNull(frontRollerVelocityRotationsPerSecond); + Objects.requireNonNull(backRollerVelocityRotationsPerSecond); + } + + public boolean at(IntakeState other) { + final double kToleranceRotationsPerSecond = 1; + + return MathUtil.isNear( + frontRollerVelocityRotationsPerSecond, + other.frontRollerVelocityRotationsPerSecond, + kToleranceRotationsPerSecond) + && MathUtil.isNear( + backRollerVelocityRotationsPerSecond, + other.backRollerVelocityRotationsPerSecond, + kToleranceRotationsPerSecond); + } +} diff --git a/src/main/java/frc/robot/intake/RollerMotorIO.java b/src/main/java/frc/robot/intake/RollerMotorIO.java deleted file mode 100644 index 283adb6..0000000 --- a/src/main/java/frc/robot/intake/RollerMotorIO.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.intake; - -/** Intake motor hardware interface. */ -public interface RollerMotorIO { - - /** Values for the roller motor hardware interface. */ - public static class RollerMotorIOValues { - /** Velocity of the roller motor in rotations per second. */ - public double velocityRotationsPerSecond = 0.0; - - /** Current draw of the roller motor in amps. */ - public double currentAmps = 0.0; - } - - /** Configures the roller motor. */ - public void configure(); - - /** - * Updates the roller motor's values. - * - * @param values - */ - public void update(RollerMotorIOValues values); - - /** - * Sets the setpoint of the roller motor. - * - * @param velocityRotationsPerSecond the velocity setpoint of the roller motor. - */ - public void setSetpoint(double velocityRotationsPerSecond); -} diff --git a/src/main/java/frc/robot/intake/RollerMotorIOSim.java b/src/main/java/frc/robot/intake/RollerMotorIOSim.java deleted file mode 100644 index 0cec0e2..0000000 --- a/src/main/java/frc/robot/intake/RollerMotorIOSim.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.intake; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.RobotConstants; - -/** Simulated roller motor. */ -public class RollerMotorIOSim implements RollerMotorIO { - - private final FlywheelSim rollerSim; - - private final SimpleMotorFeedforward rollerVelocityFeedforward; - - public RollerMotorIOSim() { - rollerSim = new FlywheelSim(DCMotor.getKrakenX60(1), 3.0, 0.03472); - - rollerVelocityFeedforward = new SimpleMotorFeedforward(0.0, 0.358); - } - - @Override - public void configure() {} - - @Override - public void update(RollerMotorIOValues values) { - values.velocityRotationsPerSecond = getRollerVelocityRotationsPerSecond(); - values.currentAmps = rollerSim.getCurrentDrawAmps(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = rollerVelocityFeedforward.calculate(velocityRotationsPerSecond); - - rollerSim.setInputVoltage(volts); - - rollerSim.update(RobotConstants.PERIODIC_DURATION); - } - - private double getRollerVelocityRotationsPerSecond() { - return rollerSim.getAngularVelocityRPM() / 60; - } -} diff --git a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java deleted file mode 100644 index dfba08e..0000000 --- a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java +++ /dev/null @@ -1,69 +0,0 @@ -package frc.robot.intake; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import frc.lib.Configurator; - -/** Roller motor using a TalonFX. */ -public class RollerMotorIOTalonFX implements RollerMotorIO { - - private final TalonFX frontTalonFX, backTalonFX; - private final SimpleMotorFeedforward frontFeedforward, backFeedforward; - - public RollerMotorIOTalonFX() { - frontTalonFX = new TalonFX(50); - backTalonFX = new TalonFX(40); - - frontFeedforward = new SimpleMotorFeedforward(0.13, 0.1683); - backFeedforward = new SimpleMotorFeedforward(0.13, 0.1759); - } - - @Override - public void configure() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Feedback.SensorToMechanismRatio = 24.0 / 16.0; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - config.CurrentLimits.SupplyCurrentLimit = 30; - config.CurrentLimits.SupplyCurrentThreshold = 30; - config.CurrentLimits.SupplyTimeThreshold = 0.1; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - - config.CurrentLimits.StatorCurrentLimit = 40; - config.CurrentLimits.StatorCurrentLimitEnable = true; - - Configurator.configureTalonFX(frontTalonFX.getConfigurator(), config); - - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - Configurator.configureTalonFX(backTalonFX.getConfigurator(), config); - } - - @Override - public void update(RollerMotorIOValues values) { - values.velocityRotationsPerSecond = getVelocity(); - values.currentAmps = frontTalonFX.getStatorCurrent().refresh().getValue(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double frontVolts = frontFeedforward.calculate(velocityRotationsPerSecond); - - frontTalonFX.setVoltage(frontVolts); - - double backVolts = backFeedforward.calculate(velocityRotationsPerSecond); - - backTalonFX.setVoltage(backVolts); - } - - public double getVelocity() { - return frontTalonFX.getVelocity().refresh().getValue(); - } -} diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIO.java b/src/main/java/frc/robot/shooter/FlywheelMotorIO.java deleted file mode 100644 index e73303e..0000000 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIO.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.shooter; - -/** Flywheel motor hardware interface. */ -public interface FlywheelMotorIO { - - /** Values for the flywheel motor hardware interface. */ - public static class FlywheelMotorIOValues { - /** Velocity of the flywheel in rotations per second. */ - public double velocityRotationsPerSecond = 0.0; - - /** Current drawn by the flywheel motor in amps. */ - public double currentAmps = 0.0; - } - - /** Configures the flywheel motor. */ - public void configure(); - - /** - * Updates the flywheel motor's values. - * - * @param values - */ - public void update(FlywheelMotorIOValues values); - - /** - * Sets the setpoint of the flywheel motor. - * - * @param velocityRotationsPerSecond the velocity setpoint of the flywheel motor. - */ - public void setSetpoint(double velocityRotationsPerSecond); -} diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIOSim.java b/src/main/java/frc/robot/shooter/FlywheelMotorIOSim.java deleted file mode 100644 index 23d3a39..0000000 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIOSim.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.shooter; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.RobotConstants; - -/** Simulated flywheel motor. */ -public class FlywheelMotorIOSim implements FlywheelMotorIO { - - private final FlywheelSim flywheelSim; - - private final SimpleMotorFeedforward flywheelVelocityFeedforward; - - public FlywheelMotorIOSim() { - flywheelSim = new FlywheelSim(DCMotor.getKrakenX60(1), 2.25, 0.009272); - - flywheelVelocityFeedforward = new SimpleMotorFeedforward(0.0, 0.2685); - } - - @Override - public void configure() {} - - @Override - public void update(FlywheelMotorIOValues values) { - values.velocityRotationsPerSecond = getFlywheelVelocityRotationsPerSecond(); - values.currentAmps = flywheelSim.getCurrentDrawAmps(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = flywheelVelocityFeedforward.calculate(velocityRotationsPerSecond); - - flywheelSim.setInputVoltage(volts); - - flywheelSim.update(RobotConstants.PERIODIC_DURATION); - } - - private double getFlywheelVelocityRotationsPerSecond() { - return flywheelSim.getAngularVelocityRPM() / 60; - } -} diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java deleted file mode 100644 index db42d83..0000000 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.shooter; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import frc.lib.Configurator; - -/** Flywheel motor using TalonFX. */ -public class FlywheelMotorIOTalonFX implements FlywheelMotorIO { - - private final TalonFX talonFX; - - private final StatusSignal velocityRotationsPerSecond, statorCurrentAmps; - - private final SimpleMotorFeedforward velocityFeedforward; - - public FlywheelMotorIOTalonFX() { - talonFX = new TalonFX(44); - - velocityRotationsPerSecond = talonFX.getVelocity(); - statorCurrentAmps = talonFX.getStatorCurrent(); - - velocityFeedforward = new SimpleMotorFeedforward(0.14, 0.2539); - } - - @Override - public void configure() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - config.CurrentLimits.StatorCurrentLimit = 40; - config.CurrentLimits.StatorCurrentLimitEnable = true; - - Configurator.configureTalonFX(talonFX.getConfigurator(), config); - } - - @Override - public void update(FlywheelMotorIOValues values) { - velocityRotationsPerSecond.refresh(); - statorCurrentAmps.refresh(); - - values.velocityRotationsPerSecond = velocityRotationsPerSecond.getValue(); - values.currentAmps = statorCurrentAmps.getValue(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = velocityFeedforward.calculate(velocityRotationsPerSecond); - - talonFX.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIO.java b/src/main/java/frc/robot/shooter/SerializerMotorIO.java deleted file mode 100644 index eebbda2..0000000 --- a/src/main/java/frc/robot/shooter/SerializerMotorIO.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.shooter; - -/** Serializer motor hardware interface. */ -public interface SerializerMotorIO { - - /** Values for the serializer motor hardware interface. */ - public static class SerializerMotorIOValues { - /** Velocity of the serializer in rotations per second. */ - public double velocityRotationsPerSecond = 0.0; - - /** Current drawn by the serializer motor in amps. */ - public double currentAmps = 0.0; - } - - /** Configures the serializer motor. */ - public void configure(); - - /** - * Updates the serializer motor's values. - * - * @param values - */ - public void update(SerializerMotorIOValues values); - - /** - * Sets the setpoint of the serializer motor. - * - * @param velocityRotationsPerSecond the velocity setpoint of the serializer motor. - */ - public void setSetpoint(double velocityRotationsPerSecond); -} diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOSim.java b/src/main/java/frc/robot/shooter/SerializerMotorIOSim.java deleted file mode 100644 index 351eee9..0000000 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOSim.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.shooter; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.RobotConstants; - -/** Simulated serializer motor. */ -public class SerializerMotorIOSim implements SerializerMotorIO { - - private final FlywheelSim serializerSim; - - private final SimpleMotorFeedforward serializerVelocityFeedforward; - - public SerializerMotorIOSim() { - serializerSim = new FlywheelSim(DCMotor.getKrakenX60(1), 3.0, 0.06202); - - serializerVelocityFeedforward = new SimpleMotorFeedforward(0.0, 0.358); - } - - @Override - public void configure() {} - - @Override - public void update(SerializerMotorIOValues values) { - values.velocityRotationsPerSecond = getSerializerVelocityRotationsPerSecond(); - values.currentAmps = serializerSim.getCurrentDrawAmps(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = serializerVelocityFeedforward.calculate(velocityRotationsPerSecond); - - serializerSim.setInputVoltage(volts); - - serializerSim.update(RobotConstants.PERIODIC_DURATION); - } - - private double getSerializerVelocityRotationsPerSecond() { - return serializerSim.getAngularVelocityRPM() / 60; - } -} diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java deleted file mode 100644 index 15ff9a8..0000000 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.shooter; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import frc.lib.Configurator; - -/** Serializer motor using TalonFX. */ -public class SerializerMotorIOTalonFX implements SerializerMotorIO { - - private final TalonFX talonFX; - - private final StatusSignal velocityRotationsPerSecond, statorCurrentAmps; - - private final SimpleMotorFeedforward velocityFeedforward; - - public SerializerMotorIOTalonFX() { - talonFX = new TalonFX(42); - - velocityRotationsPerSecond = talonFX.getVelocity(); - statorCurrentAmps = talonFX.getStatorCurrent(); - - velocityFeedforward = new SimpleMotorFeedforward(0.14, 0.2617); - } - - @Override - public void configure() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.CurrentLimits.StatorCurrentLimit = 40; - config.CurrentLimits.StatorCurrentLimitEnable = true; - - Configurator.configureTalonFX(talonFX.getConfigurator(), config); - } - - @Override - public void update(SerializerMotorIOValues values) { - velocityRotationsPerSecond.refresh(); - statorCurrentAmps.refresh(); - - values.velocityRotationsPerSecond = velocityRotationsPerSecond.getValue(); - values.currentAmps = statorCurrentAmps.getValue(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = velocityFeedforward.calculate(velocityRotationsPerSecond); - - talonFX.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index a38422a..5f5f6a5 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -1,11 +1,12 @@ package frc.robot.shooter; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; -import frc.lib.Telemetry; -import frc.robot.shooter.FlywheelMotorIO.FlywheelMotorIOValues; -import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues; +import frc.lib.controller.VelocityControllerIO; +import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; +import frc.robot.shooter.ShooterConstants.FlywheelConstants; +import frc.robot.shooter.ShooterConstants.SerializerConstants; /** Subsystem class for the shooter subsystem. */ public class Shooter extends Subsystem { @@ -13,25 +14,32 @@ public class Shooter extends Subsystem { /** Instance variable for the shooter subsystem singleton. */ private static Shooter instance = null; - /** Serializer motor. */ - private final SerializerMotorIO serializerMotor; + /** Serializer. */ + private final VelocityControllerIO serializer; - /** Serializer motor values. */ - private final SerializerMotorIOValues serializerMotorValues = new SerializerMotorIOValues(); + /** Serializer values. */ + private final VelocityControllerIOValues serializerValues; - /** Flywheel motor. */ - private final FlywheelMotorIO flywheelMotor; + /** Flywheel. */ + private final VelocityControllerIO flywheel; - /** Flywheel motor values. */ - private final FlywheelMotorIOValues flywheelMotorValues = new FlywheelMotorIOValues(); + /** Flywheel values. */ + private final VelocityControllerIOValues flywheelValues; + + private ShooterState setpoint, goal; /** Creates a new instance of the shooter subsystem. */ private Shooter() { - serializerMotor = ShooterFactory.createSerializerMotor(); - flywheelMotor = ShooterFactory.createFlywheelMotor(); + serializer = ShooterFactory.createSerializer(); + serializerValues = new VelocityControllerIOValues(); + serializer.configure(SerializerConstants.CONTROLLER_CONSTANTS); + + flywheel = ShooterFactory.createFlywheel(); + flywheelValues = new VelocityControllerIOValues(); + flywheel.configure(FlywheelConstants.CONTROLLER_CONSTANTS); - serializerMotor.configure(); - flywheelMotor.configure(); + setpoint = ShooterState.IDLE; + goal = ShooterState.IDLE; } /** @@ -49,40 +57,38 @@ public static Shooter getInstance() { @Override public void periodic() { - serializerMotor.update(serializerMotorValues); - flywheelMotor.update(flywheelMotorValues); - } + serializer.update(serializerValues); + flywheel.update(flywheelValues); - @Override - public void addToShuffleboard(ShuffleboardTab tab) { - ShuffleboardLayout serializer = Telemetry.addColumn(tab, "Serializer"); + setpoint = goal; - serializer.addDouble( - "Serializer Speed (rps)", () -> serializerMotorValues.velocityRotationsPerSecond); - serializer.addDouble("Serializer Current (A)", () -> serializerMotorValues.currentAmps); + double flywheelSetpoint = FlywheelConstants.ACCELERATION_LIMITER.calculate(setpoint.flywheelVelocityRotationsPerSecond()); + double serializerSetpoint = SerializerConstants.ACCELERATION_LIMITER.calculate(setpoint.serializerVelocityRotationsPerSecond()); - ShuffleboardLayout flywheel = Telemetry.addColumn(tab, "Flywheel"); + flywheel.setSetpoint(flywheelSetpoint); + serializer.setSetpoint(serializerSetpoint); + } - flywheel.addDouble( - "Flywheel Speed (rps)", () -> flywheelMotorValues.velocityRotationsPerSecond); - flywheel.addDouble("Flywheel Current (A)", () -> flywheelMotorValues.currentAmps); + @Override + public void addToShuffleboard(ShuffleboardTab tab) { + VelocityControllerIO.addToShuffleboard(tab, "Serializer", serializerValues); + VelocityControllerIO.addToShuffleboard(tab, "Flywheel", flywheelValues); } - public double getFlywheelVelocity() { - flywheelMotor.update(flywheelMotorValues); + public Trigger serializedNote() { + return new Trigger(() -> serializerValues.motorAmps > SerializerConstants.NOTE_AMPS); + } - return flywheelMotorValues.velocityRotationsPerSecond; + public ShooterState getState() { + return new ShooterState( + flywheelValues.velocityRotationsPerSecond, serializerValues.velocityRotationsPerSecond); } - public double getSerializerVelocity() { - serializerMotor.update(serializerMotorValues); - - return serializerMotorValues.velocityRotationsPerSecond; + public void setGoal(ShooterState goal) { + this.goal = goal; } - public void setSetpoint( - double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { - flywheelMotor.setSetpoint(flywheelVelocityRotationsPerSecond); - serializerMotor.setSetpoint(serializerVelocityRotationsPerSecond); + public boolean atGoal() { + return getState().at(goal); } } diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java index e42042e..1e38804 100644 --- a/src/main/java/frc/robot/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/shooter/ShooterConstants.java @@ -1,50 +1,93 @@ package frc.robot.shooter; import edu.wpi.first.math.filter.SlewRateLimiter; +import frc.lib.CAN; import frc.lib.MotionProfileCalculator; +import frc.lib.PIDFConstants; +import frc.lib.controller.VelocityControllerIO.VelocityControllerIOConstants; /** Constants for the shooter subsystem. */ public class ShooterConstants { /** Constants for the serializer motor used in the shooter subsystem. */ public static class SerializerConstants { - /** Velocity to apply while intaking in rotations per second. */ - public static final double INTAKE_VELOCITY = 34; + /** Serializer's CAN. */ + public static final CAN CAN = new CAN(42); - /** Velocity to apply while pulling in rotations per second. */ - public static final double PULL_VELOCITY = -20; + /** Serializer's PIDF constants. */ + public static final PIDFConstants PIDF = new PIDFConstants(); - /** Velocity to apply while serializing in rotations per second. */ - public static final double SERIALIZE_VELOCITY = 20; + static { + PIDF.kS = 0.14; + PIDF.kV = 0.2617; + } + + /** Serializer's controller constants. */ + public static final VelocityControllerIOConstants CONTROLLER_CONSTANTS = + new VelocityControllerIOConstants(); + + static { + CONTROLLER_CONSTANTS.ccwPositive = true; + CONTROLLER_CONSTANTS.neutralBrake = false; + CONTROLLER_CONSTANTS.sensorToMechanismRatio = 36.0 / 16.0; + } + + public static final double INTAKE_SPEED = 34; + + public static final double PULL_SPEED = -10; + + public static final double EJECT_SPEED = -44; + + public static final double SLOW_FEED_SPEED = 20; + + public static final double FAST_FEED_SPEED = 44; /** Maximum speed in rotations per second. */ public static final double MAXIMUM_SPEED = 45.319; - /** Speed tolerance in rotations per second. */ - public static final double SPEED_TOLERANCE = 2.5; + public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1)); + + public static final double NOTE_AMPS = 20; } /** Constants for the flywheel motor used in the shooter subsystem. */ public static class FlywheelConstants { - /** Velocity to apply while shooting into the speaker in rotations per second. */ - public static final double SPEAKER_VELOCITY = 44; + /** Flywheel's CAN. */ + public static final CAN CAN = new CAN(44); - /** Velocity to apply while passing in rotations per second. */ - public static final double PASS_VELOCITY = 12; + /** Flywheel's PIDF constants. */ + public static final PIDFConstants PIDF = new PIDFConstants(); - /** Velocity to apply while shooting into the amp in rotations per second. */ - public static final double AMP_VELOCITY = 20; + static { + PIDF.kS = 0.14; + PIDF.kV = 0.2; + } - /** Maximum speed in rotations per second. */ - public static final double MAXIMUM_SPEED = 46.711; + /** Flywheel's controller constants. */ + public static final VelocityControllerIOConstants CONTROLLER_CONSTANTS = + new VelocityControllerIOConstants(); + + static { + CONTROLLER_CONSTANTS.ccwPositive = false; + CONTROLLER_CONSTANTS.neutralBrake = true; + CONTROLLER_CONSTANTS.sensorToMechanismRatio = 28.0 / 16.0; + } + + public static final double PULL_SPEED = -20; + + public static final double SPEAKER_SPEED = 60; - /** Maximum acceleration in rotations per second per second. */ - public static final double MAXIMUM_ACCELERATION = MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25); + public static final double PODIUM_SPEED = 60; - /** Acceleration limiter. */ - public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(MAXIMUM_ACCELERATION); + public static final double LOB_SPEED = 60; + + public static final double SKIM_SPEED = 60; + + public static final double AMP_SPEED = 20; + + /** Maximum speed in rotations per second. */ + public static final double MAXIMUM_SPEED = 60; - /** Speed tolerance in rotations per second. */ - public static final double SPEED_TOLERANCE = 2.5; + public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1)); } } diff --git a/src/main/java/frc/robot/shooter/ShooterFactory.java b/src/main/java/frc/robot/shooter/ShooterFactory.java index 95a43bd..27eb8ba 100644 --- a/src/main/java/frc/robot/shooter/ShooterFactory.java +++ b/src/main/java/frc/robot/shooter/ShooterFactory.java @@ -1,33 +1,40 @@ package frc.robot.shooter; +import frc.lib.controller.VelocityControllerIO; +import frc.lib.controller.VelocityControllerIOSim; +import frc.lib.controller.VelocityControllerIOTalonFXPIDF; import frc.robot.Robot; import frc.robot.RobotConstants; import frc.robot.RobotConstants.Subsystem; +import frc.robot.shooter.ShooterConstants.FlywheelConstants; +import frc.robot.shooter.ShooterConstants.SerializerConstants; /** Helper class for creating hardware for the shooter subsystem. */ public class ShooterFactory { /** - * Creates a serializer motor. + * Creates a serializer. * - * @return a serializer motor. + * @return a serializer. */ - public static SerializerMotorIO createSerializerMotor() { - if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) - return new SerializerMotorIOTalonFX(); + public static VelocityControllerIO createSerializer() { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) { + return new VelocityControllerIOTalonFXPIDF(SerializerConstants.CAN, SerializerConstants.PIDF); + } - return new SerializerMotorIOSim(); + return new VelocityControllerIOSim(); } /** - * Creates a flywheel motor. + * Creates a flywheel. * - * @return a flywheel motor. + * @return a flywheel. */ - public static FlywheelMotorIO createFlywheelMotor() { - if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) - return new FlywheelMotorIOTalonFX(); + public static VelocityControllerIO createFlywheel() { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) { + return new VelocityControllerIOTalonFXPIDF(FlywheelConstants.CAN, FlywheelConstants.PIDF); + } - return new FlywheelMotorIOSim(); + return new VelocityControllerIOSim(); } } diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java new file mode 100644 index 0000000..4c52d56 --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -0,0 +1,49 @@ +package frc.robot.shooter; + +import edu.wpi.first.math.MathUtil; +import frc.robot.shooter.ShooterConstants.FlywheelConstants; +import frc.robot.shooter.ShooterConstants.SerializerConstants; + +import java.util.Objects; + +public record ShooterState( + double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { + + public static final ShooterState IDLE = new ShooterState(0, 0); + + public static final ShooterState INTAKE = new ShooterState(0, SerializerConstants.INTAKE_SPEED); + + public static final ShooterState PULL = new ShooterState(FlywheelConstants.PULL_SPEED, SerializerConstants.PULL_SPEED); + + public static final ShooterState EJECT = new ShooterState(0, SerializerConstants.EJECT_SPEED); + + public static final ShooterState SPEAKER_READY = new ShooterState(FlywheelConstants.SPEAKER_SPEED, 0); + + public static final ShooterState SPEAKER_SHOOT = new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED); + + public static final ShooterState PODIUM_READY = new ShooterState(FlywheelConstants.PODIUM_SPEED, 0); + + public static final ShooterState PODIUM_SHOOT = new ShooterState(FlywheelConstants.PODIUM_SPEED, SerializerConstants.FAST_FEED_SPEED); + + public static final ShooterState LOB_READY = new ShooterState(FlywheelConstants.LOB_SPEED, 0); + + public static final ShooterState LOB_SHOOT = new ShooterState(FlywheelConstants.LOB_SPEED, SerializerConstants.FAST_FEED_SPEED); + + public static final ShooterState SKIM_READY = new ShooterState(FlywheelConstants.SKIM_SPEED, 0); + + public static final ShooterState SKIM_SHOOT = new ShooterState(FlywheelConstants.SKIM_SPEED, SerializerConstants.FAST_FEED_SPEED); + + public static final ShooterState AMP_SHOOT = new ShooterState(FlywheelConstants.AMP_SPEED, SerializerConstants.SLOW_FEED_SPEED); + + public ShooterState { + Objects.requireNonNull(flywheelVelocityRotationsPerSecond); + Objects.requireNonNull(serializerVelocityRotationsPerSecond); + } + + public boolean at(ShooterState other) { + return MathUtil.isNear( + flywheelVelocityRotationsPerSecond, other.flywheelVelocityRotationsPerSecond, 5) + && MathUtil.isNear( + serializerVelocityRotationsPerSecond, other.serializerVelocityRotationsPerSecond, 5); + } +} diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 0ea5cd0..3dbfcf0 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -1,6 +1,5 @@ package frc.robot.superstructure; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -11,6 +10,7 @@ import frc.robot.arm.Arm; import frc.robot.intake.Intake; import frc.robot.shooter.Shooter; +import java.util.function.Supplier; /** Subsystem class for the superstructure subsystem. */ public class Superstructure extends Subsystem { @@ -27,7 +27,7 @@ public class Superstructure extends Subsystem { /** Reference to the shooter subsystem. */ private final Shooter shooter; - private SuperstructureState measurement, setpoint, goal; + private SuperstructureState measurement, goal; /** Creates a new instance of the superstructure subsystem. */ private Superstructure() { @@ -37,7 +37,6 @@ private Superstructure() { setPosition(SuperstructureState.INITIAL); - setpoint = SuperstructureState.INITIAL; goal = SuperstructureState.INITIAL; } @@ -57,115 +56,72 @@ public static Superstructure getInstance() { @Override public void periodic() { updateMeasurement(); - updateSetpoint(); } @Override public void addToShuffleboard(ShuffleboardTab tab) { - ShuffleboardLayout measurement = Telemetry.addColumn(tab, "Measurement"); + addStateToShuffleboard(tab, "Measurement", () -> measurement); + addStateToShuffleboard(tab, "Goal", () -> goal); - measurement.addDouble( - "Shoulder Position (deg)", - () -> Units.rotationsToDegrees(this.measurement.shoulderAngleRotations().position)); - measurement.addDouble( - "Shoulder Velocity (dps)", - () -> Units.rotationsToDegrees(this.measurement.shoulderAngleRotations().velocity)); + ShuffleboardLayout state = Telemetry.addColumn(tab, "State"); - measurement.addDouble( - "Roller Velocity (rps)", () -> this.measurement.rollerVelocityRotationsPerSecond()); + state.addString( + "State", + () -> this.getCurrentCommand() != null ? this.getCurrentCommand().getName() : "NONE"); - measurement.addDouble( - "Flywheel Velocity (rps)", () -> this.measurement.flywheelVelocityRotationsPerSecond()); + ShuffleboardLayout at = Telemetry.addColumn(tab, "At Goal?"); - measurement.addDouble( - "Serializer Velocity (rps)", () -> this.measurement.serializerVelocityRotationsPerSecond()); + at.addBoolean("At Goal?", () -> at(goal)); + } - ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint"); + private void addStateToShuffleboard( + ShuffleboardTab tab, String name, Supplier state) { + ShuffleboardLayout layout = Telemetry.addColumn(tab, name); - setpoint.addDouble( + layout.addDouble( "Shoulder Position (deg)", - () -> Units.rotationsToDegrees(this.setpoint.shoulderAngleRotations().position)); - setpoint.addDouble( + () -> Units.rotationsToDegrees(state.get().armState().shoulderRotations().position)); + layout.addDouble( "Shoulder Velocity (dps)", - () -> Units.rotationsToDegrees(this.setpoint.shoulderAngleRotations().velocity)); - - setpoint.addDouble( - "Roller Velocity (rps)", () -> this.setpoint.rollerVelocityRotationsPerSecond()); - - setpoint.addDouble( - "Flywheel Velocity (rps)", () -> this.setpoint.flywheelVelocityRotationsPerSecond()); - - setpoint.addDouble( - "Serializer Velocity (rps)", () -> this.setpoint.serializerVelocityRotationsPerSecond()); - - ShuffleboardLayout goal = Telemetry.addColumn(tab, "Goal"); + () -> Units.rotationsToDegrees(state.get().armState().shoulderRotations().velocity)); - goal.addDouble( - "Shoulder Position (deg)", - () -> Units.rotationsToDegrees(this.goal.shoulderAngleRotations().position)); - goal.addDouble( - "Shoulder Velocity (dps)", - () -> Units.rotationsToDegrees(this.goal.shoulderAngleRotations().velocity)); + layout.addDouble( + "Front Roller Velocity (rps)", + () -> state.get().intakeState().frontRollerVelocityRotationsPerSecond()); - goal.addDouble("Roller Velocity (rps)", () -> this.goal.rollerVelocityRotationsPerSecond()); + layout.addDouble( + "Back Roller Velocity (rps)", + () -> state.get().intakeState().backRollerVelocityRotationsPerSecond()); - goal.addDouble("Flywheel Velocity (rps)", () -> this.goal.flywheelVelocityRotationsPerSecond()); + layout.addDouble( + "Flywheel Velocity (rps)", + () -> state.get().shooterState().flywheelVelocityRotationsPerSecond()); - goal.addDouble( - "Serializer Velocity (rps)", () -> this.goal.serializerVelocityRotationsPerSecond()); + layout.addDouble( + "Serializer Velocity (rps)", + () -> state.get().shooterState().serializerVelocityRotationsPerSecond()); } private void updateMeasurement() { - State measuredShoulderState = arm.getMeasuredShoulderState(); - - double measuredIntakeRollerVelocity = intake.getRollerVelocity(); - - double measuredShooterFlywheelVelocity = shooter.getFlywheelVelocity(); - double measuredShooterSerializerVelocity = shooter.getSerializerVelocity(); - - measurement = - new SuperstructureState( - measuredShoulderState, - false, - measuredIntakeRollerVelocity, - measuredShooterFlywheelVelocity, - false, - measuredShooterSerializerVelocity); + measurement = new SuperstructureState(arm.getState(), intake.getState(), shooter.getState()); SuperstructureMechanism.getInstance().updateSuperstructure(measurement); } public SuperstructureState getState() { - updateMeasurement(); - return measurement; } - public void setSetpoint(SuperstructureState setpoint) { - this.setpoint = setpoint; - } - - private void updateSetpoint() { - setpoint = SuperstructureState.nextSetpoint(setpoint, goal); - - if (setpoint.shoulderManual() == false) { - arm.setSetpoint( - setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); - } - - shooter.setSetpoint( - setpoint.flywheelVelocityRotationsPerSecond(), - setpoint.serializerVelocityRotationsPerSecond()); - - intake.setSetpoint(setpoint.rollerVelocityRotationsPerSecond()); - } - public void setPosition(SuperstructureState state) { - arm.setShoulderPosition(state.shoulderAngleRotations().position); + arm.setPosition(state.armState()); } public void setGoal(SuperstructureState goal) { this.goal = goal; + + arm.setGoal(goal.armState()); + intake.setGoal(goal.intakeState()); + shooter.setGoal(goal.shooterState()); } public boolean at(SuperstructureState goal) { @@ -174,47 +130,68 @@ public boolean at(SuperstructureState goal) { return measurement.atGoal(goal); } + private Command hold(SuperstructureState goal) { + return run(() -> setGoal(goal)); + } + private Command to(SuperstructureState goal) { - return run(() -> setGoal(goal)).until(() -> at(goal)).raceWith(Commands.waitSeconds(1.0)); + return run(() -> setGoal(goal)).until(() -> at(goal)); } public Command stow() { - return to(SuperstructureState.STOW); + return hold(SuperstructureState.STOW).withName("STOW"); } public Command intake() { - return to(SuperstructureState.INTAKE); + return to(SuperstructureState.STOW) + .andThen(hold(SuperstructureState.INTAKE)) + .withName("INTAKE"); } - public Command idle() { - return to(SuperstructureState.SPEAKER_SPIN); + public Command subwoofer() { + return hold(SuperstructureState.SUBWOOFER_PULL) + .withTimeout(SuperstructureConstants.PULL_DURATION) + .andThen(to(SuperstructureState.SUBWOOFER_READY)) + .andThen(hold(SuperstructureState.SUBWOOFER_SHOOT)) + .withName("SPEAKER"); } - public Command pull() { - return Commands.deadline(Commands.waitSeconds(0.15), to(SuperstructureState.PULL)); + public Command podium() { + return hold(SuperstructureState.PODIUM_PULL) + .withTimeout(SuperstructureConstants.PULL_DURATION) + .andThen(to(SuperstructureState.PODIUM_READY)) + .andThen(hold(SuperstructureState.PODIUM_SHOOT)) + .withName("PODIUM"); } - public Command shoot() { - return pull().andThen(to(SuperstructureState.SPEAKER_SPIN).andThen(to(SuperstructureState.SPEAKER_SHOOT))); + public Command lob() { + return hold(SuperstructureState.LOB_PULL) + .withTimeout(SuperstructureConstants.PULL_DURATION) + .andThen(to(SuperstructureState.LOB_READY)) + .andThen(hold(SuperstructureState.LOB_SHOOT)) + .withName("LOB"); } - public Command pass() { - return pull().andThen(to(SuperstructureState.PASS_SPIN).andThen(to(SuperstructureState.PASS_SHOOT))); + public Command skim() { + return hold(SuperstructureState.SKIM_PULL) + .withTimeout(SuperstructureConstants.PULL_DURATION) + .andThen(to(SuperstructureState.SKIM_READY)) + .andThen(hold(SuperstructureState.SKIM_SHOOT)) + .withName("SKIM"); } - public Command ampPosition() { - return to(SuperstructureState.AMP_POSITION); + public Command amp() { + return hold(SuperstructureState.AMP_PULL) + .withTimeout(SuperstructureConstants.PULL_DURATION) + .andThen(to(SuperstructureState.AMP_POSITION)) + .andThen(hold(SuperstructureState.AMP_SHOOT)) + .withName("AMP"); } - public Command ampShoot() { - return ampPosition().andThen(to(SuperstructureState.AMP_SHOOT)); - } - public Command eject() { - return to(SuperstructureState.EJECT_POSITION).andThen(to(SuperstructureState.EJECT)); - } - - public Command manualControl() { - return to(SuperstructureState.MANUAL); + return to(SuperstructureState.EJECT_POSITION) + .andThen(Commands.waitSeconds(SuperstructureConstants.EJECT_PAUSE)) + .andThen(hold(SuperstructureState.EJECT)) + .withName("EJECT"); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index d8a9506..cd2ef46 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -1,38 +1,9 @@ package frc.robot.superstructure; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import frc.lib.MotionProfileCalculator; - public class SuperstructureConstants { - public static class ShoulderAngleConstants { - public static final Rotation2d INITIAL = Rotation2d.fromDegrees(-26.45); - - public static final Rotation2d STOW = Rotation2d.fromDegrees(-26.45); - - public static final Rotation2d SHOOT = Rotation2d.fromDegrees(-15); - - public static final Rotation2d EJECT = Rotation2d.fromDegrees(0); - - public static final Rotation2d AMP = Rotation2d.fromDegrees(90); - - /** Tolerance of the shoulder joint. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - - /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0); - - /** Maximum acceleration of the shoulder joint in rotations per second per second. */ - public static final double MAXIMUM_ACCELERATION = - MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); + public static final double PULL_DURATION = 0.15; - /** Maximum speed and acceleration of the shoulder joint. */ - public static final TrapezoidProfile.Constraints CONSTRAINTS = - new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); + public static final double EJECT_PAUSE = 0.25; - /** Motion profile of the shoulder joint using constraints. */ - public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); - } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java deleted file mode 100644 index 910ad5b..0000000 --- a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.superstructure; - -import java.util.LinkedList; -import java.util.Queue; - -public class SuperstructureGoals { - - private final Queue goals; - - public static Queue generate( - SuperstructureState start, SuperstructureState end) { - Queue goals = new LinkedList(); - - goals.add(end); - - return goals; - } - - public SuperstructureGoals(Queue goals) { - this.goals = goals; - } - - public SuperstructureGoals(SuperstructureState start, SuperstructureState end) { - this(generate(start, end)); - } - - public SuperstructureState get() { - return goals.element(); - } - - public SuperstructureState next() { - boolean hasNext = goals.size() > 1; - - if (hasNext) { - goals.remove(); - } - - return get(); - } -} diff --git a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java index c813511..b5b1f82 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import frc.lib.InterpolatableColor; import frc.robot.RobotConstants; -import frc.robot.intake.IntakeConstants.RollerConstants; +import frc.robot.intake.IntakeConstants.FrontRollerConstants; import frc.robot.shooter.ShooterConstants.FlywheelConstants; import frc.robot.shooter.ShooterConstants.SerializerConstants; @@ -132,7 +132,8 @@ public Mechanism2d getMechanism() { } public void updateSuperstructure(SuperstructureState state) { - Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position); + Rotation2d shoulderRotation = + Rotation2d.fromRotations(state.armState().shoulderRotations().position); Rotation2d offsetShoulderRotation = shoulderRotation.minus(Rotation2d.fromDegrees(90)); @@ -141,22 +142,25 @@ public void updateSuperstructure(SuperstructureState state) { flywheel.setColor( new Color8Bit( FLYWHEEL_COLOR.sample( - Math.abs(state.flywheelVelocityRotationsPerSecond()), + Math.abs(state.shooterState().flywheelVelocityRotationsPerSecond()), 0, FlywheelConstants.MAXIMUM_SPEED))); serializer.setColor( new Color8Bit( SERIALIZER_COLOR.sample( - Math.abs(state.serializerVelocityRotationsPerSecond()), + Math.abs(state.shooterState().serializerVelocityRotationsPerSecond()), 0, SerializerConstants.MAXIMUM_SPEED))); + double averageRollerVelocity = + (state.intakeState().frontRollerVelocityRotationsPerSecond() + + state.intakeState().backRollerVelocityRotationsPerSecond()) + / 2; + rollers.setColor( new Color8Bit( ROLLERS_COLOR.sample( - Math.abs(state.rollerVelocityRotationsPerSecond()), - 0, - RollerConstants.MAXIMUM_SPEED))); + Math.abs(averageRollerVelocity), 0, FrontRollerConstants.MAXIMUM_SPEED))); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 354a7e0..b473fd5 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -1,187 +1,88 @@ package frc.robot.superstructure; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import frc.robot.RobotConstants; -import frc.robot.intake.IntakeConstants.RollerConstants; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; -import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; +import frc.robot.arm.Arm; +import frc.robot.arm.ArmState; +import frc.robot.intake.Intake; +import frc.robot.intake.IntakeState; +import frc.robot.shooter.Shooter; +import frc.robot.shooter.ShooterState; import java.util.Objects; /** Represents the state of the superstructure. */ public record SuperstructureState( - State shoulderAngleRotations, - boolean shoulderManual, - double rollerVelocityRotationsPerSecond, - double flywheelVelocityRotationsPerSecond, - boolean rampFlywheelVelocity, - double serializerVelocityRotationsPerSecond) { + ArmState armState, IntakeState intakeState, ShooterState shooterState) { public static final SuperstructureState INITIAL = - new SuperstructureState(ShoulderAngleConstants.INITIAL, false, 0, 0, false, 0); + new SuperstructureState(ArmState.INITIAL, IntakeState.IDLE, ShooterState.IDLE); public static final SuperstructureState STOW = - new SuperstructureState(ShoulderAngleConstants.STOW, false, 0, 0, false, 0); - - public static final SuperstructureState INTAKE_POSITION = - new SuperstructureState(ShoulderAngleConstants.STOW, false, 0, 0, false, 0); + new SuperstructureState(ArmState.STOW, IntakeState.IDLE, ShooterState.IDLE); public static final SuperstructureState INTAKE = - new SuperstructureState( - ShoulderAngleConstants.STOW, - false, - RollerConstants.INTAKE_VELOCITY, - 0, - false, - SerializerConstants.INTAKE_VELOCITY); - - public static final SuperstructureState PULL = new SuperstructureState(ShoulderAngleConstants.STOW, false, 0, 0, false, SerializerConstants.PULL_VELOCITY); - - public static final SuperstructureState EJECT_POSITION = new SuperstructureState(ShoulderAngleConstants.EJECT, false, 0, 0, false, 0); - - public static final SuperstructureState EJECT = new SuperstructureState(ShoulderAngleConstants.EJECT, false, 0, 0, false, SerializerConstants.PULL_VELOCITY); - - public static final SuperstructureState SPEAKER_SPIN = - new SuperstructureState( - ShoulderAngleConstants.SHOOT, false, 0, FlywheelConstants.SPEAKER_VELOCITY, true, 0); - - public static final SuperstructureState SPEAKER_SHOOT = - new SuperstructureState( - ShoulderAngleConstants.SHOOT, - false, - 0, - FlywheelConstants.SPEAKER_VELOCITY, - false, - SerializerConstants.SERIALIZE_VELOCITY); - - public static final SuperstructureState PASS_SPIN = - new SuperstructureState( - ShoulderAngleConstants.STOW, false, 0, FlywheelConstants.PASS_VELOCITY, false, 0); - - public static final SuperstructureState PASS_SHOOT = - new SuperstructureState( - ShoulderAngleConstants.STOW, - false, - 0, - FlywheelConstants.PASS_VELOCITY, - false, - SerializerConstants.SERIALIZE_VELOCITY); + new SuperstructureState(ArmState.STOW, IntakeState.INTAKE, ShooterState.INTAKE); - public static final SuperstructureState AMP_POSITION = - new SuperstructureState(ShoulderAngleConstants.AMP, false, 0, 0, false, 0); + public static final SuperstructureState EJECT_POSITION = + new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.IDLE); - public static final SuperstructureState AMP_SPIN = - new SuperstructureState( - ShoulderAngleConstants.AMP, false, 0, FlywheelConstants.AMP_VELOCITY, false, 0); + public static final SuperstructureState EJECT = + new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.EJECT); - public static final SuperstructureState AMP_SHOOT = - new SuperstructureState( - ShoulderAngleConstants.AMP, - false, - 0, - FlywheelConstants.AMP_VELOCITY, - false, - SerializerConstants.SERIALIZE_VELOCITY); + public static final SuperstructureState SUBWOOFER_PULL = + new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.PULL); - public static final SuperstructureState MANUAL = new SuperstructureState(ShoulderAngleConstants.STOW, true, 0, 0, false, 0); + public static final SuperstructureState SUBWOOFER_READY = + new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SPEAKER_READY); - /** - * Creates a new superstructure state. - * - * @param shoulderAngleRotations - * @param shoulderManual - * @param rollerVelocityRotationsPerSecond - * @param flywheelVelocityRotationsPerSecond - * @param rampFlywheelVelocity - * @param serializerVelocityRotationsPerSecond - */ - public SuperstructureState { - Objects.requireNonNull(shoulderAngleRotations); - Objects.requireNonNull(shoulderManual); - Objects.requireNonNull(rollerVelocityRotationsPerSecond); - Objects.requireNonNull(flywheelVelocityRotationsPerSecond); - Objects.requireNonNull(rampFlywheelVelocity); - Objects.requireNonNull(serializerVelocityRotationsPerSecond); - } + public static final SuperstructureState SUBWOOFER_SHOOT = + new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SPEAKER_SHOOT); - /** - * Creates a new superstructure state. - * - * @param shoulderAngle - * @param shoulderManual - * @param rollerVelocityRotationsPerSecond - * @param flywheelVelocityRotationsPerSecond - * @param rampFlywheelVelocity - * @param serializerVelocityRotationsPerSecond - */ - public SuperstructureState( - Rotation2d shoulderAngle, - boolean shoulderManual, - double rollerVelocityRotationsPerSecond, - double flywheelVelocityRotationsPerSecond, - boolean rampFlywheelVelocity, - double serializerVelocityRotationsPerSecond) { - this( - new State(shoulderAngle.getRotations(), 0), - shoulderManual, - rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - rampFlywheelVelocity, - serializerVelocityRotationsPerSecond); - } + public static final SuperstructureState PODIUM_PULL = + new SuperstructureState(ArmState.PODIUM, IntakeState.IDLE, ShooterState.PULL); - /** - * Returns true if at the shoulder angle goal. - * - * @param goal - * @return true if at the shoulder angle goal. - */ - public boolean atShoulderAngleGoal(SuperstructureState goal) { - return MathUtil.isNear( - this.shoulderAngleRotations().position, - goal.shoulderAngleRotations().position, - SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations()); - } + public static final SuperstructureState PODIUM_READY = + new SuperstructureState(ArmState.PODIUM, IntakeState.IDLE, ShooterState.PODIUM_READY); - /** - * Returns true if at the roller velocity goal. - * - * @param goal - * @return true if at the roller velocity goal. - */ - public boolean atRollerVelocityGoal(SuperstructureState goal) { - return MathUtil.isNear( - this.rollerVelocityRotationsPerSecond(), - goal.rollerVelocityRotationsPerSecond(), - RollerConstants.SPEED_TOLERANCE); - } + public static final SuperstructureState PODIUM_SHOOT = + new SuperstructureState(ArmState.PODIUM, IntakeState.IDLE, ShooterState.PODIUM_SHOOT); - /** - * Returns true if at the flywheel velocity goal. - * - * @param goal - * @return true if at the flywheel velocity goal. - */ - public boolean atFlywheelVelocityGoal(SuperstructureState goal) { - return MathUtil.isNear( - this.flywheelVelocityRotationsPerSecond(), - goal.flywheelVelocityRotationsPerSecond(), - FlywheelConstants.SPEED_TOLERANCE); - } + public static final SuperstructureState LOB_PULL = + new SuperstructureState(ArmState.LOB, IntakeState.IDLE, ShooterState.PULL); + + public static final SuperstructureState LOB_READY = + new SuperstructureState(ArmState.LOB, IntakeState.IDLE, ShooterState.LOB_READY); + + public static final SuperstructureState LOB_SHOOT = + new SuperstructureState(ArmState.LOB, IntakeState.IDLE, ShooterState.LOB_SHOOT); + + public static final SuperstructureState SKIM_PULL = + new SuperstructureState(ArmState.SKIM, IntakeState.IDLE, ShooterState.PULL); + + public static final SuperstructureState SKIM_READY = + new SuperstructureState(ArmState.SKIM, IntakeState.IDLE, ShooterState.SKIM_READY); + + public static final SuperstructureState SKIM_SHOOT = + new SuperstructureState(ArmState.SKIM, IntakeState.IDLE, ShooterState.SKIM_SHOOT); + + public static final SuperstructureState AMP_PULL = + new SuperstructureState(ArmState.AMP, IntakeState.IDLE, ShooterState.PULL); + + public static final SuperstructureState AMP_POSITION = + new SuperstructureState(ArmState.AMP, IntakeState.IDLE, ShooterState.IDLE); + + public static final SuperstructureState AMP_SHOOT = + new SuperstructureState(ArmState.AMP, IntakeState.IDLE, ShooterState.AMP_SHOOT); /** - * Returns true if at the serializer velocity goal. + * Creates a new superstructure state. * - * @param goal - * @return true if at the serializer velocity goal. + * @param armState + * @param intakeState + * @param shooterState */ - public boolean atSerializerVelocityGoal(SuperstructureState goal) { - return MathUtil.isNear( - this.serializerVelocityRotationsPerSecond(), - goal.serializerVelocityRotationsPerSecond(), - SerializerConstants.SPEED_TOLERANCE); + public SuperstructureState { + Objects.requireNonNull(armState); + Objects.requireNonNull(intakeState); + Objects.requireNonNull(shooterState); } /** @@ -191,41 +92,8 @@ public boolean atSerializerVelocityGoal(SuperstructureState goal) { * @return true if at the superstructure goal. */ public boolean atGoal(SuperstructureState goal) { - return atShoulderAngleGoal(goal) - && atRollerVelocityGoal(goal) - && atFlywheelVelocityGoal(goal) - && atSerializerVelocityGoal(goal); - } - - /** - * Calculates the next setpoint. - * - * @param setpoint the previous setpoint. - * @param goal the goal. - * @return the next setpoint. - */ - public static SuperstructureState nextSetpoint( - SuperstructureState setpoint, SuperstructureState goal) { - State nextShoulderSetpoint = - ShoulderAngleConstants.MOTION_PROFILE.calculate( - RobotConstants.PERIODIC_DURATION, - setpoint.shoulderAngleRotations(), - goal.shoulderAngleRotations()); - - double accelerationLimitedFlywheelVelocity = - FlywheelConstants.ACCELERATION_LIMITER.calculate(goal.flywheelVelocityRotationsPerSecond()); - - double nextFlywheelVelocitySetpoint = - goal.rampFlywheelVelocity() - ? accelerationLimitedFlywheelVelocity - : goal.flywheelVelocityRotationsPerSecond(); - - return new SuperstructureState( - nextShoulderSetpoint, - goal.shoulderManual(), - goal.rollerVelocityRotationsPerSecond(), - nextFlywheelVelocitySetpoint, - goal.rampFlywheelVelocity(), - goal.serializerVelocityRotationsPerSecond()); + return Arm.getInstance().atGoal() + && Intake.getInstance().atGoal() + && Shooter.getInstance().atGoal(); } } diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 50c6f89..a059539 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -79,7 +79,7 @@ public static class MK4iConstants { /** Module configuration for the south east swerve module. */ public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( - new SwerveModuleCAN(22,12, 26, SWERVE_BUS), + new SwerveModuleCAN(22, 12, 26, SWERVE_BUS), new Translation2d(-X_OFFSET, -Y_OFFSET), Rotation2d.fromRotations(0.273438)); @@ -161,12 +161,9 @@ private static double calculateKv(double voltsPerRotorRotationPerSecond) { public static final PIDFConstants STEER_PIDF_CONSTANTS = new PIDFConstants(); static { - STEER_PIDF_CONSTANTS.kP = 48.0; // volts per rotation - STEER_PIDF_CONSTANTS.kD = 0.25; // volts per rotation per second - STEER_PIDF_CONSTANTS.kPositionTolerance = Units.degreesToRotations(3); - // STEER_PIDF_CONSTANTS.kVelocityConstraint = 10.0; // rotations per second - // STEER_PIDF_CONSTANTS.kAccelerationConstraint = 64.0; // rotations per second per second - STEER_PIDF_CONSTANTS.kS = 0.32; - // STEER_PIDF_CONSTANTS.kV = 0.407363; // volts per rotation per second + STEER_PIDF_CONSTANTS.kP = 54.0; // volts per rotation + STEER_PIDF_CONSTANTS.kD = 0.16; // volts per rotation per second 0.25 + STEER_PIDF_CONSTANTS.kPositionTolerance = Units.degreesToRotations(0); + STEER_PIDF_CONSTANTS.kS = 0.205; } }