diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e290dd5..b708812 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -152,7 +152,7 @@ public class RobotContainer { private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( new PivotCommand(m_robotArm, 135 + 47), new WaitCommand(0.3), - new TeleCommand(m_robotArm, 29500) + new TeleCommand(m_robotArm, 30000) // toggleClaw.asProxy(), // armToHome.asProxy() ); @@ -172,8 +172,18 @@ public class RobotContainer { private Command wait3 = new WaitCommand(3); private Command wait5 = new WaitCommand(5); + private SequentialCommandGroup taxiFar = new SequentialCommandGroup( - new TimedCommand(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.6), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive), 4.75) + placeConeMid.asProxy(), + new WaitCommand(0.3), + toggleClaw.asProxy(), + new WaitCommand(0.2), + toggleClaw.asProxy(), + new WaitCommand(0.3), + // new ParallelCommandGroup( + armToHome.asProxy() + // new TimedCommand(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, 0.6), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive), 1.0) + // ) ); private SequentialCommandGroup placeRed1Balance = new SequentialCommandGroup( @@ -188,6 +198,20 @@ public class RobotContainer { new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive) ); + private SequentialCommandGroup placeTaxi = new SequentialCommandGroup( + placeConeMid.asProxy(), + new WaitCommand(0.3), + // new InstantCommand(() -> m_robotClaw.setClaw(true), m_robotClaw), + toggleClaw.asProxy(), + new WaitCommand(0.2), + toggleClaw.asProxy(), + new WaitCommand(0.3), + new ParallelCommandGroup( + armToHome.asProxy(), + new TimedCommand(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, 0.6), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive), 4.9) + ) + ); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -220,6 +244,7 @@ public class RobotContainer { chooser.addOption("placeLow", placeLow); playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + .addOption("PlaceTaxi", placeTaxi) .addOption("PlaceRed1Balance", placeRed1Balance) .addOption("Wait3", wait3.asProxy()) .addOption("Wait5", wait5.asProxy()) diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index b520b51..e93a7a0 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -691,7 +691,7 @@ public class JoystickPlayback extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - input.close(); + // input.close(); swerve.stopModules(); } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 6e4c052..18c7bbb 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -56,7 +56,16 @@ public class Arm extends SubsystemBase { } public void setTeleVel(double vel) { - m_tele.set(ControlMode.PercentOutput, -0.5 * vel); + var armPos = getArmLength(); + + if (tele_softLimit && armPos > 0 && vel > 0) + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); + else if (tele_softLimit && armPos < 90000 && vel < 0) + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); + else if (!tele_softLimit) + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); + else + m_tele.set(ControlMode.PercentOutput, 0); } public void armSetRotation(double rot) { @@ -115,10 +124,26 @@ public class Arm extends SubsystemBase { boolean tele_softLimit = false; double tele_soft = 0; + + public void setSoftLimit(boolean lim) { + if (lim) { + tele_soft = m_tele.getSelectedSensorPosition(); + m_tele.configForwardSoftLimitThreshold(90000 - tele_soft); // 96937 + m_tele.configReverseSoftLimitThreshold(tele_soft); + // m_tele.configForwardSoftLimitEnable(true); + // m_tele.configReverseSoftLimitEnable(true); + } else { + m_tele.configForwardSoftLimitEnable(false); + m_tele.configReverseSoftLimitEnable(false); + } + + tele_softLimit = lim; + } + public void resetTeleSoftLimit() { if (!tele_softLimit) { tele_soft = m_tele.getSelectedSensorPosition(); - m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); + m_tele.configForwardSoftLimitThreshold(90000 - tele_soft); // 96937 m_tele.configReverseSoftLimitThreshold(tele_soft); m_tele.configForwardSoftLimitEnable(true); m_tele.configReverseSoftLimitEnable(true); @@ -148,7 +173,7 @@ public class Arm extends SubsystemBase { } // SmartDashboard.putNumber("Pivot CANCoder", getArmRotation()); - // SmartDashboard.putNumber("Tele Encoder", getArmLength()); + SmartDashboard.putNumber("Tele Encoder", getArmLength()); // double x = Math.cos(Math.toRadians(degrees)); }