cummmmit

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

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

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

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.

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

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