diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f72cb1a..35737cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,9 +19,9 @@ import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Arm.Setpoint; -import frc.robot.subsystems.CommandSwerveDrivetrain.RotationSetpoint; import frc.robot.subsystems.Climber; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.CommandSwerveDrivetrain.RotationSetpoint; import frc.robot.subsystems.Index; import frc.robot.subsystems.Intake; import frc.robot.subsystems.LED; @@ -115,11 +115,15 @@ public Robot() { .withTimeout(2) .andThen(Commands.parallel(m_shooter.stop(), m_index.stop())), m_arm.maintain())))); - NamedCommands.registerCommand("Arm", Commands.deferredProxy(() -> m_arm.goToSetpointAuto(Setpoint.kSpeaker).withTimeout(1.2))); + NamedCommands.registerCommand( + "Arm", + Commands.deferredProxy(() -> m_arm.goToSetpointAuto(Setpoint.kSpeaker).withTimeout(1.2))); NamedCommands.registerCommand( "Intake", Commands.deferredProxy(() -> m_superstructure.intake())); NamedCommands.registerCommand("OTFArm", m_arm.moveToPosition(0.7528).andThen(m_arm.maintain())); - NamedCommands.registerCommand("Spinup", Commands.deferredProxy(() -> m_shooter.spinup(500).andThen(m_shooter.maintain()))); + NamedCommands.registerCommand( + "Spinup", + Commands.deferredProxy(() -> m_shooter.spinup(500).andThen(m_shooter.maintain()))); NamedCommands.registerCommand("Passthrough", Commands.deferredProxy(() -> m_index.shootAuto())); // m_autoChooser.setDefaultOption("Center1425", m_drivetrain.getAutoPath("Center1425")); @@ -178,8 +182,12 @@ public Robot() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftBumper().onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kAmp, m_drive, m_scheduler)); - m_driver.rightBumper().onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kSpeaker, m_drive, m_scheduler)); + m_driver + .leftBumper() + .onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kAmp, m_drive, m_scheduler)); + m_driver + .rightBumper() + .onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kSpeaker, m_drive, m_scheduler)); m_codriver.leftBumper().whileTrue(m_index.shoot()); m_codriver.rightBumper().whileTrue(m_superstructure.outake()); @@ -187,13 +195,17 @@ public Robot() { .leftTrigger() .whileTrue( Commands.parallel( - m_arm.goToSetpoint(Setpoint.kSpeaker, m_codriver.leftTrigger()).andThen(m_arm.maintain()), + m_arm + .goToSetpoint(Setpoint.kSpeaker, m_codriver.leftTrigger()) + .andThen(m_arm.maintain()), m_shooter.spinup(1500).andThen(m_shooter.maintain()))); m_codriver .rightTrigger() .whileTrue( Commands.parallel( - m_arm.goToSetpoint(Setpoint.kAmp, m_codriver.rightTrigger()).andThen(m_arm.maintain()), + m_arm + .goToSetpoint(Setpoint.kAmp, m_codriver.rightTrigger()) + .andThen(m_arm.maintain()), m_shooter.applyVolts(8))); m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop()); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 3c58417..2ed1b08 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -51,8 +51,7 @@ public enum Setpoint { new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless); private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); - private final PIDController m_autopid = - new PIDController(10, Constants.armI, Constants.armD); + private final PIDController m_autopid = new PIDController(10, Constants.armI, Constants.armD); private final PIDController m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD); private final ArmFeedforward m_feedforward = @@ -225,7 +224,7 @@ public Command goToSetpoint(Setpoint setpoint, Trigger holdTrigger) { // case kShuttling -> tempPosition = 0.2; // } // - // final double position = tempPosition; + // final double position = tempPosition; // // Command moveCommand = moveToPosition(position); // @@ -237,9 +236,9 @@ public Command goToSetpoint(Setpoint setpoint, Trigger holdTrigger) { // goToSetpoint(Setpoint.kStowed).schedule(); // } // } - // ).until(() -> !holdTrigger.getAsBoolean()).andThen(goToSetpoint(Setpoint.kStowed)); + // ).until(() -> !holdTrigger.getAsBoolean()).andThen(goToSetpoint(Setpoint.kStowed)); // return moveCommand.andThen(holdOrReturnCommand); -} + } public Command goToSetpoint(Setpoint setpoint) { double position = 0; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 4021a9c..8ff171a 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -22,8 +22,6 @@ import com.pathplanner.lib.auto.*; import com.pathplanner.lib.commands.*; import com.pathplanner.lib.util.*; - -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -46,12 +44,9 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.LimelightHelpers; import frc.robot.generated.TunerConstants; @@ -428,44 +423,56 @@ public Command getAutoPath(String pathName) { return AutoBuilder.buildAuto(pathName); } -public Command goToSetpoint(RotationSetpoint setpoint, SwerveRequest.FieldCentric drivetrain, CommandScheduler scheduler) { + public Command goToSetpoint( + RotationSetpoint setpoint, + SwerveRequest.FieldCentric drivetrain, + CommandScheduler scheduler) { // Use AtomicReference to hold the target position, so it can be accessed in lambdas AtomicReference targetPosition = new AtomicReference<>(0.0); // Determine the target position based on the setpoint switch (setpoint) { - case kAmp: - targetPosition.set(-(Math.PI / 2)); // Example angle - break; - case kSpeaker: - targetPosition.set(Math.PI); // Facing forward - break; - default: - targetPosition.set(0.0); - break; + case kAmp: + targetPosition.set(-(Math.PI / 2)); // Example angle + break; + case kSpeaker: + targetPosition.set(Math.PI); // Facing forward + break; + default: + targetPosition.set(0.0); + break; } - // Define the command as a lambda that runs continuously until the robot reaches the target position - return Commands.run(() -> { - // Continuously update the current position - double currentPosition = m_odometry.getEstimatedPosition().getRotation().getRadians() % (2 * Math.PI); - double rotationOutput = m_rotationController.calculate(currentPosition, targetPosition.get()); - - // Apply the rotational output to the drivetrain - drivetrain.withRotationalRate(rotationOutput); - - // Debug print to verify execution - System.out.println("Moving towards setpoint..."); - - // Specify when the command should stop (when the target position is reached) - }) - .until(() -> Math.abs(m_odometry.getEstimatedPosition().getRotation().getRadians() - targetPosition.get()) < 0.01) - .withTimeout(2) - .andThen(() -> { - // Stop the drivetrain when the target position is reached - drivetrain.withRotationalRate(0); - System.out.println("Target position reached."); - }); -} - + // Define the command as a lambda that runs continuously until the robot reaches the target + // position + return Commands.run( + () -> { + // Continuously update the current position + double currentPosition = + m_odometry.getEstimatedPosition().getRotation().getRadians() % (2 * Math.PI); + double rotationOutput = + m_rotationController.calculate(currentPosition, targetPosition.get()); + + // Apply the rotational output to the drivetrain + drivetrain.withRotationalRate(rotationOutput); + + // Debug print to verify execution + System.out.println("Moving towards setpoint..."); + + // Specify when the command should stop (when the target position is reached) + }) + .until( + () -> + Math.abs( + m_odometry.getEstimatedPosition().getRotation().getRadians() + - targetPosition.get()) + < 0.01) + .withTimeout(2) + .andThen( + () -> { + // Stop the drivetrain when the target position is reached + drivetrain.withRotationalRate(0); + System.out.println("Target position reached."); + }); + } } diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java index 47852fb..b8d810a 100644 --- a/src/main/java/frc/robot/subsystems/Index.java +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -40,9 +40,9 @@ public Command shoot() { // if (!m_hasNote.getAsBoolean()) { // return runOnce(() -> {}).withName("Empty Index"); // } else { - return run(() -> m_motor.setVoltage(-8)) - // .until(m_hasNote.negate()) - .withName("Index PassThrough Teleop"); + return run(() -> m_motor.setVoltage(-8)) + // .until(m_hasNote.negate()) + .withName("Index PassThrough Teleop"); // } } @@ -50,9 +50,9 @@ public Command shootAuto() { // if (!m_hasNote.getAsBoolean()) { // return runOnce(() -> {}).withName("Empty Index"); // } else { - return run(() -> m_motor.setVoltage(-8)) - .until(m_hasNote.negate()) - .withName("Index PassThrough Auto"); + return run(() -> m_motor.setVoltage(-8)) + .until(m_hasNote.negate()) + .withName("Index PassThrough Auto"); // } } diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java index b590f87..fd73e1a 100644 --- a/src/main/java/frc/robot/subsystems/LED.java +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -34,7 +34,7 @@ public Command hasNote() { }); } - public Command intaking() { + public Command intaking() { return defer( () -> { m_activeState.set("intaking"); @@ -42,7 +42,6 @@ public Command intaking() { }); } - public Command hotpink() { return defer( () -> { diff --git a/src/main/warp-paths-v1.chor b/src/main/warp-paths-v1.chor index fb2210a..1f87aaf 100644 --- a/src/main/warp-paths-v1.chor +++ b/src/main/warp-paths-v1.chor @@ -25305,4 +25305,4 @@ }, "splitTrajectoriesAtStopPoints": true, "usesObstacles": false -} \ No newline at end of file +}