From 6da7e6fdf2c113887ac09e8fc3a37fa738013d20 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Sun, 24 Mar 2024 15:31:55 -0700 Subject: [PATCH] idaho day 3 --- src/main/java/frc/robot/RobotContainer.java | 19 ++++++++++++------- .../robot/commands/auton/AutonBuilder.java | 17 +++++++++++++---- .../shooter/ShooterFlywheelSubsystem.java | 2 +- 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d839d1fe..ae8d975f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/AutonBuilder.java b/src/main/java/frc/robot/commands/auton/AutonBuilder.java index 3561ded7..3de416e2 100644 --- a/src/main/java/frc/robot/commands/auton/AutonBuilder.java +++ b/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. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index 4222bed8..de7774d9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/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;