diff --git a/simgui.json b/simgui.json index b891371..159d4d1 100644 --- a/simgui.json +++ b/simgui.json @@ -15,6 +15,10 @@ "/Shuffleboard/Dashboard/Auto/Auto Chooser": "String Chooser", "/Shuffleboard/Dashboard/Auto/SendableChooser[0]": "String Chooser", "/Shuffleboard/Odometry/Field/Field": "Field2d", + "/Shuffleboard/Swerve/Module 0/Sync Steer": "Command", + "/Shuffleboard/Swerve/Module 1/Sync Steer": "Command", + "/Shuffleboard/Swerve/Module 2/Sync Steer": "Command", + "/Shuffleboard/Swerve/Module 3/Sync Steer": "Command", "/Shuffleboard/Vision/Field": "Field2d", "/SmartDashboard/Arm Mechanism": "Mechanism2d", "/SmartDashboard/Mechanism": "Mechanism2d", @@ -188,7 +192,7 @@ 0.0, 0.8500000238418579 ], - "height": 0, + "height": 338, "series": [ { "color": [ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 701131b..3908a47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,13 +78,14 @@ private void configureDefaultCommands() { private void configureBindings() { driverController.y().onTrue(odometry.tare()); + operatorController.leftBumper().onTrue(superstructure.stow()); operatorController.leftTrigger().onTrue(superstructure.intake()); - operatorController.rightBumper().onTrue(superstructure.spin()); + operatorController.rightBumper().onTrue(superstructure.idle()); operatorController.rightTrigger().onTrue(superstructure.shoot()); - operatorController.a().onTrue(superstructure.amp()); - operatorController.x().onTrue(superstructure.stow()); + operatorController.a().onTrue(superstructure.ampPosition()); + operatorController.b().onTrue(superstructure.ampShoot()); } /** diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java index 9c0a072..681f4e2 100644 --- a/src/main/java/frc/robot/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/shooter/ShooterConstants.java @@ -1,5 +1,7 @@ package frc.robot.shooter; +import edu.wpi.first.math.filter.SlewRateLimiter; + /** Constants for the shooter subsystem. */ public class ShooterConstants { @@ -8,11 +10,8 @@ public static class SerializerConstants { /** Velocity to apply while intaking in rotations per second. */ public static final double INTAKE_VELOCITY = 4.75; - /** Velocity to apply while serializing for a speaker shot in rotations per second. */ - public static final double SPEAKER_SERIALIZER_VELOCITY = 4.75; - - /** Velocity to apply while serializing for an amp shot in rotations per second. */ - public static final double AMP_SERIALIZE_VELOCITY = 2.5; + /** Velocity to apply while serializing in rotations per second. */ + public static final double SERIALIZE_VELOCITY = 2.5; /** Maximum speed in rotations per second. */ public static final double MAXIMUM_SPEED = 4.75; @@ -23,6 +22,9 @@ public static class SerializerConstants { /** Constants for the flywheel motor used in the shooter subsystem. */ public static class FlywheelConstants { + /** Velocity to apply while idly spinning up in rotations per second. */ + public static final double IDLE_VELOCITY = 2.5; + /** Velocity to apply while shooting into the speaker in rotations per second. */ public static final double SPEAKER_VELOCITY = 5.65; @@ -34,5 +36,8 @@ public static class FlywheelConstants { /** Speed tolerance in rotations per second. */ public static final double SPEED_TOLERANCE = 0.25; + + /** Acceleration limiter. */ + public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(1.0); } } diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 06ccd03..760f483 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -9,10 +9,7 @@ import frc.lib.Telemetry; import frc.robot.arm.Arm; import frc.robot.intake.Intake; -import frc.robot.intake.IntakeConstants.RollerConstants; import frc.robot.shooter.Shooter; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; /** Subsystem class for the superstructure subsystem. */ public class Superstructure extends Subsystem { @@ -37,10 +34,10 @@ private Superstructure() { intake = Intake.getInstance(); shooter = Shooter.getInstance(); - setPosition(SuperstructureState.INITIAL); + setPosition(SuperstructureState.STOW); - setpoint = SuperstructureState.INITIAL; - goal = SuperstructureState.INITIAL; + setpoint = SuperstructureState.STOW; + goal = SuperstructureState.STOW; } /** @@ -130,6 +127,7 @@ private void updateMeasurement() { measuredShoulderState, measuredIntakeRollerVelocity, measuredShooterFlywheelVelocity, + false, measuredShooterSerializerVelocity); SuperstructureMechanism.getInstance().updateSuperstructure(measurement); @@ -165,7 +163,7 @@ public void setGoal(SuperstructureState goal) { public boolean at(SuperstructureState goal) { updateMeasurement(); - return measurement.at(goal); + return measurement.atGoal(goal); } private Command to(SuperstructureState goal) { @@ -177,30 +175,26 @@ public Command stow() { } public Command intake() { - return to(SuperstructureState.INTAKE) + return to(SuperstructureState.INTAKE_POSITION) .andThen( - to( - SuperstructureState.INTAKE - .withSerializerVelocity(SerializerConstants.INTAKE_VELOCITY) - .withRollerVelocity(RollerConstants.INTAKE_VELOCITY))); + to(SuperstructureState.INTAKE)); } - public Command spin() { - return to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY)); + public Command idle() { + return to(SuperstructureState.IDLE); } public Command shoot() { - return to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY)) + return to(SuperstructureState.SPEAKER_SPIN) .andThen( - to( - SuperstructureState.SHOOT - .withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY) - .withSerializerVelocity(SerializerConstants.SPEAKER_SERIALIZER_VELOCITY))); + to(SuperstructureState.SPEAKER_SHOOT)); } - public Command amp() { - return to(SuperstructureState.AMP) - .andThen(to(SuperstructureState.AMP_SHOOT).withTimeout(1.0)) - .andThen(to(SuperstructureState.AMP_SERIALIZE)); + public Command ampPosition() { + return to(SuperstructureState.AMP_POSITION); + } + + public Command ampShoot() { + return ampPosition().andThen(to(SuperstructureState.AMP_SHOOT)); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 6d79b0b..6e7a1c3 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -15,40 +15,44 @@ public record SuperstructureState( State shoulderAngleRotations, double rollerVelocityRotationsPerSecond, double flywheelVelocityRotationsPerSecond, + boolean rampFlywheelVelocity, double serializerVelocityRotationsPerSecond) { - public static final SuperstructureState INITIAL = - new SuperstructureState(ShoulderAngleConstants.INITIAL); - public static final SuperstructureState STOW = - new SuperstructureState(ShoulderAngleConstants.STOW); + new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, 0); + + public static final SuperstructureState INTAKE_POSITION = new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, 0); public static final SuperstructureState INTAKE = - new SuperstructureState(ShoulderAngleConstants.STOW); + new SuperstructureState(ShoulderAngleConstants.STOW, RollerConstants.INTAKE_VELOCITY, 0, false, SerializerConstants.INTAKE_VELOCITY); + + public static final SuperstructureState IDLE = + new SuperstructureState(ShoulderAngleConstants.STOW, 0, FlywheelConstants.IDLE_VELOCITY, true, 0); + + public static final SuperstructureState SPEAKER_SPIN = new SuperstructureState(ShoulderAngleConstants.STOW, 0, FlywheelConstants.SPEAKER_VELOCITY, false, 0); - public static final SuperstructureState SHOOT = - new SuperstructureState(ShoulderAngleConstants.STOW); + public static final SuperstructureState SPEAKER_SHOOT = new SuperstructureState(ShoulderAngleConstants.STOW, 0, FlywheelConstants.SPEAKER_VELOCITY, false, SerializerConstants.SERIALIZE_VELOCITY); - public static final SuperstructureState AMP = new SuperstructureState(ShoulderAngleConstants.AMP); + public static final SuperstructureState AMP_POSITION = new SuperstructureState(ShoulderAngleConstants.AMP, 0, 0, false, 0); - public static final SuperstructureState AMP_SHOOT = - AMP.withFlywheelVelocity(FlywheelConstants.AMP_VELOCITY); + public static final SuperstructureState AMP_SPIN = new SuperstructureState(ShoulderAngleConstants.AMP, 0, FlywheelConstants.AMP_VELOCITY, false, 0); - public static final SuperstructureState AMP_SERIALIZE = - AMP_SHOOT.withSerializerVelocity(SerializerConstants.AMP_SERIALIZE_VELOCITY); + public static final SuperstructureState AMP_SHOOT = new SuperstructureState(ShoulderAngleConstants.AMP, 0, FlywheelConstants.AMP_VELOCITY, false, SerializerConstants.SERIALIZE_VELOCITY); /** * Creates a new superstructure state. * * @param shoulderAngleRotations - * @param intakeRollerVelocityRotationsPerSecond + * @param rollerVelocityRotationsPerSecond * @param flywheelVelocityRotationsPerSecond + * @param rampFlywheelVelocity * @param serializerVelocityRotationsPerSecond */ public SuperstructureState { Objects.requireNonNull(shoulderAngleRotations); Objects.requireNonNull(rollerVelocityRotationsPerSecond); Objects.requireNonNull(flywheelVelocityRotationsPerSecond); + Objects.requireNonNull(rampFlywheelVelocity); Objects.requireNonNull(serializerVelocityRotationsPerSecond); } @@ -56,98 +60,25 @@ public record SuperstructureState( * Creates a new superstructure state. * * @param shoulderAngle - * @param wristAngle * @param rollerVelocityRotationsPerSecond * @param flywheelVelocityRotationsPerSecond + * @param rampFlywheelVelocity * @param serializerVelocityRotationsPerSecond */ public SuperstructureState( Rotation2d shoulderAngle, double rollerVelocityRotationsPerSecond, double flywheelVelocityRotationsPerSecond, + boolean rampFlywheelVelocity, double serializerVelocityRotationsPerSecond) { this( - new State(shoulderAngle.getRotations(), 0.0), - rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); - } - - /** - * Creates a new superstructure state. - * - * @param shoulderAngle - * @param wristAngle - */ - public SuperstructureState(Rotation2d shoulderAngle) { - this(shoulderAngle, 0.0, 0.0, 0.0); - } - - public SuperstructureState withShoulderAngle(Rotation2d newShoulderAngle) { - return new SuperstructureState( - new State(newShoulderAngle.getRotations(), 0.0), + new State(shoulderAngle.getRotations(), 0), rollerVelocityRotationsPerSecond, flywheelVelocityRotationsPerSecond, + rampFlywheelVelocity, serializerVelocityRotationsPerSecond); } - public SuperstructureState withShoulderAngleOf(SuperstructureState other) { - return new SuperstructureState( - other.shoulderAngleRotations, - rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); - } - - public SuperstructureState withRollerVelocity(double newRollerVelocityRotationsPerSecond) { - return new SuperstructureState( - shoulderAngleRotations, - newRollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); - } - - public SuperstructureState withRollerVelocityOf(SuperstructureState other) { - return new SuperstructureState( - shoulderAngleRotations, - other.rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); - } - - public SuperstructureState withFlywheelVelocity(double newFlywheelVelocityRotationsPerSecond) { - return new SuperstructureState( - shoulderAngleRotations, - rollerVelocityRotationsPerSecond, - newFlywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); - } - - public SuperstructureState withFlywheelVelocityOf(SuperstructureState other) { - return new SuperstructureState( - shoulderAngleRotations, - rollerVelocityRotationsPerSecond, - other.flywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); - } - - public SuperstructureState withSerializerVelocity( - double newSerializerVelocityRotationsPerSecond) { - return new SuperstructureState( - shoulderAngleRotations, - rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - newSerializerVelocityRotationsPerSecond); - } - - public SuperstructureState withSerializerVelocityOf(SuperstructureState other) { - return new SuperstructureState( - shoulderAngleRotations, - rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - other.serializerVelocityRotationsPerSecond); - } - /** * Returns true if at the shoulder angle goal. * @@ -200,11 +131,17 @@ public boolean atSerializerVelocityGoal(SuperstructureState goal) { SerializerConstants.SPEED_TOLERANCE); } - public boolean at(SuperstructureState other) { - return atShoulderAngleGoal(other) - && atRollerVelocityGoal(other) - && atFlywheelVelocityGoal(other) - && atSerializerVelocityGoal(other); + /** + * Returns true if at the superstructure goal. + * + * @param goal + * @return true if at the superstructure goal. + */ + public boolean atGoal(SuperstructureState goal) { + return atShoulderAngleGoal(goal) + && atRollerVelocityGoal(goal) + && atFlywheelVelocityGoal(goal) + && atSerializerVelocityGoal(goal); } /** @@ -222,10 +159,15 @@ public static SuperstructureState nextSetpoint( setpoint.shoulderAngleRotations(), goal.shoulderAngleRotations()); + double accelerationLimitedFlywheelVelocity = FlywheelConstants.ACCELERATION_LIMITER.calculate(goal.flywheelVelocityRotationsPerSecond()); + + double nextFlywheelVelocitySetpoint = goal.rampFlywheelVelocity() ? accelerationLimitedFlywheelVelocity : goal.flywheelVelocityRotationsPerSecond(); + return new SuperstructureState( nextShoulderSetpoint, goal.rollerVelocityRotationsPerSecond(), - goal.flywheelVelocityRotationsPerSecond(), + nextFlywheelVelocitySetpoint, + goal.rampFlywheelVelocity(), goal.serializerVelocityRotationsPerSecond()); } }