diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index ed8e584..9f720c9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -186,7 +186,7 @@ public void periodic() { } io.runPosition( - setpoint.position / drumRadius / numStages, + setpoint.position / drumRadius, feedforward.calculateWithVelocities(setpoint.velocity, previousVelocity)); // Check at goal @@ -284,7 +284,7 @@ public Command homingSequence() { @AutoLogOutput(key = "Elevator/MeasuredHeightMeters") public double getPositionMeters() { - return inputs.positionRads * drumRadius * numStages; + return inputs.positionRads * drumRadius; } public double getGoalMeters() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 3973822..8479a03 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -5,18 +5,18 @@ public class ElevatorConstants { // Pitch from Floor to Elevator - public static final Rotation2d elevatorPitch = Rotation2d.fromDegrees(84.5); + public static final Rotation2d elevatorPitch = Rotation2d.fromDegrees(86.0); // Pitch from Elevator to Dispenser - public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(73.0); + public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(61.6); - public static final double originToBaseHeightMeters = Units.inchesToMeters(5.149922); + public static final double originToBaseHeightMeters = + Units.inchesToMeters(6.288); // <-- highest edge on slam plate + // 6.148 to the lowest edge, 6.218 to centroid - public static final double drumRadius = Units.inchesToMeters(1.751 / 2.0); - public static final double gearing = 3.0; - public static final int numStages = 2; + public static final double drumRadius = Units.inchesToMeters(1.0); + public static final double gearing = 4.0; - // public static final double maxTravel = Units.inchesToMeters(42.244094); // TODO - public static final double maxTravel = Units.inchesToMeters(42); // TODO + public static final double maxTravel = Units.inchesToMeters(41); // TODO public static final double carriageMassKg = Units.lbsToKilograms(6.0); public static final double stagesMassKg = Units.lbsToKilograms(12.0);