diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 9f4003f..9564110 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -404,6 +404,7 @@ public static class ElevatorConstants { public static final double ELEVATOR_SPOOL_DIAMETER = 0.04 + 0.003; //0.04m for spool diameter, 0.003 for rope diameter public static final double ELEVATOR_GEAR_RATIO = 3.0; public static final double ELEVATOR_DANGER_ZONE = 0.4180619200456253; + public static final double ELEVATOR_DEFAULT_POSITION_WHEN_DISABLED = 0.46; public static final TunableNumber motionAcceleration = new TunableNumber("Elevator/MotionAcceleration", 200); @@ -426,9 +427,9 @@ public static class ElevatorConstants { public static final TunableNumber L4_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L4", 1.57); public static final TunableNumber P1_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/P1", - 0.79); + 0.50); public static final TunableNumber P2_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/P2", - 1.29); + 0.80); public static final TunableNumber FUNNEL_INTAKE_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/FunnnelIntake", 0.35); public static final TunableNumber ELEVATOR_ZEROING_CURRENT = new TunableNumber("Elevator zeroing current", diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index 5132ef6..06dce6f 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -54,6 +54,15 @@ public ClimberIOReal() { motionMagicConfigs.MotionMagicJerk = ClimberConstants.CLIMBER_JERK.get(); config.withMotionMagic(motionMagicConfigs); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + velocityRotPerSec, + tempCelsius, + appliedVolts, + supplyCurrentAmps, + statorCurrentAmps, + currentPositionRot); + motor.getConfigurator().apply(config); motor.optimizeBusUtilization(); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 3197248..e0f89e3 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -4,6 +4,8 @@ import frc.robot.utils.TunableNumber; import org.littletonrobotics.junction.Logger; +//TODO: change motion logic and reset command afterwards + public class ClimberSubsystem extends SubsystemBase { private final ClimberIO io; private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index de58057..dd5c421 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -54,8 +54,8 @@ public ElevatorIOReal() { currentLimitsConfigs.StatorCurrentLimit = 80.0; currentLimitsConfigs.SupplyCurrentLimit = 30.0; - leader.setPosition(heightToTalonPos(0.4)); - follower.setPosition(heightToTalonPos(0.4)); + leader.setPosition(heightToTalonPos(ELEVATOR_DEFAULT_POSITION_WHEN_DISABLED)); + follower.setPosition(heightToTalonPos(ELEVATOR_DEFAULT_POSITION_WHEN_DISABLED)); MotorOutputConfigs leaderMotorConfigs = new MotorOutputConfigs(); leaderMotorConfigs.NeutralMode = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/utils/DestinationSupplier.java b/src/main/java/frc/robot/utils/DestinationSupplier.java index e0ab5dc..754515e 100644 --- a/src/main/java/frc/robot/utils/DestinationSupplier.java +++ b/src/main/java/frc/robot/utils/DestinationSupplier.java @@ -48,27 +48,19 @@ public void updateChassisSetpoint(chassisSetpoint setpoint) { public double getElevatorSetpoint(boolean useCoral) { this.useCoral = useCoral; if (useCoral) { - switch (currentElevSetpointCoral) { - case L1: - return RobotConstants.ElevatorConstants.L1_EXTENSION_METERS.get(); - case L2: - return RobotConstants.ElevatorConstants.L2_EXTENSION_METERS.get(); - case L3: - return RobotConstants.ElevatorConstants.L3_EXTENSION_METERS.get(); - case L4: - return RobotConstants.ElevatorConstants.L4_EXTENSION_METERS.get(); - default: - return RobotConstants.ElevatorConstants.L2_EXTENSION_METERS.get(); - } + return switch (currentElevSetpointCoral) { + case L1 -> RobotConstants.ElevatorConstants.L1_EXTENSION_METERS.get(); + case L2 -> RobotConstants.ElevatorConstants.L2_EXTENSION_METERS.get(); + case L3 -> RobotConstants.ElevatorConstants.L3_EXTENSION_METERS.get(); + case L4 -> RobotConstants.ElevatorConstants.L4_EXTENSION_METERS.get(); + default -> RobotConstants.ElevatorConstants.L2_EXTENSION_METERS.get(); + }; }else{ - switch (currentElevSetpointPoke) { - case P1: - return RobotConstants.ElevatorConstants.P1_EXTENSION_METERS.get(); - case P2: - return RobotConstants.ElevatorConstants.P2_EXTENSION_METERS.get(); - default: - return RobotConstants.ElevatorConstants.P1_EXTENSION_METERS.get(); - } + return switch (currentElevSetpointPoke) { + case P1 -> RobotConstants.ElevatorConstants.P1_EXTENSION_METERS.get(); + case P2 -> RobotConstants.ElevatorConstants.P2_EXTENSION_METERS.get(); + default -> RobotConstants.ElevatorConstants.P1_EXTENSION_METERS.get(); + }; } }