auto tings

Nins97 · Nins97 · commit eb6b40bae6d3 · 2024-03-04T17:45:45.000-07:00
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);

Original file line number	Diff line number	Diff line change
`@@ -23,6 +23,12 @@`
`23`	`23`	`"name": "ShootNote"`
`24`	`24`	`}`
`25`	`25`	`},`
	`26`	`+ {`
	`27`	`+ "type": "wait",`
	`28`	`+ "data": {`
	`29`	`+ "waitTime": 0.5`
	`30`	`+ }`
	`31`	`+ },`
`26`	`32`	`{`
`27`	`33`	`"type": "parallel",`
`28`	`34`	`"data": {`
`@@ -40,7 +46,7 @@`
`40`	`46`	`{`
`41`	`47`	`"type": "wait",`
`42`	`48`	`"data": {`
`43`		`- "waitTime": 0.1`
	`49`	`+ "waitTime": 0.5`
`44`	`50`	`}`
`45`	`51`	`},`
`46`	`52`	`{`
`@@ -72,4 +78,4 @@`
`72`	`78`	`},`
`73`	`79`	`"folder": "Center Speaker",`
`74`	`80`	`"choreoAuto": false`
`75`		`-}`
	`81`	`+}`
Original file line number	Diff line number	Diff line change
`@@ -21,6 +21,6 @@ public static Command intakeUntilDetected(`
`21`	`21`	`.unless(transport::atSensor)`
`22`	`22`	`.andThen(`
`23`	`23`	`Commands.parallel(`
`24`		`- intake.setIntakeVoltageCommand(0.0), transport.setVoltageCommand(0.0)));`
	`24`	`+ intake.stopCommand(), transport.stopMovementCommand()));`
`25`	`25`	`}`
`26`	`26`	`}`
Original file line number	Diff line number	Diff line change
`@@ -261,8 +261,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {`
`261`	`261`
`262`	`262`	`@Override`
`263`	`263`	`public void periodic() {`
`264`		`- // TODO: make this or elegant`
`265`		`- if (atBottomLimit()) {`
	`264`	`+ if (atBottomLimit() && !isHomed) {`
`266`	`265`	`isHomed = true;`
`267`	`266`	`setEncoderPosition(0.0);`
`268`	`267`	`}`
Original file line number	Diff line number	Diff line change
`@@ -91,4 +91,8 @@ public Command checkIntakeCommand() {`
`91`	`91`	`public Command setIntakeVoltageCommand(double voltage) {`
`92`	`92`	`return this.run(() -> this.setIntakeVoltage(voltage));`
`93`	`93`	`}`
	`94`	`+`
	`95`	`+ public Command stopCommand() {`
	`96`	`+ return this.runOnce(() -> setIntakeVoltage(0.0));`
	`97`	`+ }`
`94`	`98`	`}`
Original file line number	Diff line number	Diff line change
`@@ -29,7 +29,8 @@ public class SlapdownFeederSubsystem extends SubsystemBase {`
`29`	`29`	`public SlapdownFeederSubsystem() {`
`30`	`30`	`configMotors();`
`31`	`31`
`32`		`- setDefaultCommand(setVoltageCommand(0.0).ignoringDisable(true).withName("SlapdownFeederDefault"));`
	`32`	`+ setDefaultCommand(`
	`33`	`+ setVoltageCommand(0.0).ignoringDisable(true).withName("SlapdownFeederDefault"));`
`33`	`34`	`}`
`34`	`35`
`35`	`36`	`private void configMotors() {`