Reordering of logic for #24

Only do work if we need to move.
Also remove redundant deadzone checking

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -59,8 +59,8 @@ private void setDefaultCommands() {
     // () -> driverController.getRawButton(OI.Driver.slowDriveButton)
     // ));
 
-    arm.setDefaultCommand(new ArmTeleop(arm, 
-      () -> DeadzonedAxis(inputProcessing(getStickValue(manipulatorController, Axis.kLeftY)))));
+    arm.setDefaultCommand(new ArmTeleop(arm,
+      () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY);
   }
 
   private void setBindingsDriver() {
@@ -123,7 +123,7 @@ public Command getAutonomousCommand() {
   /**
    * Flips an axis' Y coordinates upside down, but only if the select axis is a
    * joystick axis
-   * 
+   *
    * @param hid  The controller/plane joystick the axis is on
    * @param axis The processed axis
    * @return The processed value.
@@ -135,7 +135,7 @@ private double getStickValue(GenericHID hid, Axis axis) {
   /**
    * Processes an input from the joystick into a value between -1 and 1,
    * sinusoidally instead of linearly
-   * 
+   *
    * @param value The value to be processed.
    * @return The processed value.
    */
@@ -151,25 +151,25 @@ private double inputProcessing(double value) {
   /**
    * Combines both getStickValue and inputProcessing into a single function for
    * processing joystick outputs
-   * 
+   *
    * @param hid  The controller/plane joystick the axis is on
    * @param axis The processed axis
    * @return The processed value.
    */
   private double ProcessedAxisValue(GenericHID hid, Axis axis) {
-    return inputProcessing(getStickValue(hid, axis));
+    return DeadzonedAxis(inputProcessing(getStickValue(hid, axis)));
   }
 
   /**
    * Returns zero if a axis input is inside the deadzone
-   * 
+   *
    * @param hid  The controller/plane joystick the axis is on
    * @param axis The processed axis
    * @return The processed value.
    */
   private double DeadzonedAxis(double axOut) {
-    return (-OI.JOY_THRESH < axOut && axOut < OI.JOY_THRESH) ? 0.0 : axOut;
+    return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut;
   }
 
-  
+
 }

src/main/java/org/carlmontrobotics/commands/ArmTeleop.java

@@ -47,13 +47,18 @@ public void execute() {
     // use trapazoid math and controllerMoveArm method from arm subsytem to apply
     // voltage to the motor
     double speeds = getRequestedSpeeds();
-    double currTime = Timer.getFPGATimestamp();
-    double deltaT = currTime - lastTime;
-    lastTime = currTime;
-    if (speeds == 0){// If no input, return so that the actual goal set by the instant command wouldn't be overriden, can be further explained in this issue: https://github.com/DeepBlueRobotics/RobotCode2024/issues/24
+
+    if (speeds == 0){//if no input, don't set any goals.
+      lastTime = Timer.getFPGATimestamp();
       return;
     }
-    double goalArmRad = goalState.position + speeds * deltaT;
+
+    double currTime = Timer.getFPGATimestamp();
+    double deltaT = currTime - lastTime;//only move by a tick of distance at once
+    lastTime = currTime;
+
+    double goalArmRad = goalState.position + speeds * deltaT;//speed*time = dist
+
     goalArmRad = MathUtil.clamp(goalArmRad, UPPER_ANGLE_LIMIT_RAD, LOWER_ANGLE_LIMIT_RAD);
     goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD,
         armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
@@ -64,14 +69,7 @@ public void execute() {
   }
 
   public double getRequestedSpeeds() {
-    double rawArmVel;
-    if (Math.abs(joystick.getAsDouble()) <= Constants.OI.JOY_THRESH) {
-      rawArmVel = 0.0;
-    } else {
-      rawArmVel = armSubsystem.getMaxAccelRad() * joystick.getAsDouble();
-    }
-
-    return rawArmVel;
+    return armSubsystem.getMaxAccelRad() * joystick.getAsDouble();
   }
 
   // Called once the command ends or is interrupted.