diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c549d19..73917d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,7 +88,7 @@ private void configureBindings() { operatorController.leftBumper().onTrue(superstructure.eject()); operatorController.leftTrigger().onTrue(superstructure.intake()); - operatorController.rightBumper().onTrue(superstructure.podium()); + operatorController.rightBumper().onTrue(superstructure.skim()); operatorController.rightTrigger().onTrue(superstructure.subwoofer()); operatorController.a().onTrue(superstructure.amp()); diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 40cd1c4..f481a44 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -10,34 +10,39 @@ import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; import frc.robot.arm.ArmConstants.ShoulderConstants; -/** Subsystem class for the arm subsystem. */ +/** Arm subsystem. */ public class Arm extends Subsystem { /** Instance variable for the arm subsystem singleton. */ private static Arm instance = null; - /** Shoulder. */ + /** Shoulder position controller. */ private final PositionControllerIO shoulder; - /** Shoulder values. */ + /** Shoulder position controller values. */ private final PositionControllerIOValues shoulderValues; - private double lastTimeSeconds; + /** Arm's goal. Set by superstructure to use as end goal for setpoints. */ + private ArmState goal; - /** Arm goal. */ - private ArmState setpoint, goal; + /** Arm's setpoint. Updated periodically to reach the goal within constraints. */ + private ArmState setpoint; - /** Creates a new instance of the arm subsystem. */ + /** Time of the start of the previous periodic call. Used for calculating the time elapsed since the previous setpoint was generated. */ + private double previousTimeSeconds; + + /** Creates a new arm subsystem and configures arm hardware. */ private Arm() { shoulder = ArmFactory.createShoulder(); - shoulderValues = new PositionControllerIOValues(); shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS); - lastTimeSeconds = Timer.getFPGATimestamp(); + shoulderValues = new PositionControllerIOValues(); + + previousTimeSeconds = Timer.getFPGATimestamp(); - setPosition(ArmState.INITIAL); - setpoint = ArmState.INITIAL; - goal = ArmState.INITIAL; + setPosition(ArmState.STOW_POSITION); + setpoint = ArmState.STOW_POSITION; + goal = ArmState.STOW_POSITION; } /** @@ -59,17 +64,18 @@ public void periodic() { double timeSeconds = Timer.getFPGATimestamp(); - double dt = timeSeconds - lastTimeSeconds; - - lastTimeSeconds = timeSeconds; + // Calculate the time elapsed in seconds since the previous setpoint was generated + double timeElapsedSeconds = timeSeconds - previousTimeSeconds; setpoint = new ArmState( ShoulderConstants.MOTION_PROFILE.calculate( - dt, setpoint.shoulderRotations(), goal.shoulderRotations())); + timeElapsedSeconds, setpoint.shoulderRotations(), goal.shoulderRotations())); shoulder.setSetpoint( setpoint.shoulderRotations().position, setpoint.shoulderRotations().velocity); + + previousTimeSeconds = timeSeconds; } @Override @@ -82,21 +88,41 @@ public void addToShuffleboard(ShuffleboardTab tab) { () -> Units.rotationsToDegrees(setpoint.shoulderRotations().position)); } + /** + * Returns the arm's state. + * + * @return the arm's state. + */ public ArmState getState() { return new ArmState( new TrapezoidProfile.State( shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond)); } + /** + * Sets the arm's goal state. + * + * @param goal the arm's goal state. + */ public void setGoal(ArmState goal) { this.goal = goal; } + /** + * Returns true if at the arm's set goal. + * + * @return true if at the arm's set goal. + */ public boolean atGoal() { return getState().at(goal); } - public void setPosition(ArmState armState) { + /** + * Sets the position of the shoulder motor encoders. + * + * @param armState the position. + */ + private 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 dc425ea..0513ad2 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -25,10 +25,10 @@ public static class ShoulderConstants { public static final PIDFConstants PIDF = new PIDFConstants(); static { - PIDF.kS = 0.14; - PIDF.kG = 0.5125; - PIDF.kV = 4.0; - PIDF.kP = 4.0; + PIDF.kS = 0.14; // volts + PIDF.kG = 0.5125; // volts + PIDF.kV = 4.0; // volts per rotation per second + PIDF.kP = 4.0; // volts per rotation } /** Shoulder's controller constants. */ @@ -36,10 +36,24 @@ public static class ShoulderConstants { new PositionControllerIOConstants(); static { + // The leading shoulder motor has the correct reference frame CONTROLLER_CONSTANTS.ccwPositiveMotor = true; + + // The shoulder's absolute encoder has the wrong reference frame CONTROLLER_CONSTANTS.ccwPositiveAbsoluteEncoder = false; + + // Use brake mode to hold the arm in place + // Since the arm rests on a hard stop this isn't strictly necessary, + // but it prevents the arm from being knocked around if disabled CONTROLLER_CONSTANTS.neutralBrake = true; + + // Ask CAD if they can calculate this reduction or count the gears CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571; + + // 1. Rest the arm in the stow position + // 2. Use a digital level to determine the actual angle of the stow position + // 3. Observe the value reported by the absolute encoder while the arm is in the stow position + // 4. Calculate the absolute encoder offset by subtracting the absolute encoder value from the actual angle CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-173.135); } @@ -57,20 +71,10 @@ public static class ShoulderConstants { /** 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(-26); - - 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(60); + /** Shoulder angle when the arm is stowed. */ + public static final Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(-26); - public static final Rotation2d BLOOP = Rotation2d.fromDegrees(-26); + /** Shoulder angle when the shooter is parallel to the ground. Used for flat shots. */ + public static final Rotation2d FLAT_ANGLE = Rotation2d.fromDegrees(30); } } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index 6690f46..cde536b 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -7,25 +7,17 @@ import frc.robot.arm.ArmConstants.ShoulderConstants; import java.util.Objects; +/** Represents an arm's state */ public record ArmState(State shoulderRotations) { - public static final ArmState INITIAL = new ArmState(ShoulderConstants.STOW); + /** State for stow position. */ + public static final ArmState STOW_POSITION = new ArmState(ShoulderConstants.STOW_ANGLE); - public static final ArmState STOW = new ArmState(ShoulderConstants.STOW); + /** State for flat position. */ + public static final ArmState FLAT_POSITION = new ArmState(ShoulderConstants.FLAT_ANGLE); - 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 static final ArmState BLOOP = new ArmState(ShoulderConstants.BLOOP); + /** State for amp position. */ + public static final ArmState AMP_POSITION = new ArmState(Rotation2d.fromDegrees(60)); public ArmState { Objects.requireNonNull(shoulderRotations); diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index e91b342..faf98b0 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -5,8 +5,6 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -17,8 +15,6 @@ import frc.robot.superstructure.Superstructure; import frc.robot.swerve.Swerve; import frc.robot.swerve.SwerveConstants; -import java.util.function.Consumer; -import java.util.function.Supplier; /** Subsystem class for the auto subsystem. */ public class Auto extends Subsystem { @@ -44,40 +40,38 @@ private Auto() { superstructure = Superstructure.getInstance(); swerve = Swerve.getInstance(); - Supplier robotPositionSupplier = () -> odometry.getPosition(); - - Consumer robotPositionConsumer = position -> odometry.setPosition(position); - - Supplier swerveChassisSpeedsSupplier = () -> swerve.getChassisSpeeds(); - - Consumer swerveChassisSpeedsConsumer = - chassisSpeeds -> swerve.setChassisSpeeds(chassisSpeeds); - - HolonomicPathFollowerConfig holonomicPathFollowerConfig = + AutoBuilder.configureHolonomic( + odometry::getPosition, + odometry::setPosition, + swerve::getChassisSpeeds, + swerve::setChassisSpeeds, + // TODO Move some of these constants to AutoConstants new HolonomicPathFollowerConfig( new PIDConstants(1.0), new PIDConstants(1.0), SwerveConstants.MAXIMUM_ATTAINABLE_SPEED, SwerveConstants.NORTH_EAST_MODULE_CONFIG.position().getNorm(), - new ReplanningConfig()); - - AutoBuilder.configureHolonomic( - robotPositionSupplier, - robotPositionConsumer, - swerveChassisSpeedsSupplier, - swerveChassisSpeedsConsumer, - holonomicPathFollowerConfig, + new ReplanningConfig()), AllianceFlipHelper::shouldFlip, swerve); + registerNamedCommands(); + + autoChooser = AutoBuilder.buildAutoChooser(); + } + + /** + * Registers PathPlanner named commands. + */ + private void registerNamedCommands() { NamedCommands.registerCommand("stow", superstructure.stow()); NamedCommands.registerCommand( - "shoot", superstructure.subwoofer().withTimeout(1.5)); // 1 second could work - NamedCommands.registerCommand( - "shootNoPull", superstructure.subwooferNoPull().withTimeout(1.5)); // 1 second could work + "shoot", superstructure.subwoofer().withTimeout(1.5)); NamedCommands.registerCommand("intake", superstructure.intakeInstant()); - autoChooser = AutoBuilder.buildAutoChooser(); + // TODO Deprecate after competition + NamedCommands.registerCommand( + "shootNoPull", superstructure.subwooferNoPull().withTimeout(1.5)); } /** @@ -93,9 +87,6 @@ public static Auto getInstance() { return instance; } - @Override - public void periodic() {} - @Override public void addToShuffleboard(ShuffleboardTab tab) { Telemetry.makeFullscreen(tab.add("Auto Chooser", autoChooser)); diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java index 3f54e3d..48835d2 100644 --- a/src/main/java/frc/robot/shooter/ShooterState.java +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -20,12 +20,6 @@ public record ShooterState( public static final ShooterState SUBWOOFER = new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED); - public static final ShooterState PODIUM = - new ShooterState(FlywheelConstants.PODIUM_SPEED, SerializerConstants.FAST_FEED_SPEED); - - public static final ShooterState LOB = - new ShooterState(FlywheelConstants.LOB_SPEED, SerializerConstants.FAST_FEED_SPEED); - public static final ShooterState SKIM = new ShooterState(FlywheelConstants.SKIM_SPEED, SerializerConstants.FAST_FEED_SPEED); diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index f287d28..0892b51 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -37,8 +37,6 @@ private Superstructure() { intake = Intake.getInstance(); shooter = Shooter.getInstance(); - setPosition(SuperstructureState.STOW); - goal = SuperstructureState.STOW; } @@ -103,10 +101,6 @@ private void addStateToShuffleboard( () -> state.get().shooterState().serializerVelocityRotationsPerSecond()); } - public void setPosition(SuperstructureState state) { - arm.setPosition(state.armState()); - } - public void setGoal(SuperstructureState goal) { this.goal = goal; @@ -183,14 +177,6 @@ public Command subwoofer() { return shoot(SuperstructureState.SUBWOOFER).withName("SUBWOOFER"); } - public Command podium() { - return shoot(SuperstructureState.PODIUM).withName("PODIUM"); - } - - public Command lob() { - return shoot(SuperstructureState.LOB).withName("LOB"); - } - public Command skim() { return shoot(SuperstructureState.SKIM).withName("SKIM"); } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 995c078..1755137 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -10,34 +10,28 @@ public record SuperstructureState( ArmState armState, IntakeState intakeState, ShooterState shooterState) { public static final SuperstructureState STOW = - new SuperstructureState(ArmState.STOW, IntakeState.IDLE, ShooterState.IDLE); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLE, ShooterState.IDLE); public static final SuperstructureState INTAKE = - new SuperstructureState(ArmState.STOW, IntakeState.INTAKE, ShooterState.INTAKE); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.INTAKE, ShooterState.INTAKE); public static final SuperstructureState EJECT_POSITION = - new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.IDLE); + new SuperstructureState(ArmState.FLAT_POSITION, IntakeState.EJECT, ShooterState.IDLE); public static final SuperstructureState EJECT = - new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.EJECT); + new SuperstructureState(ArmState.FLAT_POSITION, IntakeState.EJECT, ShooterState.EJECT); public static final SuperstructureState SUBWOOFER = - new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SUBWOOFER); - - public static final SuperstructureState PODIUM = - new SuperstructureState(ArmState.PODIUM, IntakeState.IDLE, ShooterState.PODIUM); - - public static final SuperstructureState LOB = - new SuperstructureState(ArmState.LOB, IntakeState.IDLE, ShooterState.LOB); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLE, ShooterState.SUBWOOFER); public static final SuperstructureState SKIM = - new SuperstructureState(ArmState.SKIM, IntakeState.IDLE, ShooterState.SKIM); + new SuperstructureState(ArmState.FLAT_POSITION, IntakeState.IDLE, ShooterState.SKIM); public static final SuperstructureState AMP = - new SuperstructureState(ArmState.AMP, IntakeState.IDLE, ShooterState.AMP); + new SuperstructureState(ArmState.AMP_POSITION, IntakeState.IDLE, ShooterState.AMP); public static final SuperstructureState BLOOP = - new SuperstructureState(ArmState.BLOOP, IntakeState.IDLE, ShooterState.BLOOP); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLE, ShooterState.BLOOP); /** * Creates a new superstructure state.