Add getter methods and target checking for telemetry

src/main/java/frc/robot/RobotContainer.java

@@ -28,7 +28,7 @@ private void configureBindings() {
     // When left trigger is pressed the arm will go to vertical position, and when not pressed it
     // will
     // stop
-    joystick.leftTrigger().onTrue(arm.vertical());
+    joystick.leftTrigger().whileTrue(arm.vertical());
 
     // When right trigger is pressed the flywheel will run at a fast speed, and when not pressed it
     // will spin slow

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

@@ -4,6 +4,9 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
+
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
@@ -13,6 +16,7 @@
 import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -28,6 +32,9 @@ public class Arm extends SubsystemBase {
   // Position output control for the arm
   private final MotionMagicVoltage positionOut = new MotionMagicVoltage(0);
 
+  // Tolerance for the arm position
+  private final Angle tolerance = Degrees.of(1.0);
+
   public Arm() {
     // Create and apply the configuration for the leader motor
     TalonFXConfiguration config = new TalonFXConfiguration();
@@ -60,9 +67,9 @@ public void periodic() {
   /**
    * Sets the position for the arm.
    *
-   * @param position The position to set in rotations.
+   * @param position The position to set.
    */
-  public void setPosition(double position) {
+  public void setPosition(Angle position) {
     // Apply the position output to the leader motor
     leader.setControl(positionOut.withPosition(position));
   }
@@ -74,7 +81,7 @@ public void setPosition(double position) {
    */
   public Command vertical() {
     // Command to run the arm to vertical position and stop it afterward
-    return runOnce(() -> setPosition(0.25));
+    return runOnce(() -> setPosition(Degrees.of(90)));
   }
 
   /**
@@ -84,7 +91,7 @@ public Command vertical() {
    */
   public Command horizontal() {
     // Command to run the arm to horizontal position and stop it afterward
-    return runOnce(() -> setPosition(0.5));
+    return runOnce(() -> setPosition(Rotations.of(0.5)));
   }
 
   /**
@@ -96,6 +103,36 @@ public Command stopCommand() {
     return runOnce(() -> stop());
   }
 
+  /**
+   * Checks if the arm is at its target position.
+   *
+   * @return true if at target position, false otherwise
+   */
+  public boolean isAtTarget() {
+    // Check if the arm's position is near the target position
+    return getPosition().isNear(getTargetPosition(), tolerance);
+  }
+
+  /**
+   * Gets the current position of the arm.
+   *
+   * @return The current position of the arm.
+   */
+  public Angle getPosition() {
+    // Get the current position of the arm from the CANcoder
+    return encoder.getPosition().getValue();
+  }
+
+  /**
+   * Gets the target position for the arm.
+   *
+   * @return The target position of the arm.
+   */
+  public Angle getTargetPosition() {
+    // Return the target position
+    return positionOut.getPositionMeasure();
+  }
+
   // Stop the arm motor
   public void stop() {
     leader.stopMotor();

src/main/java/frc/robot/subsystems/Flywheel.java

@@ -4,13 +4,17 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -26,6 +30,9 @@ public class Flywheel extends SubsystemBase {
   // Velocity output control for the flywheel
   private final MotionMagicVelocityVoltage velocityOut = new MotionMagicVelocityVoltage(0);
 
+  // Tolerance for the flywheel velocity
+  private final AngularVelocity tolerance = RotationsPerSecond.of(0.25);
+
   public Flywheel() {
     // Set the follower to follow the leader motor
     follower.setControl(new Follower(leader.getDeviceID(), true));
@@ -55,9 +62,9 @@ public void periodic() {
   /**
    * Sets the velocity for the flywheel.
    *
-   * @param velocity The velocity to set in rotations per second.
+   * @param velocity The velocity to set.
    */
-  public void setVelocity(double velocity) {
+  public void setVelocity(AngularVelocity velocity) {
     // Apply the velocity output to the leader motor
     leader.setControl(velocityOut.withVelocity(velocity));
   }
@@ -69,7 +76,7 @@ public void setVelocity(double velocity) {
    */
   public Command runSlow() {
     // Command to run the flywheel at a slow speed
-    return runOnce(() -> setVelocity(25));
+    return runOnce(() -> setVelocity(RotationsPerSecond.of(50)));
   }
 
   /**
@@ -79,7 +86,7 @@ public Command runSlow() {
    */
   public Command runFast() {
     // Command to run the flywheel at a fast speed
-    return runOnce(() -> setVelocity(75));
+    return runOnce(() -> setVelocity(DegreesPerSecond.of(3600)));
   }
 
   /**
@@ -91,6 +98,38 @@ public Command stopCommand() {
     return runOnce(() -> stop());
   }
 
+  /**
+   * Checks if the flywheel is at its target speed.
+   *
+   * @return true if at target speed, false otherwise
+   */
+  public boolean isAtTarget() {
+    return getVelocity()
+        .isNear(
+            getTargetVelocity(),
+            tolerance); // Check if the current velocity is near the target velocity
+  }
+
+  /**
+   * Gets the current velocity of the flywheel.
+   *
+   * @return The current velocity of the flywheel.
+   */
+  public AngularVelocity getVelocity() {
+    // Get the current velocity of the flywheel
+    return leader.getVelocity().getValue();
+  }
+
+  /**
+   * Gets the target velocity for the flywheel.
+   *
+   * @return The target velocity of the flywheel.
+   */
+  public AngularVelocity getTargetVelocity() {
+    // Return the target velocity
+    return velocityOut.getVelocityMeasure();
+  }
+
   // Stop the flywheel motors
   public void stop() {
     leader.stopMotor();