adding changes to ArmTeleop

src/main/java/org/carlmontrobotics/Constants.java

@@ -72,6 +72,7 @@ public static final class IntakeShooter {
 	}
 
 	public static final class OI {
+		public static final double JOY_THRESH = 0.01;
         public static final class Driver {
             public static final int port = 0;
         }

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

@@ -4,50 +4,66 @@
 
 package org.carlmontrobotics.commands;
 
+import org.carlmontrobotics.Constants;
+import static org.carlmontrobotics.Constants.Arm.*;
 
 import java.util.function.DoubleSupplier;
 
 import org.carlmontrobotics.subsystems.Arm;
 
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 
+import edu.wpi.first.wpilibj2.command.Command;
 
-public class ArmTeleop extends CommandBase {
+import edu.wpi.first.wpilibj.Timer;
+
+import edu.wpi.first.math.MathUtil;
+
+
+public class ArmTeleop extends Command {
   private final DoubleSupplier joystick; 
   private final Arm armSubsystem;
+  private double lastTime = 0;
+
   /** Creates a new ArmTeleop. */
   public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) {
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.armSubsystem = armSubsystem);
     joystick = joystickSupplier;
-    this.armSubsystem = armSubsystem;
-    addRequirements(armSubsystem);
   }
-  public void controllerMoveArm(DoubleSupplier rightJoystick) {
-    /*
-    // move the arm around based off the right joystick movement on the manipulator joystick
-    //use the trapezoid thingy from robot code 2023
-    //Math below | Summary: Take in controller Y axis as a double then calculate amount of volts needed to pass to the arm and when to stop based off of the controller movement. Done so by finding the constraints of the arm, translating the controller numbers, and finding how many volts and when to stop using feedvolts and PID
-      double kgv = getKg();
-      double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0);
-      double armPIDVolts = armPID.calculate(getArmPos(), state.position);
-      if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) ||
-          (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) {
-            forbFlag = true;  
-          armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
-          */
 
-
-  }
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    armSubsystem.setArmGoal(armSubsystem.getArmPos(), 0);
+    lastTime = Timer.getFPGATimestamp();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     double value = joystick.getAsDouble();
     // 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;
+
+    double goalArmRad = armSubsystem.getCurrentArmGoal().position + speeds * deltaT; // fix later
+
+    goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
+
+    lastTime = currTime;
+  }
+
+  public double getRequestedSpeeds() {
+    double rawArmVel;
+    if (Math.abs(joystick.getAsDouble()) <= Constants.OI.JOY_THRESH) {
+      rawArmVel = 0.0;
+    } else {
+      rawArmVel = MAX_FF_VEL * joystick.getAsDouble();
+    }
+
+    return rawArmVel;
   }
 
   // Called once the command ends or is interrupted.