Elevator Accurate

Team3256 · Oct 8, 2023 · ebc1180 · ebc1180
1 parent 6e34434
commit ebc1180
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 26 deletions.
diff --git 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
@@ -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
@@ -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();
   }