Reordered Controls in Superstructure

frc3512 · Feb 23, 2024 · acb4250 · acb4250
1 parent 50cb530
commit acb4250
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 46 deletions.
diff --git a/src/main/java/frc3512/robot/Constants.java b/src/main/java/frc3512/robot/Constants.java
@@ -73,7 +73,7 @@ public static final class ArmConstants {
 
     public static final double stowPosition = 0.4;
     public static final double closeShootingPosition = 0.22;
-    public static final double farShootingPosition = 0.7;
+    public static final double farShootingPosition = 0.35;
     public static final double ampPosition = 0.95;
     public static final double intakePosition = 0.17;
   }
@@ -88,6 +88,7 @@ public static final class ElevatorConstants {
 
     public static final double stowPosition = 0.25;
     public static final double outPosition = -1.55;
+    public static final double ampPosition = -0.5;
     public static final double kMaxVelocityRadPerSecond = 10.0;
     public static final double kMaxAccelerationRadPerSecSquared = 15.0;
 

diff --git a/src/main/java/frc3512/robot/subsystems/Elevator.java b/src/main/java/frc3512/robot/subsystems/Elevator.java
@@ -85,6 +85,11 @@ public void outElevator() {
     enable();
   }
 
+  public void ampElevator() {
+    setGoal(ElevatorConstants.ampPosition);
+    enable();
+  }
+
   public void stopElevator() {
     elevatorMotor.set(0);
     disable();

diff --git a/src/main/java/frc3512/robot/subsystems/Superstructure.java b/src/main/java/frc3512/robot/subsystems/Superstructure.java
@@ -47,39 +47,28 @@ public Superstructure() {
   public void configureBindings() {
     driverXbox.x().onTrue(new InstantCommand(() -> swerve.zeroGyro()));
 
-    // Shooter/intake controls
-    appendageJoystick.button(3).onTrue(new InstantCommand(() -> shootake.i_wanna_intake()));
-    appendageJoystick.button(3).onFalse(new InstantCommand(() -> shootake.i_dont_wanna_intake()));
-
-    appendageJoystick.button(7).onTrue(new InstantCommand(() -> shootake.i_wanna_outtake()));
-    appendageJoystick.button(7).onFalse(new InstantCommand(() -> shootake.i_dont_wanna_outtake()));
-
-    appendageJoystick.button(10).onTrue(new InstantCommand(() -> shootake.shootFar()));
-    appendageJoystick.button(10).onFalse(new InstantCommand(() -> shootake.stopShooting()));
-
-    appendageJoystick.button(11).onTrue(new InstantCommand(() -> shootake.shootMedium()));
-    appendageJoystick.button(11).onFalse(new InstantCommand(() -> shootake.stopShooting()));
-
-    appendageJoystick.button(12).onTrue(new InstantCommand(() -> shootake.shootClose()));
-    appendageJoystick.button(12).onFalse(new InstantCommand(() -> shootake.stopShooting()));
-
-    appendageJoystick
-        .button(9)
-        .whileFalse(new InstantCommand(() -> shootake.overrideBeamBreak("auto")));
+
     appendageJoystick
-        .button(9)
-        .whileTrue(new InstantCommand(() -> shootake.overrideBeamBreak("manual")));
+        .button(1)
+        .onTrue(
+            new InstantCommand(() -> arm.ampShootingPos())
+                .andThen(new WaitCommand(0.75))
+                .andThen(new InstantCommand(() -> elevator.ampElevator())));
 
     appendageJoystick
         .button(2)
+        .onTrue(
+            new InstantCommand(() -> arm.closeShootingPos())
+                .andThen(new InstantCommand(() -> elevator.outElevator())));
+
+    appendageJoystick
+        .button(3)
         .onTrue(
             new InstantCommand(() -> shootake.shootFar())
-            /*.andThen(new InstantCommand(() -> arm.closeShootingPos()))
-            .andThen(new InstantCommand(() -> elevator.outElevator()))*/
             );
-
+            
     appendageJoystick
-        .button(2)
+        .button(3)
         .onFalse(
             new InstantCommand(() -> shootake.roweIntake())
                 .andThen(new WaitCommand(.75))
@@ -88,37 +77,55 @@ public void configureBindings() {
                 .andThen(new InstantCommand(() -> arm.stowArm()))
                 .andThen(new InstantCommand(() -> elevator.stowElevator())));
 
-    /*appendageJoystick.button(2).onFalse(new InstantCommand(() -> shootake.stopIntakeOutake())
-    .andThen(new InstantCommand(() -> shootake.stopShooting())));*/
-
-    // Elevator and Arm controls
-    appendageJoystick
-        .button(1)
-        .onTrue(
-            new InstantCommand(() -> arm.closeShootingPos())
-                .andThen(new InstantCommand(() -> elevator.outElevator())));
-    // appendageJoystick.button(2).onTrue(new InstantCommand(() -> arm.farShootingPos()));
-
     appendageJoystick
         .button(4)
         .onTrue(
-            new InstantCommand(() -> arm.ampShootingPos())
-                .andThen(new WaitCommand(0.75))
-                .andThen(new InstantCommand(() -> elevator.outElevator())));
+            new InstantCommand(() -> arm.stowArm())
+                .andThen(new WaitCommand(.5))
+                .andThen(new InstantCommand(() -> elevator.stowElevator())));
 
     appendageJoystick
         .button(5)
         .onTrue(
-            new InstantCommand(() -> arm.stowArm())
-                .andThen(new WaitCommand(.5))
-                .andThen(new InstantCommand(() -> elevator.stowElevator())));
+            new InstantCommand(() -> elevator.outElevator())
+            .andThen(new WaitCommand(.5))
+            .andThen(new InstantCommand(() -> arm.intakePos())));
 
     appendageJoystick
         .button(6)
+        .onTrue(new InstantCommand(() -> shootake.i_wanna_intake()));
+
+    appendageJoystick
+        .button(6)
+        .onFalse(new InstantCommand(() -> shootake.i_dont_wanna_intake()));
+
+    appendageJoystick
+        .button(7)
+        .onTrue(new InstantCommand(() -> shootake.i_wanna_outtake()));
+    appendageJoystick
+        .button(7)
+        .onFalse(new InstantCommand(() -> shootake.i_dont_wanna_outtake()));
+
+    appendageJoystick
+        .button(9)
+        .whileFalse(new InstantCommand(() -> shootake.overrideBeamBreak("auto")));
+    appendageJoystick
+        .button(9)
+        .whileTrue(new InstantCommand(() -> shootake.overrideBeamBreak("manual")));
+
+    appendageJoystick
+        .button(10)
         .onTrue(
-            new InstantCommand(() -> elevator.outElevator())
-                .andThen(new WaitCommand(.5))
-                .andThen(new InstantCommand(() -> arm.intakePos())));
+            new InstantCommand(() -> arm.farShootingPos())
+            .andThen(new InstantCommand(() -> elevator.outElevator())));
+
+    appendageJoystick
+        .button(11)
+        .onTrue(new InstantCommand(() -> elevator.outElevator()));
+
+    appendageJoystick
+        .button(12)
+        .onTrue(new InstantCommand(() -> elevator.stowElevator()));
   }
 
   public void configureAxisActions() {