soft limit change + auto fix

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())

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();
   }
 

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));
     }