idaho day 3

src/main/java/frc/robot/RobotContainer.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -182,7 +183,8 @@ public RobotContainer() {
         autonBuilder = new AutonBuilder(
             intakePivotSubsystem, intakeRollerSubsystem, 
             shooterFlywheelSubsystem, shooterPivotSubsystem, 
-            elevatorSubsystem, 
+            elevatorSubsystem,
+            climbSubsystem, 
             swerveSubsystem, 
             lightBarSubsystem, fmsSubsystem
         );
@@ -319,10 +321,13 @@ private void configureBindings() {
 
         // elevatorSubsystem.setManual();
 
-        // elevatorSubsystem.setDefaultCommand(new InstantCommand(() ->
-        //     elevatorSubsystem.setManualPower(mechController.getRightX()),
-        //     elevatorSubsystem)
-        // );
+        elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> {
+            if (mechController.getPOV() == 0) {
+                elevatorSubsystem.setTargetState(ElevatorState.TRAP);
+            } else if (mechController.getPOV() == 180) {
+                elevatorSubsystem.setTargetState(ElevatorState.ZERO);
+            } 
+        }, elevatorSubsystem));
 
         /* INTAKE TEST */
 
@@ -451,7 +456,7 @@ private void configureBindings() {
                 }
             } else {
                 shooterPivotSubsystem.setAutoAimBoolean(false);
-                if (mechController.getPOV() == 0) {
+                if (mechController.getPOV() == 90) {
                     shooterPivotSubsystem.setAngle(Units.degreesToRadians(60));
                     shooterFlywheelSubsystem.setShooterMotorSpeed(.4);
                     if (shooterFlywheelSubsystem.atSpeed()) {
@@ -547,6 +552,6 @@ private void configureBindings() {
      * @return The selected autonomous command.
      */
     public Command getAutonomousCommand() {
-        return autonPathChooser.getSelected();
+        return autonBuilder.getMiddleFourPiece();//autonPathChooser.getSelected();
     }
 }

src/main/java/frc/robot/commands/auton/AutonBuilder.java

@@ -24,6 +24,7 @@
 import frc.robot.commands.shooter.flywheel.ShooterFlywheelStopCommand;
 import frc.robot.commands.shooter.pivot.ShooterPivotAimCommand;
 import frc.robot.subsystems.FieldManagementSubsystem;
+import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.intake.IntakePivotSubsystem;
 import frc.robot.subsystems.intake.IntakeRollerSubsystem;
@@ -45,6 +46,7 @@ public class AutonBuilder {
     private final ShooterPivotSubsystem shooterPivotSubsystem;
     private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
     private final ElevatorSubsystem elevatorSubsystem;
+    private final ClimbSubsystem climbSubsystem;
     private final SwerveSubsystem swerveSubsystem;
     private final LightBarSubsystem lightBarSubsystem;
     private final FieldManagementSubsystem fmsSubsystem;
@@ -58,6 +60,7 @@ public AutonBuilder(IntakePivotSubsystem intakePivotSubsystem,
                              ShooterFlywheelSubsystem shooterFlywheelSubsystem,
                              ShooterPivotSubsystem shooterPivotSubsystem,
                              ElevatorSubsystem elevatorSubsystem,
+                             ClimbSubsystem climbSubsystem,
                              SwerveSubsystem swerveSubsystem,
                              LightBarSubsystem lightBarSubsystem,
                              FieldManagementSubsystem fmsSubsystem) {
@@ -66,6 +69,7 @@ public AutonBuilder(IntakePivotSubsystem intakePivotSubsystem,
         this.shooterFlywheelSubsystem = shooterFlywheelSubsystem;
         this.shooterPivotSubsystem = shooterPivotSubsystem;
         this.elevatorSubsystem = elevatorSubsystem;
+        this.climbSubsystem = climbSubsystem;
         this.swerveSubsystem = swerveSubsystem;
         this.lightBarSubsystem = lightBarSubsystem;
         this.fmsSubsystem = fmsSubsystem;
@@ -128,7 +132,7 @@ public Command goAndIntake(ChoreoTrajectory intakeTrajectory) {
      */
     public Command shoot() {
         return new ShooterPivotAimCommand(shooterPivotSubsystem)
-            .alongWith(new SetCalculatedAngleCommand(swerveSubsystem))
+            .alongWith(new SetCalculatedAngleCommand(swerveSubsystem)).withTimeout(1)
             .andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem).until(
                 () -> !intakeRollerSubsystem.getRockwellSensorValue())
             .andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem)).withTimeout(.5)
@@ -186,13 +190,18 @@ public SequentialCommandGroup buildAuton(Pose2d initPose, Command... commands) {
     // starts furthest away from amp in SPECIAL START POSITION, sweeps center notes starting furthest away from amp
     public SequentialCommandGroup getBottomBottomCenterDistruptor() {
         ChoreoTrajectory trajectory = Choreo.getTrajectory("BottomBottomCenterDisruptor");
-        return new SequentialCommandGroup(followPath(trajectory));
+        return new SequentialCommandGroup(
+            resetSwerve(GRTUtil.mirrorAcrossField(trajectory.getInitialPose(), fmsSubsystem::isRedAlliance)),
+            followPath(trajectory));
     }
 
     // starts furthest away from amp in SPECIAL START POSITION, sweeps center notes starting closest to amp
     public SequentialCommandGroup getBottomTopCenterDistruptor() {
-        ChoreoTrajectory trajectory = Choreo.getTrajectory("BottomTopCenterDisruptor");
-        return new SequentialCommandGroup(followPath(trajectory));
+        ChoreoTrajectory trajectory = Choreo.getTrajectory("BottomTopCenterDistruptor");
+        return new SequentialCommandGroup(
+            resetSwerve(GRTUtil.mirrorAcrossField(trajectory.getInitialPose(), fmsSubsystem::isRedAlliance)),
+            new InstantCommand(() -> climbSubsystem.setSpeeds(-0.2, -0.2)),
+            followPath(trajectory));
     }
 
     /** Starts amp side and shoots the preloaded note. */

src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java

@@ -79,7 +79,7 @@ public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier red
                               5.6, 
                               ShooterConstants.MAX_SHOOTER_DISTANCE};
 
-        double[] topSpeeds = {.4, .5, .7, .75, .75, .75, .75};
+        double[] topSpeeds = {.6, .5, .7, .75, .75, .75, .75};
         double[] bottomSpeeds = {.5, .5, .35, .4, .4, .75, .75};
 
         targetTopRPS = 0.0;