Cleanup

.gitattributes

@@ -0,0 +1,2 @@
+src/main/deploy/** linguist-language=JSON linguist-generated=true
+**/*.java               text=auto

.gitignore

@@ -162,7 +162,7 @@ bin/
 *.iml
 *.ipr
 *.iws
-.idea/
+
 out/
 
 # Fleet

src/main/java/org/team1540/robot2024/Constants.java

@@ -41,7 +41,7 @@ public enum Mode {
     public static final double LOOP_PERIOD_SECS = 0.02;
 
     public static class SwerveConfig {
-        public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "" : "";
+        public static final String CAN_BUS  = IS_COMPETITION_ROBOT ? "" : "";
         public static final int FRONT_LEFT  = IS_COMPETITION_ROBOT ? 3 : 0;
         public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
         public static final int BACK_LEFT   = IS_COMPETITION_ROBOT ? 7 : 0;
@@ -63,6 +63,7 @@ public static class Drivetrain {
         public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
         public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
     }
+
     public static class Indexer {
         // TODO: fix these constants
         public static final int INTAKE_ID = 11;
@@ -166,7 +167,7 @@ public static class Elevator {
         public static final double ELEVATOR_MAX_HEIGHT = ELEVATOR_MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate
         public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT;
 
-        public static final double GEAR_RATIO = 2.0 / 1.0; //TODO: Get constants right sometime
+        public static final double GEAR_RATIO = 2.0; //TODO: Get constants right sometime
         public static final int LEADER_ID = -1;
         public static final int FOLLOWER_ID = -1;
         public static final double KS = 0.25;
@@ -182,7 +183,7 @@ public static class Elevator {
         public static final double SPROCKET_RADIUS_M = 0.022;
         public static final double SPROCKET_CIRCUMFERENCE_M = 2 * SPROCKET_RADIUS_M * Math.PI;
         public static final double MOTOR_ROTS_TO_METERS = GEAR_RATIO * SPROCKET_CIRCUMFERENCE_M;
-        public static final double ERROR_TOLERANCE = 0.03;
+        public static final double POS_ERR_TOLERANCE_METERS = 0.03;
         public static final double SIM_CARRIAGE_MASS_KG = 1.55; //TODO: check this number :)
 
         public enum ElevatorState {
@@ -208,14 +209,15 @@ public enum ElevatorState {
             AMP(27.0); //TODO: Find these values :D
 
             public final double heightMeters;
+
             ElevatorState(double heightMeters) {
                 this.heightMeters = heightMeters;
             }
         }
     }
 
     public static class Tramp {
-        public static final double GEAR_RATIO = 3.0 / 1.0;
+        public static final double GEAR_RATIO = 3.0;
         public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
         public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later
         public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -8,35 +8,36 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.IntakeCommand;
+import org.team1540.robot2024.commands.shooter.ShootSequence;
+import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIOSim;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIOTalonFX;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.indexer.IndexerIO;
+import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
+import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
+import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.tramp.TrampIO;
 import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
 import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
-import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.subsystems.vision.AprilTagVision;
 import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
 import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
 import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
-import org.team1540.robot2024.subsystems.indexer.Indexer;
-import org.team1540.robot2024.subsystems.indexer.IndexerIO;
-import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
-import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
 
 import static org.team1540.robot2024.Constants.SwerveConfig;
@@ -66,8 +67,6 @@ public class RobotContainer {
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
     // TODO: testing dashboard inputs, remove for comp
-    public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
-    public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -138,13 +137,10 @@ public RobotContainer() {
                                 new AprilTagVisionIO() {},
                                 (ignored) -> {},
                                 () -> 0.0,
-                                new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));
-
-                indexer =
-                        new Indexer(
-                                new IndexerIO() {}
+                                new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)
                         );
 
+                indexer = new Indexer(new IndexerIO() {});
                 tramp = new Tramp(new TrampIO() {});
 
                 break;
@@ -187,7 +183,7 @@ private void configureButtonBindings() {
                 ).ignoringDisable(true)
         );
 
-        copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
+        copilot.a().onTrue(new ShootSequence(shooter, indexer))
                 .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
 
         driver.a().onTrue(new IntakeCommand(indexer));

src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java

@@ -76,7 +76,7 @@ public void add(double velocity, double voltage) {
         }
 
         public void print() {
-            if (velocityData.size() == 0 || voltageData.size() == 0) {
+            if (velocityData.isEmpty() || voltageData.isEmpty()) {
                 return;
             }
 
@@ -87,10 +87,10 @@ public void print() {
                             1);
 
             System.out.println("FF Characterization Results:");
-            System.out.println("\tCount=" + Integer.toString(velocityData.size()) + "");
-            System.out.println(String.format("\tR2=%.5f", regression.R2()));
-            System.out.println(String.format("\tkS=%.5f", regression.beta(0)));
-            System.out.println(String.format("\tkV=%.5f", regression.beta(1)));
+            System.out.println("\tCount=" + velocityData.size());
+            System.out.printf("\tR2=%.5f%n", regression.R2());
+            System.out.printf("\tkS=%.5f%n", regression.beta(0));
+            System.out.printf("\tkV=%.5f%n", regression.beta(1));
         }
     }
 }

src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java

@@ -27,9 +27,9 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle
 
     @Override
     public void initialize() {
-       xLimiter.reset(0);
-       yLimiter.reset(0);
-       rotLimiter.reset(0);
+        xLimiter.reset(0);
+        yLimiter.reset(0);
+        rotLimiter.reset(0);
     }
 
     @Override
@@ -46,7 +46,7 @@ public void execute() {
         // Calculate new linear velocity
         Translation2d linearVelocity =
                 new Pose2d(new Translation2d(), linearDirection)
-                .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
+                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
 
         // Convert to field relative speeds & send command
         drivetrain.runVelocity(

src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java

@@ -9,9 +9,9 @@
 public class DeclimbSequence extends SequentialCommandGroup {
     public DeclimbSequence(Elevator elevator, Hooks hooks) {
         addCommands(
-            new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
-            hooks.undeployHooksCommand(), //Release hooks
-            new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
+                new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
+                hooks.undeployHooksCommand(), //Release hooks
+                new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
         );
     }
 }

src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java

@@ -1,16 +1,17 @@
 package org.team1540.robot2024.commands.climb;
 
-import org.team1540.robot2024.subsystems.tramp.Tramp;
-
 import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.tramp.Tramp;
 
 //TODO: Write this command Tramp people :D
 public class ScoreInTrap extends Command {
     private final Tramp tramp;
+
     public ScoreInTrap(Tramp tramp) {
         this.tramp = tramp;
         addRequirements(tramp);
     }
+
     @Override
     public void initialize() {
         //TODO: Score in trap :D

src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java

@@ -1,10 +1,9 @@
 package org.team1540.robot2024.commands.elevator;
 
-import org.team1540.robot2024.Constants;
-import org.team1540.robot2024.subsystems.elevator.Elevator;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.subsystems.elevator.Elevator;
 
 public class ElevatorManualCommand extends Command {
 
@@ -19,8 +18,7 @@ public ElevatorManualCommand(Elevator elevator, CommandXboxController copilot) {
 
     @Override
     public void execute() {
-        // elevator.setVoltage(copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis() + Constants.Elevator.KG);
-        elevator.setElevatorPosition(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS*Constants.LOOP_PERIOD_SECS*(copilot.getRightTriggerAxis()-copilot.getLeftTriggerAxis()));
+        elevator.setElevatorPosition(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS * Constants.LOOP_PERIOD_SECS * (copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis()));
     }
 
     @Override

src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java

@@ -13,6 +13,7 @@ public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) {
         this.state = state;
         addRequirements(elevator);
     }
+
     @Override
     public void initialize() {
         elevator.setElevatorPosition(state.heightMeters);
@@ -22,6 +23,7 @@ public void initialize() {
     public void end(boolean interrupted) {
         elevator.stop();
     }
+
     @Override
     public boolean isFinished() {
         return elevator.isAtSetpoint();

src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java

@@ -20,7 +20,7 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return indexer.isNotePresent();
+        return indexer.isNoteStaged();
     }
 
     @Override

src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java

@@ -0,0 +1,35 @@
+package org.team1540.robot2024.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.tramp.Tramp;
+
+public class StageTrampCommand extends Command {
+
+    private final Tramp tramp;
+    private final Indexer indexer;
+
+    public StageTrampCommand(Tramp tramp, Indexer indexer) {
+        this.tramp = tramp;
+        this.indexer = indexer;
+        addRequirements(tramp, indexer);
+    }
+
+    @Override
+    public void initialize() {
+        tramp.setPercent(0.5);
+        indexer.setFeederVelocity(-600);
+        indexer.setIntakePercent(0.5);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return tramp.isNoteStaged();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexer.stopFeeder();
+        tramp.stop();
+    }
+}

src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java

@@ -0,0 +1,25 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+class PrepareShooterCommand extends Command {
+    private final Shooter shooter;
+
+    public PrepareShooterCommand(Shooter shooter) {
+        this.shooter = shooter;
+        addRequirements(shooter);
+    }
+    @Override
+    public void execute() {
+        // TODO: Make this dynamically update based on estimated pose
+        shooter.setFlywheelSpeeds(1000, 1000);
+        shooter.setPivotPosition(Rotation2d.fromDegrees(30));
+    }
+
+    @Override
+    public boolean isFinished() {
+        return shooter.areFlywheelsSpunUp() && shooter.isPivotAtSetpoint();
+    }
+}