Tim md2

src/main/java/frc/robot/Robot.java

@@ -148,7 +148,6 @@ public void teleopInit() {
       CommandScheduler.getInstance()
           .schedule(new ZeroElevatorHardStop(ElevatorSubsystem.getInstance()));
     }
-
     // CommandScheduler.getInstance();
     // .schedule(zeroArm); // TODO: Fix this to not expose the CommandScheduler
   }

src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java

@@ -33,4 +33,4 @@ public void end(boolean interrupted) {}
   public boolean isFinished() {
     return elevatorSubsystem.isAtPosition();
   }
-}
+}

src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorHeight.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ElevatorCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
+import frc.robot.subsystems.ElevatorSubsystem;
+
+/** An example command that uses an example subsystem. */
+public class SetElevatorHeight extends Command {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final ElevatorSubsystem m_elevator;
+
+  private ElevatorPositions position;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public SetElevatorHeight(ElevatorSubsystem elevator, ElevatorPositions position) {
+    m_elevator = elevator;
+    this.position = position;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(elevator);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_elevator.setPosition(position.getHeight());
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_elevator.isAtPosition();
+  }
+}

src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java

@@ -54,4 +54,4 @@ public boolean isFinished() {
 
     return timesExceededCurrent >= 10;
   }
-}
+}

src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java

@@ -28,14 +28,14 @@ public void execute() {}
   @Override
   public void end(boolean interrupted) {
     DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false);
-    elevatorSubsystem.resetPositionFiltered();
+    // elevatorSubsystem.resetPositionFiltered();
   }
 
   @Override
   public boolean isFinished() {
     DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition);
     boolean inPosition = elevatorSubsystem.isAtPosition() && elevatorSubsystem.atIntake();
-    DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition);
+    // DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition);
 
     if (inPosition) {
       ticksAtPosition++;

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

@@ -14,19 +14,20 @@
 import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANrange;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
+
 import dev.doglog.DogLog;
 import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.ElevatorConstants;
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.util.LoggedTalonFX;
 
-// import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 public class ElevatorSubsystem extends SubsystemBase {
   private static ElevatorSubsystem instance;
 
@@ -42,19 +43,14 @@ public class ElevatorSubsystem extends SubsystemBase {
   private ElevatorPositions currentLevel;
   private CANrange distance; // Time of Flight (ToF) sensor
 
+  private float tolerance;
+
   private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0);
   private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0);
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
 
