diff --git a/simgui.json b/simgui.json index 9f0a0e0..6d31540 100644 --- a/simgui.json +++ b/simgui.json @@ -19,6 +19,7 @@ "/SmartDashboard/Arm Mechanism": "Mechanism2d", "/SmartDashboard/Mechanism": "Mechanism2d", "/SmartDashboard/SendableChooser[0]": "String Chooser", + "/SmartDashboard/Superstructure": "Mechanism2d", "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d", "/SmartDashboard/arm mech": "Mechanism2d" }, @@ -33,7 +34,7 @@ "visible": true } }, - "/SmartDashboard/Mechanism": { + "/SmartDashboard/Superstructure": { "window": { "visible": true } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 03f7376..dff5b3f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,9 +82,7 @@ private void configureBindings() { operatorController.rightTrigger().onTrue(superstructure.shoot()); operatorController.a().onTrue(superstructure.amp()); - operatorController.b().onTrue(superstructure.intake()); operatorController.x().onTrue(superstructure.stow()); - operatorController.y().onTrue(superstructure.shoot()); } /** diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 3168dbd..e978f6e 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -76,7 +76,7 @@ public void setShoulderPosition(double positionRotations) { shoulderMotor.setPosition(positionRotations); } - public void setShoulderSetpoint(double positionRotations, double velocityRotationsPerSecond) { + public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond); } } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index c826288..3c4e813 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -2,11 +2,8 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.robot.intake.IntakeConstants.RollerMotorConstants; import frc.robot.intake.RollerMotorIO.RollerMotorIOValues; /** Subsystem class for the intake subsystem. */ @@ -60,25 +57,7 @@ public double getRollerVelocity() { return rollerMotorValues.velocityRotationsPerSecond; } - /** - * Returns a command that intakes using the rollers. - * - * @return a command that intakes using the rollers. - */ - public Command intake() { - return Commands.runEnd( - () -> rollerMotor.setSetpoint(RollerMotorConstants.INTAKE_VELOCITY), - () -> rollerMotor.setSetpoint(0.0)); - } - - /** - * Returns a command that outtakes using the rollers. - * - * @return a command that outtakes using the rollers. - */ - public Command outtake() { - return Commands.runEnd( - () -> rollerMotor.setSetpoint(RollerMotorConstants.OUTTAKE_VELOCITY), - () -> rollerMotor.setSetpoint(0.0)); + public void setSetpoint(double rollerVelocityRotationsPerSecond) { + rollerMotor.setSetpoint(rollerVelocityRotationsPerSecond); } } diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index a685658..bd3ed32 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -5,7 +5,7 @@ /** Constants for the intake subsystem. */ public class IntakeConstants { /** Constants for the roller motor. */ - public static class RollerMotorConstants { + public static class RollerConstants { /** Velocity to apply when intaking in rotations per second. */ public static final double INTAKE_VELOCITY = 1.0; diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 754d9fb..293ca34 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -2,14 +2,10 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.shooter.FlywheelMotorIO.FlywheelMotorIOValues; import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; /** Subsystem class for the shooter subsystem. */ public class Shooter extends Subsystem { @@ -81,36 +77,9 @@ public double getSerializerVelocity() { return serializerMotorValues.velocityRotationsPerSecond; } - /** - * Intakes a note. - * - * @return a command that intakes a note. - */ - public Command intake() { - return Commands.runEnd( - () -> serializerMotor.setSetpoint(SerializerConstants.INTAKE_VELOCITY), - () -> serializerMotor.setSetpoint(0.0)); - } - - /** - * Serializes a note. - * - * @return a command that serializes a note. - */ - public Command serialize() { - return Commands.runEnd( - () -> serializerMotor.setSetpoint(SerializerConstants.SERIALIZE_VELOCITY), - () -> serializerMotor.setSetpoint(0.0)); - } - - /** - * Spins the flywheel. - * - * @return a command that spins the flywheel. - */ - public Command spin() { - return Commands.runEnd( - () -> flywheelMotor.setSetpoint(FlywheelConstants.SHOOT_VELOCITY), - () -> flywheelMotor.setSetpoint(0.0)); + public void setSetpoint( + double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { + flywheelMotor.setSetpoint(flywheelVelocityRotationsPerSecond); + serializerMotor.setSetpoint(serializerVelocityRotationsPerSecond); } } diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java index 273ec3c..9cfe1c9 100644 --- a/src/main/java/frc/robot/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/shooter/ShooterConstants.java @@ -8,19 +8,25 @@ 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 in rotations per second. */ - public static final double SERIALIZE_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; - /** Maximum tengential speed in meters per second. */ - public static final double MAXIMUM_TANGENTIAL_SPEED = 4.75; + /** Velocity to apply while serializing for an amp shot in rotations per second. */ + public static final double AMP_SERIALIZE_VELOCITY = 2.5; + + /** Maximum tengential speed in rotations per second. */ + public static final double MAXIMUM_SPEED = 4.75; } /** Constants for the flywheel motor used in the shooter subsystem. */ public static class FlywheelConstants { - /** Velocity to apply while shooting in rotations per second. */ - public static final double SHOOT_VELOCITY = 5.65; + /** Velocity to apply while shooting into the speaker in rotations per second. */ + public static final double SPEAKER_VELOCITY = 5.65; + + /** Velocity to apply while shooting into the amp in rotations per second. */ + public static final double AMP_VELOCITY = 2.5; - /** Maximum tangential speed in meters per second. */ - public static final double MAXIMUM_TANGENTIAL_SPEED = 5.65; + /** Maximum tangential speed in rotations per second. */ + public static final double MAXIMUM_SPEED = 5.65; } } diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index d9264d1..856d3b8 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -9,7 +9,10 @@ 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 { @@ -141,8 +144,14 @@ public SuperstructureState getState() { private void updateSetpoint() { setpoint = SuperstructureState.nextSetpoint(setpoint, goal); - arm.setShoulderSetpoint( + arm.setSetpoint( setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); + + shooter.setSetpoint( + setpoint.flywheelVelocityRotationsPerSecond(), + setpoint.serializerVelocityRotationsPerSecond()); + + intake.setSetpoint(setpoint.rollerVelocityRotationsPerSecond()); } public void setPosition(SuperstructureState state) { @@ -151,41 +160,46 @@ public void setPosition(SuperstructureState state) { public void setGoal(SuperstructureState goal) { this.goal = goal; - - // resetMotionProfile(); } - // private void resetMotionProfile() { - // setpoint = measurement; - // } - public boolean at(SuperstructureState goal) { updateMeasurement(); return measurement.at(goal); } - public Command to(SuperstructureState goal) { + private Command to(SuperstructureState goal) { return new ToGoal(goal); } - public Command initial() { - return to(SuperstructureState.INITIAL); - } - public Command stow() { return to(SuperstructureState.STOW); } public Command intake() { - return to(SuperstructureState.INTAKE); + return to(SuperstructureState.INTAKE) + .andThen( + to( + SuperstructureState.INTAKE + .withSerializerVelocity(SerializerConstants.INTAKE_VELOCITY) + .withRollerVelocity(RollerConstants.INTAKE_VELOCITY))); } public Command shoot() { - return to(SuperstructureState.SHOOT); + return to(SuperstructureState.SHOOT) + .andThen( + to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY)) + .withTimeout(1.0)) + .andThen( + to( + SuperstructureState.SHOOT + .withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY) + .withSerializerVelocity(SerializerConstants.SPEAKER_SERIALIZER_VELOCITY))); } public Command amp() { - return to(SuperstructureState.AMP); + return to(SuperstructureState.AMP) + .andThen(to(SuperstructureState.AMP_SHOOT).withTimeout(1.0)) + .andThen(to(SuperstructureState.AMP_SERIALIZE)); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java index b5e5441..d86684b 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java @@ -11,6 +11,7 @@ import frc.lib.InterpolatableColor; import frc.robot.RobotConstants; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; +import frc.robot.intake.IntakeConstants.RollerConstants; import frc.robot.shooter.ShooterConstants.FlywheelConstants; import frc.robot.shooter.ShooterConstants.SerializerConstants; @@ -21,7 +22,7 @@ public class SuperstructureMechanism { private final Mechanism2d mechanism; - private MechanismLigament2d shoulder, flywheel, serializer; + private MechanismLigament2d shoulder, flywheel, serializer, rollers; private final double WIDTH = 2 @@ -46,22 +47,23 @@ public class SuperstructureMechanism { private final double SERIALIZER_LENGTH = Units.inchesToMeters(7.5); + private final Translation2d ORIGIN_TO_ROLLERS = + new Translation2d(Units.inchesToMeters(5.75), Units.inchesToMeters(2.5)); + private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray); - private final InterpolatableColor flywheelColor = + private final InterpolatableColor FLYWHEEL_COLOR = new InterpolatableColor(Color.kLightGray, Color.kSalmon); - private final InterpolatableColor serializerColor = + + private final InterpolatableColor SERIALIZER_COLOR = new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue); + private final InterpolatableColor ROLLERS_COLOR = + new InterpolatableColor(Color.kLightGray, Color.kSpringGreen); + private SuperstructureMechanism() { mechanism = new Mechanism2d(WIDTH, HEIGHT); - initializeArmMechanism(); - - initializeFramePerimeterMechanisms(); - } - - private void initializeArmMechanism() { Translation2d armRootTranslation = ORIGIN.plus(ORIGIN_TO_SHOULDER_BASE); double armThickness = Units.inchesToMeters(2) * 100; @@ -91,9 +93,16 @@ private void initializeArmMechanism() { shoulder.append( new MechanismLigament2d( "serializer", SERIALIZER_LENGTH, -35, armThickness, DEFAULT_COLOR)); - } - private void initializeFramePerimeterMechanisms() { + Translation2d rollersRootTranslation = ORIGIN.plus(ORIGIN_TO_ROLLERS); + + rollers = + mechanism + .getRoot("intake", rollersRootTranslation.getX(), rollersRootTranslation.getY()) + .append( + new MechanismLigament2d( + "rollers", Units.inchesToMeters(12), 0, armThickness, DEFAULT_COLOR)); + double framePerimeterThickness = Units.inchesToMeters(1) * 10; mechanism @@ -136,16 +145,23 @@ public void updateSuperstructure(SuperstructureState state) { flywheel.setColor( new Color8Bit( - flywheelColor.sample( + FLYWHEEL_COLOR.sample( Math.abs(state.flywheelVelocityRotationsPerSecond()), 0, - FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED))); + FlywheelConstants.MAXIMUM_SPEED))); serializer.setColor( new Color8Bit( - serializerColor.sample( + SERIALIZER_COLOR.sample( Math.abs(state.serializerVelocityRotationsPerSecond()), 0, - SerializerConstants.MAXIMUM_TANGENTIAL_SPEED))); + SerializerConstants.MAXIMUM_SPEED))); + + rollers.setColor( + new Color8Bit( + ROLLERS_COLOR.sample( + Math.abs(state.rollerVelocityRotationsPerSecond()), + 0, + RollerConstants.MAXIMUM_SPEED))); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index c73fc14..7b349e9 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import frc.robot.RobotConstants; +import frc.robot.shooter.ShooterConstants.FlywheelConstants; +import frc.robot.shooter.ShooterConstants.SerializerConstants; import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; import java.util.Objects; @@ -28,6 +30,12 @@ public record SuperstructureState( public static final SuperstructureState AMP = new SuperstructureState(ShoulderAngleConstants.AMP); + public static final SuperstructureState AMP_SHOOT = + AMP.withFlywheelVelocity(FlywheelConstants.AMP_VELOCITY); + + public static final SuperstructureState AMP_SERIALIZE = + AMP_SHOOT.withSerializerVelocity(SerializerConstants.AMP_SERIALIZE_VELOCITY); + /** * Creates a new superstructure state. *