From b5bc54789847a4be4ce6dbe929715f903a5be7e3 Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Wed, 15 Mar 2023 09:53:52 -0600
Subject: [PATCH] auto button

---
 .../java/frc4388/robot/RobotContainer.java    | 17 ++++++++-
 .../java/frc4388/robot/commands/RunArmIn.java | 37 +++++++++++++++++++
 .../java/frc4388/robot/subsystems/Arm.java    | 14 ++++++-
 3 files changed, 64 insertions(+), 4 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/RunArmIn.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 1b6fb4c..71b1399 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -14,7 +14,9 @@ import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoBalance;
@@ -25,6 +27,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
 import frc4388.robot.commands.PivotCommand;
 import frc4388.robot.commands.PlaybackChooser;
+import frc4388.robot.commands.RunArmIn;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
 
@@ -149,13 +152,23 @@ public class RobotContainer {
             .onTrue(new PivotCommand(m_robotArm, 135));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new PivotCommand(m_robotArm, 210));
+            .onTrue(new PivotCommand(m_robotArm, 225));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
+            .onTrue(new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
+
+        // TODO: put this into a variable
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+            .onTrue(new ParallelCommandGroup(
+                new InstantCommand(() -> m_robotClaw.toggle()),
+                new SequentialCommandGroup(
+                    new RunArmIn(m_robotArm),
+                    new PivotCommand(m_robotArm, 135)
+                )
+            ));
         
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
         //     .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
diff --git a/src/main/java/frc4388/robot/commands/RunArmIn.java b/src/main/java/frc4388/robot/commands/RunArmIn.java
new file mode 100644
index 0000000..a865e9d
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/RunArmIn.java
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Arm;
+
+public class RunArmIn extends CommandBase {
+  private final Arm m_arm;
+
+  public RunArmIn(Arm arm) {
+    m_arm = arm;
+    addRequirements(arm);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_arm.setTeleVel(-1);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_arm.isTeleIn();
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 06fab10..1d017e0 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -65,6 +65,11 @@ public class Arm extends SubsystemBase {
         m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
     }
 
+    public boolean isTeleIn() {
+        return m_tele.isFwdLimitSwitchClosed() == 1 ||
+            m_tele.getSelectedSensorPosition() < reverse_tele;
+    }
+
     public void armSetRotation(double rot) {
         if (rot > 1 || rot < 0) return;
         // Move arm code
@@ -127,6 +132,8 @@ public class Arm extends SubsystemBase {
             m_tele.configReverseSoftLimitThreshold(tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
             m_tele.configReverseSoftLimitEnable(true);
+
+            reverse_tele = tele_soft;
         } else {
             m_tele.configForwardSoftLimitEnable(false);
             m_tele.configReverseSoftLimitEnable(false);
@@ -135,8 +142,9 @@ public class Arm extends SubsystemBase {
         tele_softLimit = !tele_softLimit;
     }
 
-    boolean resetable  = true;
-    boolean tele_reset = true;
+    boolean resetable    = true;
+    boolean tele_reset   = true;
+    double  reverse_tele = 0;
 
     @Override
     public void periodic() {
@@ -148,6 +156,8 @@ public class Arm extends SubsystemBase {
             m_tele.configReverseSoftLimitThreshold(1000  - tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
             m_tele.configReverseSoftLimitEnable(true);
+            
+            reverse_tele = 1000 - tele_soft;
             tele_reset = false;
         } else if (m_tele.isFwdLimitSwitchClosed() == 0) {
             tele_reset = true;