tele no PID, cube high/mid needs further testing

src/main/java/frc4388/robot/RobotContainer.java

@@ -114,16 +114,19 @@ public class RobotContainer {
     };
 
     // TODO: find actual values
-    private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
-        toggleClaw.asProxy(),
-        armToHome.asProxy()
-    );
+    private SequentialCommandGroup placeConeHigh =
+        new SequentialCommandGroup(
+            // new InstantCommand(() -> System.out.println("Placing cone high")),
+            new PivotCommand(m_robotArm, 64 + 135),
+            new InstantCommand(() -> m_robotArm.setRotVel(0)),
+            new TeleCommand(m_robotArm, 95642),
+            toggleClaw.asProxy(),
+            armToHome.asProxy()
+        );
 
     private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
+        new PivotCommand(m_robotArm, 70 + 135),
+        new TeleCommand(m_robotArm, 32866),
         toggleClaw.asProxy(),
         armToHome.asProxy()
     );
@@ -247,7 +250,8 @@ public class RobotContainer {
         
         // interrupt button
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(interruptCommand.asProxy());
+            .onTrue(placeConeHigh.asProxy());
+            // .onTrue(interruptCommand.asProxy());
 
         // place high
         new POVButton(getDeadbandedOperatorController(), 0)

src/main/java/frc4388/robot/commands/Arm/TeleCommand.java

@@ -4,30 +4,36 @@
 
 package frc4388.robot.commands.Arm;
 
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.commands.PelvicInflammatoryDisease;
 import frc4388.robot.subsystems.Arm;
 
-public class TeleCommand extends PelvicInflammatoryDisease {
-  private final Arm    arm;
-  private final double target;
+public class TeleCommand extends CommandBase {
+  private final Arm     arm;
+  private final double  target;
+  private       boolean goIn;
   
   /** Creates a new ArmCommand. */
   public TeleCommand(Arm arm, double target) {
-    super(0.6, 0, 0, 0, 0);
     this.arm    = arm;
     this.target = target;
     addRequirements(arm);
   }
 
   @Override
-  public double getError() {
-    return (arm.getArmLength() - target) /
-           (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+  public void initialize() {
+    this.goIn = target < arm.getArmLength();
   }
 
   @Override
-  public void runWithOutput(double output) {
-    arm.setTeleVel(output);
+  public void execute() {
+    arm.setTeleVel(goIn ? 1 : -1);
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (goIn) return arm.getArmLength() < target;
+    else      return arm.getArmLength() > target;
   }
 }

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -40,6 +40,8 @@ public class Arm extends SubsystemBase {
     }
 
     public void setRotVel(double vel) {
+        if (vel > 1) vel = 1;
+
         var degrees = Math.abs(getArmRotation()) - 135;
         SmartDashboard.putNumber("arm degrees", degrees);
         SmartDashboard.putNumber("arm rot vel", vel);
@@ -48,6 +50,8 @@ public class Arm extends SubsystemBase {
             m_pivot.set(ControlMode.PercentOutput, 0);
         } else if (degrees > 90 && vel > 0) {
             m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+        } else if (degrees < 25 && vel < 0) {
+            m_pivot.set(ControlMode.PercentOutput, .15 * vel);
         } else {
             m_pivot.set(ControlMode.PercentOutput, .3 * vel);
         }
@@ -78,7 +82,7 @@ public class Arm extends SubsystemBase {
     }
 
     public double getArmLength() {
-        return m_tele.getSelectedSensorPosition();
+        return m_tele.getSelectedSensorPosition() - tele_soft;
     }
 
     public double getArmRotation() {
@@ -112,9 +116,10 @@ public class Arm extends SubsystemBase {
     }
 
     boolean tele_softLimit = false;
+    double  tele_soft = 0;
     public void resetTeleSoftLimit() {
         if (!tele_softLimit) {
-            var tele_soft = m_tele.getSelectedSensorPosition();
+            tele_soft = m_tele.getSelectedSensorPosition();
             m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
             m_tele.configReverseSoftLimitThreshold(tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
@@ -145,6 +150,7 @@ public class Arm extends SubsystemBase {
         }
 
         // double x = Math.cos(Math.toRadians(degrees));
+        SmartDashboard.putNumber("arm length", getArmLength());
     }
 
     public void killSoftLimits() {