-  private ElevatorSubsystem() {
-    // Initialize motors
-    elevatorZeroed = false;
-    elevatorFilter = LinearFilter.singlePoleIIR(0.1, 0.02);
-
-    distance =
-        new CANrange(
-            ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME);
-
+  /** Creates a new ExampleSubsystem. */
+  public ElevatorSubsystem() {
     motor1 =
         new LoggedTalonFX(
             "subsystems/Elevator/motor1",
@@ -65,7 +61,6 @@ private ElevatorSubsystem() {
             "subsystems/Elevator/motor2",
             ElevatorConstants.MOTOR2_PORT,
             Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME);
-
     currentLevel = ElevatorPositions.Intake;
 
     // Set up motor followers and deal with inverted motors
@@ -121,9 +116,9 @@ private ElevatorSubsystem() {
 
     m1Config.apply(moc);
     m2Config.apply(moc);
-
+    
     master = motor1;
-    currentHeightToF = elevatorFilter.calculate(getToFDistance());
+    currentHeightToF = 0f;//elevatorFilter.calculate(getToFDistance());
     resetPositionFiltered();
   }
 
@@ -177,20 +172,6 @@ public void reduceCurrentLimits() {
     master.updateCurrentLimits(30, 10);
   }
 
-  public void resetCurrentLimits() {
-    master.updateCurrentLimits(
-        Constants.ElevatorConstants.STATOR_CURRENT_LIMIT,
-        Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT);
-  }
-
-  public void resetElevatorPositionToZero() {
-    master.setPosition(0);
-    // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0));
-    // master.setPosition(0);
-    // master.setControl(controlRequest.withPosition(0).withSlot(0));
-    // master.setPosition(0);
-  }
-
   public boolean checkCurrent() {
     double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude());
     double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude());
@@ -203,24 +184,38 @@ public boolean checkCurrent() {
     return false;
   }
 
-  public double getError() {
-    return currentLevel.height
-            * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS
-            / Constants.ElevatorConstants.CARRAIGE_UPDUCTION
-        - master.getPosition().getValueAsDouble();
+  /**
+   * Example command factory method.
+   *
+   * @return a command
+   */
+  public Command exampleMethodCommand() {
+    // Inline construction of command goes here.
+    // Subsystem::RunOnce implicitly requires `this` subsystem.
+    return runOnce(
+        () -> {
+          /* one-time action goes here */
+        });
   }
 
-  public ElevatorPositions getLevel() {
-    return currentLevel;
+  /**
+   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
+   *
+   * @return value of some boolean subsystem state, such as a digital sensor.
+   */
+  public boolean exampleCondition() {
+    // Query some boolean state, such as a digital sensor.
+    return false;
   }
 
-  public boolean atIntake() {
-    return currentLevel.equals(ElevatorPositions.Intake);
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
   }
 
-  public void elevateTo(ElevatorPositions level) {
-    this.currentLevel = level;
-    this.setPosition(level.height);
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
   }
 
   public void setPosition(double height) {
@@ -231,11 +226,40 @@ public void setPosition(double height) {
                     * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS
                     / ElevatorConstants.CARRAIGE_UPDUCTION)
             .withSlot(0));
-    DogLog.log(
-        "subsystems/Elevator/elevatorSetpoint(rot)",
-        height
+  }
+
+  public void elevateTo(ElevatorPositions level) {
+    this.currentLevel = level;
+    this.setPosition(level.height);
+  }
+
+  public ElevatorPositions getLevel() {
+    return currentLevel;
+  }
+
+  public boolean atIntake() {
+    return currentLevel.equals(ElevatorPositions.Intake);
+  }
+
+  public void resetCurrentLimits() {
+    master.updateCurrentLimits(
+        Constants.ElevatorConstants.STATOR_CURRENT_LIMIT,
+        Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT);
+  }
+
+  public void resetElevatorPositionToZero() {
+    master.setPosition(0);
+    // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0));
+    // master.setPosition(0);
+    // master.setControl(controlRequest.withPosition(0).withSlot(0));
+    // master.setPosition(0);
+  }
+
+  public double getError() {
+    return currentLevel.height
             * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS
-            / ElevatorConstants.CARRAIGE_UPDUCTION);
+            / Constants.ElevatorConstants.CARRAIGE_UPDUCTION
+        - master.getPosition().getValueAsDouble();
   }
 
   public void ElevatorTorqueMode() {
@@ -261,8 +285,9 @@ public boolean canFunnelTransferCoralToScoring() {
   public double getToFDistance() {
     // 0.11 is the sensor offset
     DogLog.log(
-        "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble());
-    return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET;
+        "subsystems/Elevator/ToF/DistanceNoOffset", 0f/*distance.getDistance().getValueAsDouble()*/);
+    return 0f;
+    //return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET;
   }
 
   public boolean isElevatorZeroed() {
@@ -272,58 +297,4 @@ public boolean isElevatorZeroed() {
   public void elevatorHasBeenZeroed() {
     elevatorZeroed = true;
   }
-
-  @Override
-  public void periodic() {
-    currentHeightToF = elevatorFilter.calculate(getToFDistance());
-    // Time of Flight Sensor
-    DogLog.log("subsystems/Elevator/getError", getError());
-    DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance());
-    DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected());
-    DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF);
-
-    DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition());
-    DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition());
-    DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight());
-    DogLog.log(
-        "subsystems/Elevator/targetHeightRot",
-        currentLevel.getHeight()
-            * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS
-            / Constants.ElevatorConstants.CARRAIGE_UPDUCTION);
-    DogLog.log(
-        "subsystems/Elevator/currentHeightDist",
-        master.getPosition().getValueAsDouble()
-            * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE
-            * Constants.ElevatorConstants.CARRAIGE_UPDUCTION);
-    DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble());
-    DogLog.log(
-        "subsystems/Elevator/command",
-        this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName());
-    DogLog.log(
-        "subsystems/Elevator/resetPositionBoolean",
-        this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake));
-    DogLog.log(
-        "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake));
-    DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString());
-    DogLog.log(
-        "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble());
-    DogLog.log(
-        "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble());
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // Simulate encoder behavior based on motor speed
-    double simulatedSpeed = master.getVelocity().getValueAsDouble();
-    double currentPosition = master.getPosition().getValueAsDouble();
-
-    // Update simulated position based on speed (simplified example)
-    double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop
-    master.setPosition(
-        newPosition); // Alarming to have this since running this on the robot will lead to
-
-    // Log simulation data for debugging
-    DogLog.log("Simulated Position", newPosition);
-    DogLog.log("Simulated Speed", simulatedSpeed);
-  }
 }

vendordeps/DogLog.json

@@ -3,7 +3,7 @@
         {
             "groupId": "com.github.jonahsnider",
             "artifactId": "doglog",
-            "version": "2025.3.0"
+            "version": "2025.9.2"
         }
     ],
     "fileName": "DogLog.json",
@@ -15,6 +15,6 @@
         "https://jitpack.io"
     ],
     "cppDependencies": [],
-    "version": "2025.3.0",
+    "version": "2025.9.2",
     "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4"
 }