auto tings

RegisJesuitRobotics · Mar 5, 2024 · eb6b40b · eb6b40b
1 parent f8c2ac9
commit eb6b40b
Show file tree

Hide file tree

Showing 13 changed files with 54 additions and 31 deletions.
diff --git a/.run/Format.run.xml b/.run/Format.run.xml
@@ -2,7 +2,7 @@
   <configuration default="false" name="Format" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
-      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$/Competition2024" />
       <option name="externalSystemIdString" value="GRADLE" />
       <option name="scriptParameters" value="" />
       <option name="taskDescriptions">
@@ -18,7 +18,6 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/src/main/deploy/pathplanner/autos/CenterSpeaker2NoteCloseMid.auto b/src/main/deploy/pathplanner/autos/CenterSpeaker2NoteCloseMid.auto
@@ -23,6 +23,12 @@
             "name": "ShootNote"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.5
+          }
+        },
         {
           "type": "parallel",
           "data": {
@@ -40,7 +46,7 @@
                     {
                       "type": "wait",
                       "data": {
-                        "waitTime": 0.1
+                        "waitTime": 0.5
                       }
                     },
                     {
@@ -72,4 +78,4 @@
   },
   "folder": "Center Speaker",
   "choreoAuto": false
-}
+}
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
@@ -123,14 +123,18 @@ private Command intakeUntilNoteAndPrepareShot() {
       return Commands.print("Intaking");
     }
     return Commands.deadline(
-            IntakingCommands.intakeUntilDetected(
-                intakeSubsystem, slapdownSuperstructure, transportSubsystem),
-            ElevatorWristCommands.elevatorWristIntakePosition(elevatorSubsystem, wristSubsystem))
+            Commands.parallel(
+                IntakingCommands.intakeUntilDetected(
+                    intakeSubsystem, slapdownSuperstructure, transportSubsystem),
+                ElevatorWristCommands.elevatorWristIntakePosition(elevatorSubsystem, wristSubsystem)
+                    .until(
+                        ElevatorWristCommands.elevatorWristToleranceTrigger(
+                            elevatorSubsystem, wristSubsystem))))
         .andThen(
             Commands.parallel(
                 ElevatorWristCommands.elevatorWristCloseSpeakerCommand(
                     elevatorSubsystem, wristSubsystem),
-                ScoringCommands.shootSetpointCloseSpeakerCommand(shooterSubsystem)));
+                ScoringCommands.shootSetpointCloseSpeakerCommand(shooterSubsystem)).until(() -> true)).andThen(Commands.print("I WANT TO SHOOT"));
   }
 
   public Command autoStart() {
@@ -146,14 +150,14 @@ private Command shootNote() {
       return Commands.print("Shooting Note!").andThen(Commands.waitSeconds(0.5));
     }
     return Commands.deadline(
-            Commands.sequence(
-                    shooterAndElevatorWristInToleranceCommand(),
-                    ScoringCommands.transportCloseSpeakerCommand(transportSubsystem)
-                        .until(() -> !transportSubsystem.atSensor()))
-                .andThen(Commands.waitSeconds(0.1), transportSubsystem.stopMovementCommand()),
-            ScoringCommands.shootSetpointCloseSpeakerCommand(shooterSubsystem),
-            ElevatorWristCommands.elevatorWristCloseSpeakerCommand(
-                elevatorSubsystem, wristSubsystem));
+        Commands.sequence(
+                Commands.print("I LOVE SHOOTING"),
+                shooterAndElevatorWristInToleranceCommand(),
+                ScoringCommands.transportCloseSpeakerCommand(transportSubsystem)
+                    .until(() -> !transportSubsystem.atSensor()))
+            .andThen(Commands.waitSeconds(0.1), transportSubsystem.stopMovementCommand()),
+        ScoringCommands.shootSetpointCloseSpeakerCommand(shooterSubsystem),
+        ElevatorWristCommands.elevatorWristCloseSpeakerCommand(elevatorSubsystem, wristSubsystem));
   }
 
   private Command shooterAndElevatorWristInToleranceCommand() {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -93,6 +93,10 @@ private void configureLEDs() {
             new LEDState(
                 () -> Alert.getDefaultGroup().hasAnyErrors(),
                 new AlternatePattern(2.0, Color.kRed, Color.kBlack)),
+            new LEDState(
+                    () -> DriverStation.isTeleopEnabled() && elevatorSubsystem.atBottomLimit() && wristSubsystem.atBottom(),
+                    new SolidPattern(Color.kGreen)
+            ),
             // Default disabled pattern
             new LEDState(
                 DriverStation::isDisabled,

diff --git a/src/main/java/frc/robot/commands/ElevatorWristCommands.java b/src/main/java/frc/robot/commands/ElevatorWristCommands.java
@@ -23,7 +23,12 @@ public static Command elevatorWristIntakePosition(
 
   public static Command elevatorWristInToleranceCommand(
       ElevatorSubsystem elevatorSubsystem, WristSubsystem wristSubsystem) {
-    return Commands.waitUntil(new Trigger(elevatorSubsystem::atGoal).and(wristSubsystem::atGoal));
+    return Commands.waitUntil(elevatorWristToleranceTrigger(elevatorSubsystem, wristSubsystem));
+  }
+
+  public static Trigger elevatorWristToleranceTrigger(
+      ElevatorSubsystem elevatorSubsystem, WristSubsystem wristSubsystem) {
+    return new Trigger(elevatorSubsystem::atGoal).and(wristSubsystem::atGoal);
   }
 
   public static Command elevatorWristCloseSpeakerCommand(

diff --git a/src/main/java/frc/robot/commands/IntakingCommands.java b/src/main/java/frc/robot/commands/IntakingCommands.java
@@ -21,6 +21,6 @@ public static Command intakeUntilDetected(
         .unless(transport::atSensor)
         .andThen(
             Commands.parallel(
-                intake.setIntakeVoltageCommand(0.0), transport.setVoltageCommand(0.0)));
+                intake.stopCommand(), transport.stopMovementCommand()));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -261,8 +261,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
 
   @Override
   public void periodic() {
-    // TODO: make this or elegant
-    if (atBottomLimit()) {
+    if (atBottomLimit() && !isHomed) {
       isHomed = true;
       setEncoderPosition(0.0);
     }

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
@@ -91,4 +91,8 @@ public Command checkIntakeCommand() {
   public Command setIntakeVoltageCommand(double voltage) {
     return this.run(() -> this.setIntakeVoltage(voltage));
   }
+
+  public Command stopCommand() {
+    return this.runOnce(() -> setIntakeVoltage(0.0));
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
@@ -25,31 +25,27 @@ public LEDSubsystem() {
     int i = 0;
     splitBuffers[0] = ledBuffer.split(0, FRONT_LEFT_SIZE);
     if (FRONT_LEFT_SIZE < MAX_SIZE) {
-      splitBuffers[0] =
-          splitBuffers[0].concatenate(new FakeLEDBuffer(MAX_SIZE - FRONT_LEFT_SIZE));
+      splitBuffers[0] = splitBuffers[0].concatenate(new FakeLEDBuffer(MAX_SIZE - FRONT_LEFT_SIZE));
     }
     i += FRONT_LEFT_SIZE;
 
     splitBuffers[1] = ledBuffer.split(i, i + BACK_LEFT_SIZE);
     splitBuffers[1] = splitBuffers[1].reversed();
     if (BACK_LEFT_SIZE < MAX_SIZE) {
-      splitBuffers[1] =
-          splitBuffers[1].concatenate(new FakeLEDBuffer(MAX_SIZE - BACK_LEFT_SIZE));
+      splitBuffers[1] = splitBuffers[1].concatenate(new FakeLEDBuffer(MAX_SIZE - BACK_LEFT_SIZE));
     }
     i += BACK_LEFT_SIZE;
 
     splitBuffers[2] = ledBuffer.split(i, i + BACK_RIGHT_SIZE);
     if (BACK_RIGHT_SIZE < MAX_SIZE) {
-      splitBuffers[2] =
-          splitBuffers[2].concatenate(new FakeLEDBuffer(MAX_SIZE - BACK_RIGHT_SIZE));
+      splitBuffers[2] = splitBuffers[2].concatenate(new FakeLEDBuffer(MAX_SIZE - BACK_RIGHT_SIZE));
     }
     i += BACK_RIGHT_SIZE;
 
     splitBuffers[3] = ledBuffer.split(i, i + FRONT_RIGHT_SIZE);
     splitBuffers[3] = splitBuffers[3].reversed();
     if (FRONT_RIGHT_SIZE < MAX_SIZE) {
-      splitBuffers[3] =
-          splitBuffers[3].concatenate(new FakeLEDBuffer(MAX_SIZE - FRONT_RIGHT_SIZE));
+      splitBuffers[3] = splitBuffers[3].concatenate(new FakeLEDBuffer(MAX_SIZE - FRONT_RIGHT_SIZE));
     }
     i += FRONT_RIGHT_SIZE;
 

diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownFeederSubsystem.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownFeederSubsystem.java
@@ -29,7 +29,8 @@ public class SlapdownFeederSubsystem extends SubsystemBase {
   public SlapdownFeederSubsystem() {
     configMotors();
 
-    setDefaultCommand(setVoltageCommand(0.0).ignoringDisable(true).withName("SlapdownFeederDefault"));
+    setDefaultCommand(
+        setVoltageCommand(0.0).ignoringDisable(true).withName("SlapdownFeederDefault"));
   }
 
   private void configMotors() {

diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java
@@ -70,7 +70,8 @@ public SlapdownRotationSubsystem() {
     rotationEncoder = rotationMotor.getEncoder();
     configMotors();
 
-    setDefaultCommand(setVoltageCommand(0.0).ignoringDisable(true).withName("SlapdownRotationDefault"));
+    setDefaultCommand(
+        setVoltageCommand(0.0).ignoringDisable(true).withName("SlapdownRotationDefault"));
   }
 
   private void configMotors() {

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -213,7 +213,7 @@ private void configSteerMotor(SwerveModuleConfiguration config) {
         config.steerMotorInverted()
             ? InvertedValue.Clockwise_Positive
             : InvertedValue.CounterClockwise_Positive;
-    motorConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    motorConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     motorConfiguration.Feedback.FeedbackRemoteSensorID = config.steerEncoderID();
     motorConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
     motorConfiguration.Feedback.RotorToSensorRatio = config.sharedConfiguration().steerGearRatio();

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java
@@ -133,6 +133,10 @@ public boolean atGoal() {
     return controller.atGoal();
   }
 
+  public boolean atBottom() {
+    return controller.getGoal().position == WRIST_MIN.getRadians() && atGoal();
+  }
+
   public void setVoltage(double voltage) {
     wristMotor.setVoltage(voltage);
     wristVoltageReq.append(voltage);