Fixing loop to command

Author: Nins97

src/main/java/frc/robot/FieldConstants.java

@@ -35,8 +35,8 @@ public class FieldConstants {
   public static final class StagingLocations {
     public static double centerlineX = fieldLength / 2.0;
     // TODO: FIX THIS
-    public static final double stagingThreshold = 0;
-    public static final double ampThreshold = 0;
+    public static final double stagingThreshold = 5;
+    public static final double ampThreshold = 5;
 
     // need to update
     public static double centerlineFirstY = Units.inchesToMeters(29.638);

src/main/java/frc/robot/commands/elevator/AmpPlaceCommand.java

@@ -1,11 +1,10 @@
 package frc.robot.commands.elevator;
 
-import static frc.robot.Constants.ElevatorConstants.*;
+import static frc.robot.Constants.ScoringConstants.*;
 import static frc.robot.Constants.WristConstants.*;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.shooter.ShootAtAngleCommand;
-import frc.robot.commands.wrist.WristToPositionCommand;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.transport.TransportSubsystem;
@@ -27,8 +26,8 @@ public AmpPlaceCommand(
     this.wristSubsystem = wristSubsystem;
     this.transportSubsystem = transportSubsystem;
     addCommands(
-        new ElevatorToPositionCommand(elevatorSubsystem, ELEVATOR_AMP_POS),
-        new WristToPositionCommand(wristSubsystem, WRIST_AMP_POSITION),
+        elevatorSubsystem.setElevatorPositionCommand(AMP_ELEVATOR_HEIGHT),
+        wristSubsystem.setPosiitonCommand(WRIST_AMP_POSITION),
         new ShootAtAngleCommand(
             shooterSubsystem, transportSubsystem, wristSubsystem, WRIST_AMP_POSITION));
   }

src/main/java/frc/robot/commands/elevator/ElevatorToPositionCommand.java

@@ -1,10 +1,8 @@
 package frc.robot.commands.elevator;
 
-import static frc.robot.Constants.ElevatorConstants.*;
 import static frc.robot.Constants.WristConstants.*;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.commands.wrist.WristToPositionCommand;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.wrist.WristSubsystem;
 
@@ -18,7 +16,7 @@ public ElevatorWristParallelCommand(
     this.wristSubsystem = wristSubsystem;
 
     addCommands(
-        new WristToPositionCommand(wristSubsystem, WRIST_MIN),
-        new ElevatorToPositionCommand(elevatorSubsystem, 0));
+        wristSubsystem.setPosiitonCommand(WRIST_MIN),
+        elevatorSubsystem.setElevatorPositionCommand(0));
   }
 }

src/main/java/frc/robot/commands/elevator/ElevatorWristParallelCommand.java

@@ -4,7 +4,6 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.commands.wrist.WristToPositionCommand;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.wrist.WristSubsystem;
 
@@ -19,7 +18,6 @@ public RunWristAndFlywheels(
     this.shooterSubsystem = shooterSubsystem;
 
     addCommands(
-        new WristToPositionCommand(wristSubsystem, shootingAngle),
-        shooterSubsystem.runFlyRPM(SHOOTING_RPM));
+        wristSubsystem.setPosiitonCommand(shootingAngle), shooterSubsystem.runFlyRPM(SHOOTING_RPM));
   }
 }

src/main/java/frc/robot/commands/shooter/RunWristAndFlywheels.java

@@ -77,15 +77,18 @@ private void configMotors() {
             () -> elevatorEncoder.setVelocityConversionFactor(METERS_PER_REV / 60),
             () -> elevatorEncoder.getVelocityConversionFactor() == METERS_PER_REV / 60,
             MiscConstants.CONFIGURATION_ATTEMPTS);
