Added ShooterDelay Command (Untested)

team4909 · Feb 10, 2024 · 37d1f41 · 37d1f41
1 parent c74d794
commit 37d1f41
Show file tree

Hide file tree

Showing 5 changed files with 55 additions and 7 deletions.
diff --git a/src/main/deploy/pathplanner/autos/4 Note Auto.auto b/src/main/deploy/pathplanner/autos/4 Note Auto.auto
@@ -11,6 +11,24 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "sub shot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ShooterDelay"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop"
+          }
+        },
         {
           "type": "path",
           "data": {

diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024-Crescendo";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 68;
-  public static final String GIT_SHA = "9243f8415dbb3800f08c1a116584eceb4ee2d671";
-  public static final String GIT_DATE = "2024-02-09 19:37:25 EST";
+  public static final int GIT_REVISION = 70;
+  public static final String GIT_SHA = "c74d794181fd63762f96924992e695590d4d27d8";
+  public static final String GIT_DATE = "2024-02-09 21:11:48 EST";
   public static final String GIT_BRANCH = "Black_Rev2_Arm";
-  public static final String BUILD_DATE = "2024-02-09 21:06:34 EST";
-  public static final long BUILD_UNIX_TIME = 1707530794973L;
+  public static final String BUILD_DATE = "2024-02-10 10:16:00 EST";
+  public static final long BUILD_UNIX_TIME = 1707578160035L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -85,14 +85,22 @@ public Robot() {
             () -> -m_driverController.getLeftX(),
             // This needs to be getRawAxis(2) when using sim on a Mac
             () -> -m_driverController.getRightX()));
-    NamedCommands.registerCommand("stop", m_shooter.Stop());
+    NamedCommands.registerCommand("stop", m_shooter.Stop().withTimeout(0.5));
     NamedCommands.registerCommand("sensorIntake", SensorIntake());
     NamedCommands.registerCommand("intake", m_intake.intake(speakerShot));
-    NamedCommands.registerCommand("shoot", m_shooter.Shoot());
+    NamedCommands.registerCommand("shoot", m_shooter.Shoot().withTimeout(2));
+    NamedCommands.registerCommand("sub shot", m_arm.goToDeg(20, 25));
+    NamedCommands.registerCommand("arm down", m_arm.goDown());
+    NamedCommands.registerCommand("feed", m_shooter.Feeder());
+    NamedCommands.registerCommand("ShooterDelay", m_shooter.ShooterDelay());
 
     m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser", AutoBuilder.buildAutoChooser());
+    m_shooter.setDefaultCommand(m_shooter.Shoot());
     m_shooter.setDefaultCommand(m_shooter.Stop());
     m_intake.setDefaultCommand(m_intake.Stop());
+    // m_arm.setDefaultCommand(m_arm.goToDeg(20, 25));
+    // m_arm.setDefaultCommand(m_arm.goDown());
+
     m_autoChooser.addOption(
         "Drive SysId (Quasistatic Forward)",
         m_drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
@@ -141,6 +149,8 @@ public Robot() {
         .onTrue(m_arm.goToDegSeq(100, 0, -70))
         .onFalse(m_arm.goDown());
 
+    m_operatorController.a().onTrue(m_arm.goToDegSeq(110, 0, 0)).onFalse(m_arm.goDown());
+
     m_operatorController.rightTrigger().onTrue(m_arm.goDown()).onFalse(m_arm.goDown());
 
     m_operatorController

diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -13,6 +13,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -127,6 +128,13 @@ public Command goToDegSeq(double j1ParDeg, double j2ParDeg, double j2SeqDeg) {
 
   public Command goDown() {
     return new SequentialCommandGroup(
+        Commands.runOnce(
+            () -> {
+              // var highPConfig = new Slot0Configs();
+              // highPConfig.kP = 1;
+
+            },
+            this),
         goToDeg(m_lJoint2, m_j2Request, m_j2Ratio, 0),
         new WaitUntilCommand(
             () -> {

diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java
@@ -8,8 +8,12 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.BionicWaitCommand;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -115,4 +119,12 @@ public Command FeederOut() {
         .withTimeout(1)
         .finallyDo(() -> Stop());
   }
+
+  public Command ShooterDelay() {
+    return Commands.sequence(
+        new ParallelCommandGroup(
+            Shoot(),
+            new BionicWaitCommand(() -> SmartDashboard.getNumber("ShooterDelay", defaultDelay))),
+        new RepeatCommand(Intake()));
+  }
 }