cummmmit

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