From 8d5e11b267356b392dd7ccfd99dc96ca7961dfd0 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 1/7] 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;

From 8240f8f3c4b7aa30af659f70c78f51457e3ba1db Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Wed, 15 Mar 2023 09:57:01 -0600
Subject: [PATCH 2/7] drift fix (not tuned)

---
 src/main/java/frc4388/robot/Constants.java              | 7 ++++---
 src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 6 +++---
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 83ac014..2d8438b 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -24,9 +24,10 @@ import frc4388.utility.LEDPatterns;
 public final class Constants {
   public static final class SwerveDriveConstants {
 
-    public static final double MAX_ROT_SPEED = 1.5;
-    public static final double MIN_ROT_SPEED = 0.8;
-    public static       double ROTATION_SPEED = MAX_ROT_SPEED;
+    public static final double MAX_ROT_SPEED        = 1.5;
+    public static final double MIN_ROT_SPEED        = 0.8;
+    public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
+    public static       double ROT_CORRECTION_SPEED = MIN_ROT_SPEED;
 
     public static final class IDs {
       public static final int LEFT_FRONT_WHEEL_ID = 2;
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index fcaadb9..1f28f3e 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -58,9 +58,9 @@ public class SwerveDrive extends SubsystemBase {
       double rot = 0;
       if (rightStick.getNorm() > 0.1) {
         rotTarget = gyro.getRotation2d();
-        rot = rightStick.getX();
+        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
       } else {
-        rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
+        rot = rotTarget.minus(gyro.getRotation2d()).getRadians() * SwerveDriveConstants.ROT_CORRECTION_SPEED;
       }
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
@@ -68,7 +68,7 @@ public class SwerveDrive extends SubsystemBase {
       Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1));
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);

From 745a5b51c039f2de1f1507be192ced726d41f354 Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Wed, 15 Mar 2023 09:57:31 -0600
Subject: [PATCH 3/7] fixed num

---
 src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 1f28f3e..44ab39f 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -56,7 +56,7 @@ public class SwerveDrive extends SubsystemBase {
     if (fieldRelative) {
 
       double rot = 0;
-      if (rightStick.getNorm() > 0.1) {
+      if (rightStick.getNorm() > 0.01) {
         rotTarget = gyro.getRotation2d();
         rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
       } else {

From f7b804680b7d523364b439ab6d405b0cc2f7924f Mon Sep 17 00:00:00 2001
From: Aarav <aaravshah18@gmail.com>
Date: Wed, 15 Mar 2023 11:35:08 -0600
Subject: [PATCH 4/7] fixed buttons

---
 src/main/java/frc4388/robot/RobotContainer.java    | 2 +-
 src/main/java/frc4388/robot/commands/RunArmIn.java | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 71b1399..583944c 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -161,7 +161,7 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
 
         // TODO: put this into a variable
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new ParallelCommandGroup(
                 new InstantCommand(() -> m_robotClaw.toggle()),
                 new SequentialCommandGroup(
diff --git a/src/main/java/frc4388/robot/commands/RunArmIn.java b/src/main/java/frc4388/robot/commands/RunArmIn.java
index a865e9d..914e173 100644
--- a/src/main/java/frc4388/robot/commands/RunArmIn.java
+++ b/src/main/java/frc4388/robot/commands/RunArmIn.java
@@ -22,7 +22,7 @@ public class RunArmIn extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_arm.setTeleVel(-1);
+    m_arm.setTeleVel(1);
   }
 
   // Called once the command ends or is interrupted.

From 29eeee1f7fb76bfa26b680c68e071ff3f4302162 Mon Sep 17 00:00:00 2001
From: Aarav <aaravshah18@gmail.com>
Date: Wed, 15 Mar 2023 13:03:02 -0600
Subject: [PATCH 5/7] commit stuff

---
 src/main/java/frc4388/robot/Constants.java    |  5 ++++-
 .../java/frc4388/robot/RobotContainer.java    |  6 ++++--
 .../java/frc4388/robot/commands/RunArmIn.java |  2 +-
 .../java/frc4388/robot/subsystems/Arm.java    | 10 ++--------
 .../frc4388/robot/subsystems/SwerveDrive.java | 20 ++++++++++++-------
 5 files changed, 24 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 2d8438b..f82c168 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -27,7 +27,10 @@ public final class Constants {
     public static final double MAX_ROT_SPEED        = 1.5;
     public static final double MIN_ROT_SPEED        = 0.8;
     public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
-    public static       double ROT_CORRECTION_SPEED = MIN_ROT_SPEED;
+    public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
+
+    public static final double CORRECTION_MIN = 10;
+    public static final double CORRECTION_MAX = 50;
 
     public static final class IDs {
       public static final int LEFT_FRONT_WHEEL_ID = 2;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 583944c..77e5f1b 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -17,6 +17,7 @@ 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.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoBalance;
@@ -122,7 +123,7 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive));
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(m_robotArm.getArmRotation()-135), m_robotSwerveDrive));
         //     // .onFalse()
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -164,8 +165,9 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new ParallelCommandGroup(
                 new InstantCommand(() -> m_robotClaw.toggle()),
+                new RunArmIn(m_robotArm),
                 new SequentialCommandGroup(
-                    new RunArmIn(m_robotArm),
+                    new WaitCommand(.25),
                     new PivotCommand(m_robotArm, 135)
                 )
             ));
diff --git a/src/main/java/frc4388/robot/commands/RunArmIn.java b/src/main/java/frc4388/robot/commands/RunArmIn.java
index 914e173..e1fddd5 100644
--- a/src/main/java/frc4388/robot/commands/RunArmIn.java
+++ b/src/main/java/frc4388/robot/commands/RunArmIn.java
@@ -12,7 +12,7 @@ public class RunArmIn extends CommandBase {
 
   public RunArmIn(Arm arm) {
     m_arm = arm;
-    addRequirements(arm);
+    addRequirements();
   }
 
   // Called when the command is initially scheduled.
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 1d017e0..2f4e844 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -55,9 +55,9 @@ public class Arm extends SubsystemBase {
         if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
             m_pivot.set(ControlMode.PercentOutput, 0);
         } else if (degrees > 90 && vel > 0) {
-            m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+            m_pivot.set(ControlMode.PercentOutput, .1 * vel);
         } else {
-            m_pivot.set(ControlMode.PercentOutput, .3 * vel);
+            m_pivot.set(ControlMode.PercentOutput, .25 * vel);
         }
     }
 
@@ -169,12 +169,6 @@ public class Arm extends SubsystemBase {
     boolean soft_limits = true;
     public void killSoftLimits() {
         resetTeleSoftLimit();
-        var pivot_soft = m_pivot.getSelectedSensorPosition();
-        var tele_soft  = m_tele.getSelectedSensorPosition();
-        
-        m_pivot.configForwardSoftLimitEnable(!soft_limits);
-        m_pivot.configReverseSoftLimitEnable(!soft_limits);
-
         soft_limits = !soft_limits;
     }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 44ab39f..2edb77d 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -12,6 +12,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -37,7 +38,7 @@ public class SwerveDrive extends SubsystemBase {
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
   
-  public Rotation2d rotTarget = new Rotation2d();
+  public double rotTarget = 0;
   public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   /** Creates a new SwerveDrive. */
@@ -56,11 +57,13 @@ public class SwerveDrive extends SubsystemBase {
     if (fieldRelative) {
 
       double rot = 0;
-      if (rightStick.getNorm() > 0.01) {
-        rotTarget = gyro.getRotation2d();
+      if (rightStick.getNorm() > 0.05) {
+        rotTarget = gyro.getAngle();
         rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
+        SmartDashboard.putBoolean("drift correction", false);
       } else {
-        rot = rotTarget.minus(gyro.getRotation2d()).getRadians() * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+        SmartDashboard.putBoolean("drift correction", true);
+        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
       }
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
@@ -95,7 +98,7 @@ public class SwerveDrive extends SubsystemBase {
 
   public void resetGyro() {
     gyro.reset();
-    rotTarget = new Rotation2d(0);
+    rotTarget = 0;
   }
   
   public void stopModules() {
@@ -113,11 +116,14 @@ public class SwerveDrive extends SubsystemBase {
     // This method will be called once per scheduler run
   }
 
-  public void toggleGear() {
-    if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
+  public void toggleGear(double angle) {
+    if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW
+        && Math.abs(angle) < 2) {
       this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX;
     } else {
       this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
     }
   }
 

From 9322997ce8e14fe5e2b2a2dc4971458d242c6254 Mon Sep 17 00:00:00 2001
From: Aarav <aaravshah18@gmail.com>
Date: Wed, 15 Mar 2023 15:29:52 -0600
Subject: [PATCH 6/7] cummmmit

---
 .../java/frc4388/robot/RobotContainer.java    |  7 ++--
 .../frc4388/robot/commands/ExtendArmTo.java   | 36 +++++++++++++++++++
 .../frc4388/robot/commands/PivotCommand.java  |  2 +-
 .../java/frc4388/robot/commands/RunArmIn.java |  2 +-
 .../frc4388/robot/commands/TeleCommand.java   |  2 +-
 .../java/frc4388/robot/subsystems/Arm.java    | 20 ++++++-----
 6 files changed, 54 insertions(+), 15 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/ExtendArmTo.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 77e5f1b..1e721ef 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -89,8 +89,8 @@ public class RobotContainer {
             .withName("SwerveDrive DefaultCommand"));
 
         m_robotArm.setDefaultCommand(new RunCommand(() -> {
-            m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
-            m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
+            m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY(), false);
+            m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY(), false);
         }, m_robotArm)
         .withName("Arm DefaultCommand"));
         
@@ -165,9 +165,8 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new ParallelCommandGroup(
                 new InstantCommand(() -> m_robotClaw.toggle()),
-                new RunArmIn(m_robotArm),
                 new SequentialCommandGroup(
-                    new WaitCommand(.25),
+                    new RunArmIn(m_robotArm),
                     new PivotCommand(m_robotArm, 135)
                 )
             ));
diff --git a/src/main/java/frc4388/robot/commands/ExtendArmTo.java b/src/main/java/frc4388/robot/commands/ExtendArmTo.java
new file mode 100644
index 0000000..d2db4b4
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ExtendArmTo.java
@@ -0,0 +1,36 @@
+// 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 ExtendArmTo extends CommandBase {
+    private final Arm     m_arm;
+    private final double  m_target;
+    private final boolean m_direction;
+
+    public ExtendArmTo(Arm arm, double units) {
+        m_arm       = arm;
+        m_target    = units;
+        m_direction = m_target > 1;
+        addRequirements(m_arm);
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_arm.setTeleVel(m_direction ? 1 : -1, false);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        if (m_direction)
+            return m_arm.getTeleUnit() > m_target;
+        else
+            return m_arm.getTeleUnit() < m_target;
+    }
+}
diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java
index a84690d..fcb54d1 100644
--- a/src/main/java/frc4388/robot/commands/PivotCommand.java
+++ b/src/main/java/frc4388/robot/commands/PivotCommand.java
@@ -27,6 +27,6 @@ public class PivotCommand extends PelvicInflammatoryDisease {
   @Override
   public void runWithOutput(double output) {
     SmartDashboard.putNumber("pivot output", output);
-    arm.setRotVel(output);
+    arm.setRotVel(output, true);
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/RunArmIn.java b/src/main/java/frc4388/robot/commands/RunArmIn.java
index e1fddd5..c1ac04b 100644
--- a/src/main/java/frc4388/robot/commands/RunArmIn.java
+++ b/src/main/java/frc4388/robot/commands/RunArmIn.java
@@ -22,7 +22,7 @@ public class RunArmIn extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_arm.setTeleVel(1);
+    m_arm.setTeleVel(1, true);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java
index 02c8c27..87270c8 100644
--- a/src/main/java/frc4388/robot/commands/TeleCommand.java
+++ b/src/main/java/frc4388/robot/commands/TeleCommand.java
@@ -27,6 +27,6 @@ public class TeleCommand extends PelvicInflammatoryDisease {
 
   @Override
   public void runWithOutput(double output) {
-    arm.setTeleVel(output);
+    arm.setTeleVel(output, true);
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 2f4e844..ac33f65 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -47,7 +47,7 @@ public class Arm extends SubsystemBase {
         this(pivot, tele, encoder, false);
     }
 
-    public void setRotVel(double vel) {
+    public void setRotVel(double vel, boolean fast) {
         var degrees = Math.abs(getArmRotation()) - 135;
         SmartDashboard.putNumber("arm degrees", degrees);
         SmartDashboard.putNumber("arm rot vel", vel);
@@ -55,21 +55,25 @@ public class Arm extends SubsystemBase {
         if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
             m_pivot.set(ControlMode.PercentOutput, 0);
         } else if (degrees > 90 && vel > 0) {
-            m_pivot.set(ControlMode.PercentOutput, .1 * vel);
+            m_pivot.set(ControlMode.PercentOutput, .2 * vel);
         } else {
-            m_pivot.set(ControlMode.PercentOutput, .25 * vel);
+            m_pivot.set(ControlMode.PercentOutput, (fast ? .8 : .3) * vel);
         }
     }
 
-    public void setTeleVel(double vel) {
-        m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
+    public void setTeleVel(double vel, boolean speed) {
+        m_tele.set(ControlMode.PercentOutput, (speed ? -.9 : .7) * vel);
     }
 
     public boolean isTeleIn() {
-        return m_tele.isFwdLimitSwitchClosed() == 1 ||
+        return m_tele.isRevLimitSwitchClosed() == 1 ||
             m_tele.getSelectedSensorPosition() < reverse_tele;
     }
 
+    public double getTeleUnit() {
+        return m_tele.getSelectedSensorPosition() - reverse_tele;
+    }
+
     public void armSetRotation(double rot) {
         if (rot > 1 || rot < 0) return;
         // Move arm code
@@ -104,8 +108,8 @@ public class Arm extends SubsystemBase {
                            (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
 
         if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
-            setRotVel(pivot);
-            setTeleVel(tele);
+            setRotVel(pivot, false);
+            setTeleVel(tele, false);
         }
     }
 

From da2255b180aa01a6dca229113076ebc194f60e6c Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Wed, 15 Mar 2023 17:46:12 -0600
Subject: [PATCH 7/7] change

---
 .../frc4388/robot/commands/LimeAlign.java     | 34 -------------------
 .../frc4388/robot/subsystems/SwerveDrive.java | 13 +++++--
 2 files changed, 10 insertions(+), 37 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/LimeAlign.java

diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java
deleted file mode 100644
index c811a3e..0000000
--- a/src/main/java/frc4388/robot/commands/LimeAlign.java
+++ /dev/null
@@ -1,34 +0,0 @@
-// 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.SwerveDrive;
-
-public class LimeAlign extends CommandBase {
-  public LimeAlign(SwerveDrive drive) {
-    addRequirements(drive);
-  }
-
-  // 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() {}
-
-  // 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 false;
-  }
-}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 2edb77d..d4a27fa 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -67,7 +67,9 @@ public class SwerveDrive extends SubsystemBase {
       }
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+      Translation2d speed = leftStick.times(speedAdjust * leftStick.getNorm());
+      // Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm());
+
       Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
@@ -117,12 +119,17 @@ public class SwerveDrive extends SubsystemBase {
   }
 
   public void toggleGear(double angle) {
-    if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW
-        && Math.abs(angle) < 2) {
+    if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW && Math.abs(angle) < 2) {
+
       this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+      SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MAX_ROT_SPEED;
+
       SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX;
     } else {
+
       this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+      SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED;
+
       SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
     }
   }