diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d1a766..e757c01 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -325,11 +325,11 @@ public void configureElevator() { new SetEndEffectorState( elevatorSubsystem, armSubsystem, - SetEndEffectorState.EndEffectorPreset.DOUBLE_SUBSTATION_CONE), + SetEndEffectorState.EndEffectorPreset.DOUBLE_SUBSTATION_CONE,false), new SetEndEffectorState( elevatorSubsystem, armSubsystem, - SetEndEffectorState.EndEffectorPreset.DOUBLE_SUBSTATION_CUBE), + SetEndEffectorState.EndEffectorPreset.DOUBLE_SUBSTATION_CUBE, false), this::isCurrentPieceCone)); operator.a().onTrue(new ZeroArmSensor(armSubsystem)); // zero driver // zero/cube/fallen cone diff --git a/src/main/java/frc/robot/elevator/commands/SetElevatorPosition.java b/src/main/java/frc/robot/elevator/commands/SetElevatorPosition.java index d8e4286..b03f2e3 100644 --- a/src/main/java/frc/robot/elevator/commands/SetElevatorPosition.java +++ b/src/main/java/frc/robot/elevator/commands/SetElevatorPosition.java @@ -22,10 +22,12 @@ public class SetElevatorPosition extends ProfiledPIDCommand { private ElevatorPreset elevatorPreset; /** Constructor for setting the elevator to a setpoint in the parameters */ - public SetElevatorPosition(Elevator elevatorSubsystem, double setpointPosition){ - this(elevatorSubsystem,setpointPosition,false); + public SetElevatorPosition(Elevator elevatorSubsystem, double setpointPosition) { + this(elevatorSubsystem, setpointPosition, false); } - public SetElevatorPosition(Elevator elevatorSubsystem, double setpointPosition, boolean accurate) { + + public SetElevatorPosition( + Elevator elevatorSubsystem, double setpointPosition, boolean accurate) { super( new ProfiledPIDController(kElevatorP, kElevatorI, kElevatorD, kElevatorConstraints), elevatorSubsystem::getElevatorPosition, @@ -38,7 +40,7 @@ public SetElevatorPosition(Elevator elevatorSubsystem, double setpointPosition, this.setpointPosition = setpointPosition; this.elevatorSubsystem = elevatorSubsystem; - if (!accurate){ + if (!accurate) { getController().setTolerance(kTolerancePosition, kToleranceVelocity); } else { getController().setTolerance(kAccurateTolerancePosition, kToleranceVelocity); diff --git a/src/main/java/frc/robot/elevator/commands/SetEndEffectorState.java b/src/main/java/frc/robot/elevator/commands/SetEndEffectorState.java index 7bb8588..0521cb0 100644 --- a/src/main/java/frc/robot/elevator/commands/SetEndEffectorState.java +++ b/src/main/java/frc/robot/elevator/commands/SetEndEffectorState.java @@ -19,7 +19,7 @@ public class SetEndEffectorState extends ParentCommand { private Elevator elevatorSubsystem; private Rotation2d armAngle; private double elevatorExtension; - boolean accurate = false; + boolean accurate; public enum EndEffectorPreset { // scoring @@ -48,19 +48,22 @@ public enum EndEffectorPreset { public final Arm.ArmPreset armPreset; public final Elevator.ElevatorPreset elevatorPreset; - EndEffectorPreset(Arm.ArmPreset armPreset, Elevator.ElevatorPreset elevatorPreset) { this.armPreset = armPreset; this.elevatorPreset = elevatorPreset; } } - public SetEndEffectorState( Elevator elevatorSubsystem, Arm armSubsystem, EndEffectorPreset endEffectorPreset) { this(elevatorSubsystem, armSubsystem, endEffectorPreset, false); } - public SetEndEffectorState( Elevator elevatorSubsystem, Arm armSubsystem, EndEffectorPreset endEffectorPreset, boolean accurate){ + + public SetEndEffectorState( + Elevator elevatorSubsystem, + Arm armSubsystem, + EndEffectorPreset endEffectorPreset, + boolean accurate) { super(); this.armSubsystem = armSubsystem; this.elevatorSubsystem = elevatorSubsystem; @@ -72,23 +75,13 @@ public SetEndEffectorState( Elevator elevatorSubsystem, Arm armSubsystem, EndEff @Override public void initialize() { - if (!accurate){ - addChildCommands( - Commands.sequence( - new SetElevatorPosition(elevatorSubsystem,elevatorExtension), - Commands.deadline( - new SetArmAngle(armSubsystem,armAngle), - new KeepElevatorAtPosition( - elevatorSubsystem,elevatorExtension)))); // may not work to add like this - } else { addChildCommands( - Commands.sequence( - new SetElevatorPosition(elevatorSubsystem,elevatorExtension,true), - Commands.deadline( - new SetArmAngle(armSubsystem,armAngle), - new KeepElevatorAtPosition( - elevatorSubsystem,elevatorExtension)))); // may not work to add like this - } + Commands.sequence( + new SetElevatorPosition(elevatorSubsystem, elevatorExtension, accurate), + Commands.deadline( + new SetArmAngle(armSubsystem, armAngle), + new KeepElevatorAtPosition( + elevatorSubsystem, elevatorExtension)))); // may not work to add like this super.initialize(); }