-    faultInitializing |= RaiderUtils.applyAndCheckRev(elevatorMotor::burnFlashWithDelay, () -> true, MiscConstants.CONFIGURATION_ATTEMPTS);
+    faultInitializing |=
+        RaiderUtils.applyAndCheckRev(
+            elevatorMotor::burnFlashWithDelay, () -> true, MiscConstants.CONFIGURATION_ATTEMPTS);
 
-    elevatorEventEntry.append("Elevator motor initialized" + (faultInitializing ? " with faults" : ""));
+    elevatorEventEntry.append(
+        "Elevator motor initialized" + (faultInitializing ? " with faults" : ""));
     elevatorAlert.set(faultInitializing);
   }
 
   public void atBottomLimit() {
     if (bottomLimit.get()) {
-      leftMotor.setPosition(0);
+      elevatorEncoder.setPosition(0);
     }
   }
 
@@ -99,19 +102,19 @@ public boolean atGoal() {
   }
 
   public void setEncoderPosition(double position) {
-    leftMotor.setPosition(position);
+    elevatorEncoder.setPosition(position);
   }
 
   public void setVoltage(double voltage) {
-    leftMotor.setVoltage(voltage);
+    elevatorMotor.setVoltage(voltage);
   }
 
   public double getPosition() {
-    return leftMotor.getPosition().getValue();
+    return elevatorEncoder.getPosition();
   }
 
   public void stopMove() {
-    leftMotor.setVoltage(0);
+    elevatorMotor.setVoltage(0);
   }
 
   public Command runElevatorCommand(double voltage) {
@@ -127,22 +130,22 @@ public Command runElevatorCommand(double voltage) {
   }
 
   public Command setElevatorPositionCommand(double position) {
-    return this.run(() -> this.setDesiredPosition(position));
+    return this.run(
+        () -> {
+          controller.setGoal(position);
+          double feedbackOutput = controller.calculate(getPosition());
+          TrapezoidProfile.State currentSetpoint = controller.getSetpoint();
+
+          setVoltage(
+              feedbackOutput + feedforward.calculate(getPosition(), currentSetpoint.velocity));
+        });
   }
 
   @Override
-  public void periodic() {
-    double feedbackOutput = controller.calculate(getPosition());
-
-    TrapezoidProfile.State currentSetpoint = controller.getSetpoint();
-    double combinedOutput = feedbackOutput + feedforward.calculate(currentSetpoint.velocity);
-    leftMotor.setVoltage(combinedOutput);
-    atBottomLimit();
-    logValues();
-  }
+  public void periodic() {}
 
   private void logValues() {
-    leftMotor.logValues();
+    elevatorMotor.logValues();
     if (FF_GAINS.hasChanged()) {
       feedforward = FF_GAINS.createFeedforward();
     }

src/main/java/frc/robot/commands/wrist/WristToPositionCommand.java

@@ -143,7 +143,15 @@ private double getPosition() {
   }
 
   public Command setRotationGoalCommand(Rotation2d goal) {
-    return this.run(() -> this.setRotationGoal(goal)).until(rotationController::atGoal);
+    return this.run(
+        () -> {
+          this.setRotationGoal(goal);
+          double feedbackOutput = rotationController.calculate(getPosition());
+          TrapezoidProfile.State currentSetpoint = rotationController.getSetpoint();
+
+          setRotationVoltage(
+              feedbackOutput + rotationFF.calculate(getPosition(), currentSetpoint.velocity));
+        });
   }
 
   public Command setFeederVoltageCommand(double voltage) {

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -35,7 +35,7 @@ public class WristSubsystem extends SubsystemBase {
           "/wrist/pid", WRIST_PID_GAINS, TRAPEZOIDAL_PROFILE_GAINS);
 
   private final DoubleTelemetryEntry wristVoltageReq =
-      new DoubleTelemetryEntry("/wrist/voltage", TUNING_MODE);
+      new DoubleTelemetryEntry("/wrist/voltage", true);
   private final EventTelemetryEntry wristEventEntry = new EventTelemetryEntry("/wrist/events");
 
   public WristSubsystem() {