Added variable elevator constants, also fixed elevator gear reduction

comp/src/main/java/org/team100/frc2025/Elevator/Elevator.java

@@ -25,11 +25,8 @@
 public class Elevator extends SubsystemBase implements Glassy {
     /** Creates a new Elevator. */
 
-    private static final double kElevatorReduction = 2; // TODO CHANGE THIS
-    private static final double kElevatorWheelDiamater = 1; // TODO CHANGE THIS
-
-    private static final double kWristReduction = 1; // TODO CHANGE THIS
-    private static final double kWristWheelDiameter = 1; // TODO CHANGE THIS
+    private static final double kElevatorReduction = 2; 
+    private static final double kElevatorWheelDiamater = 0.0381; 
 
     private final OutboardLinearPositionServo starboardServo;
     private final OutboardLinearPositionServo portServo;
@@ -45,35 +42,18 @@ public Elevator(
 
         LoggerFactory starboardLogger = child.child("Starboard");
         LoggerFactory portLogger = child.child("Port");
-        LoggerFactory wristLogger = child.child("Wrist");
 
         LoggerFactory starboardMotorLogger = child.child("Starboard Motor");
         LoggerFactory portMotorLogger = child.child("Port Motor");
-        LoggerFactory wristMotorLogger = child.child("Wrist Motor");
 
         int elevatorSupplyLimit = 60;
         int elevatorStatorLimit = 90;
 
-        // PIDConstants elevatorPID = new PIDConstants(2, 0, 0);
         PIDConstants elevatorPID = PIDConstants.makePositionPID(2);
 
         Feedforward100 elevatorFF = Feedforward100.makeKraken6Elevator();
-        // TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(220, 220,
-        // 0.05); // TODO CHANGE THESE
         TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(100, 100, 0.05); // TODO CHANGE THESE
 
-        int wristSupplyLimit = 60;
-        int wristStatorLimit = 90;
-
-        PIDConstants wristPID = PIDConstants.makePositionPID(10);
-
-        Feedforward100 wristFF = Feedforward100.makeKraken6Wrist();
-        PIDController wristPIDController = new PIDController(wristPID.getPositionP(), wristPID.getPositionI(),
-                wristPID.getPositionD());
-        wristPIDController.setTolerance(0.02);
-        wristPIDController.setIntegratorRange(0, 1);
-        TrapezoidProfile100 wristProfile = new TrapezoidProfile100(20, 20, 0.05); // TODO CHANGE THESE
-
         switch (Identity.instance) {
             case FRC_100_ea4 -> {
                 Kraken6Motor starboardMotor = new Kraken6Motor(starboardMotorLogger, starboardID, MotorPhase.FORWARD,
@@ -138,12 +118,20 @@ public void resetWristProfile() {
     }
 
     /**
-     * TODO: calibrate this in meters
      */
     public void setPosition(double x) {
-        // TODO: change gravity depending on position (it gets heavier)
+        if (getPosition() < .9) {
         starboardServo.setPosition(x, 1.3); // 54 max
         portServo.setPosition(x, 1.3); // 54 max
+        } else if (getPosition() > 1.8) {
+            //TODO get these constants
+            starboardServo.setPosition(x, 1.7); // 54 max
+            portServo.setPosition(x, 1.7); // 54 max
+        } else {
+            //TODO get these constants
+            starboardServo.setPosition(x, 1.5); // 54 max
+            portServo.setPosition(x, 1.5); // 54 max
+        }
     }
 
     public void setDutyCycle(double value) {
@@ -154,7 +142,6 @@ public void setDutyCycle(double value) {
     }
 
     /**
-     * TODO: calibrate this in meters
      */
     public double getPosition() {
         return starboardServo.getPosition().orElse(0);

comp/src/main/java/org/team100/frc2025/Elevator/ElevatorDown.java

@@ -20,7 +20,7 @@ public void initialize() {
     @Override
     public void execute() {
         // m_elevator.setDutyCycle(-0.05);
-        m_elevator.setPosition(6); //16 for l3
+        m_elevator.setPosition(0.2286); //16 for l3
     }
 
     @Override

comp/src/main/java/org/team100/frc2025/Elevator/SetElevator.java

@@ -25,7 +25,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_elevator.setPosition(11.2); //24.5 for l3
+    m_elevator.setPosition(0.426); //24.5 for l3
   }
 
   // Called once the command ends or is interrupted.

comp/src/main/java/org/team100/frc2025/RobotContainer.java

@@ -24,6 +24,7 @@
 import org.team100.lib.async.Async;
 import org.team100.lib.async.AsyncFactory;
 import org.team100.lib.commands.drivetrain.DriveToPoseSimple;
+import org.team100.lib.commands.drivetrain.DriveToPoseWithProfile;
 import org.team100.lib.commands.drivetrain.DriveToPoseWithTrajectory;
 import org.team100.lib.commands.drivetrain.DriveToTranslationWithFront;
 import org.team100.lib.commands.drivetrain.FullCycle2;
@@ -251,18 +252,18 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
 
         whileTrue(driverControl::driveToObject,
 
-                // new DriveToPoseWithProfile(
-                // fieldLog,
-                // () -> (Optional.of(m_layout.getTagPose(DriverStation.getAlliance().get(),
-                // 16).get().toPose2d()
-                // .plus(new Transform2d(0, -0.75, new Rotation2d(Math.PI / 2))))),
-                // m_drive,
-                // holonomicController,
-                // profile));
-                new DriveToPoseWithTrajectory(
-                        () -> m_layout.getTagPose(DriverStation.getAlliance().get(), 16).get().toPose2d()
-                                .plus(new Transform2d(0, -1, new Rotation2d(Math.PI / 2))),
-                        m_drive, (start, end) -> planner.movingToRest(start, end), holonomicController, viz));
+                new DriveToPoseWithProfile(
+                fieldLog,
+                () -> (Optional.of(m_layout.getTagPose(DriverStation.getAlliance().get(),
+                16).get().toPose2d()
+                .plus(new Transform2d(0, -0.75, new Rotation2d(Math.PI / 2))))),
+                m_drive,
+                holonomicController,
+                profile));
+                // new DriveToPoseWithTrajectory(
+                //         () -> m_layout.getTagPose(DriverStation.getAlliance().get(), 16).get().toPose2d()
+                //                 .plus(new Transform2d(0, -1, new Rotation2d(Math.PI / 2))),
+//                        m_drive, (start, end) -> planner.movingToRest(start, end), holonomicController, viz));
 
         whileTrue(driverControl::driveOneMeter,
                 // new DriveToPoseWithProfile(

lib/src/main/java/org/team100/lib/config/Camera.java

@@ -74,7 +74,7 @@ public enum Camera {
                     new Rotation3d(0, Math.toRadians(-6), Math.toRadians(60)))),
 
     /**
-     * Funnal 
+     * Funnel 
      */
     FUNNEL("1e5acbaa5a7f9d10",
             new Transform3d(