WARP 2024 Code (#104)

Co-authored-by: = <paul.hodges@student.education.wa.edu.au>
CurtinFRC · Aug 29, 2024 · 54c806f · 54c806f
1 parent f97e9ef
commit 54c806f
Show file tree

Hide file tree

Showing 30 changed files with 32,912 additions and 6,994 deletions.
diff --git a/src/main/deploy/choreo/Amp1425.1.traj b/src/main/deploy/choreo/Amp1425.1.traj
diff --git a/src/main/deploy/choreo/Amp1425.2.traj b/src/main/deploy/choreo/Amp1425.2.traj
diff --git a/src/main/deploy/choreo/Amp1425.3.traj b/src/main/deploy/choreo/Amp1425.3.traj
diff --git a/src/main/deploy/choreo/Amp1425.4.traj b/src/main/deploy/choreo/Amp1425.4.traj
diff --git a/src/main/deploy/choreo/Amp1425.traj b/src/main/deploy/choreo/Amp1425.traj
diff --git a/src/main/deploy/choreo/Center1425.1.traj b/src/main/deploy/choreo/Center1425.1.traj
diff --git a/src/main/deploy/choreo/Center1425.2.traj b/src/main/deploy/choreo/Center1425.2.traj
diff --git a/src/main/deploy/choreo/Center1425.3.traj b/src/main/deploy/choreo/Center1425.3.traj
diff --git a/src/main/deploy/choreo/Center1425.4.traj b/src/main/deploy/choreo/Center1425.4.traj
diff --git a/src/main/deploy/choreo/Center1425.traj b/src/main/deploy/choreo/Center1425.traj
diff --git a/src/main/deploy/choreo/Centre1423.1.traj b/src/main/deploy/choreo/Centre1423.1.traj
diff --git a/src/main/deploy/choreo/Centre1423.2.traj b/src/main/deploy/choreo/Centre1423.2.traj
diff --git a/src/main/deploy/choreo/Centre1423.3.traj b/src/main/deploy/choreo/Centre1423.3.traj
diff --git a/src/main/deploy/choreo/Centre1423.4.traj b/src/main/deploy/choreo/Centre1423.4.traj
diff --git a/src/main/deploy/choreo/Centre1423.traj b/src/main/deploy/choreo/Centre1423.traj
diff --git a/src/main/deploy/choreo/Centre1423Blue.1.traj b/src/main/deploy/choreo/Centre1423Blue.1.traj
diff --git a/src/main/deploy/choreo/Centre1423Blue.2.traj b/src/main/deploy/choreo/Centre1423Blue.2.traj
diff --git a/src/main/deploy/choreo/Centre1423Blue.3.traj b/src/main/deploy/choreo/Centre1423Blue.3.traj
diff --git a/src/main/deploy/choreo/Centre1423Blue.4.traj b/src/main/deploy/choreo/Centre1423Blue.4.traj
diff --git a/src/main/deploy/choreo/Centre1423Blue.traj b/src/main/deploy/choreo/Centre1423Blue.traj
diff --git a/src/main/deploy/pathplanner/autos/Amp1425.auto b/src/main/deploy/pathplanner/autos/Amp1425.auto
@@ -0,0 +1,93 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.8108881115913391,
+      "y": 6.541628837585449
+    },
+    "rotation": 57.99459009616007
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Spinup"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Arm"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Amp1425.1"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Passthrough"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Amp1425.2"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Amp1425.3"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Amp1425.4"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": true
+}
diff --git a/src/main/deploy/pathplanner/autos/Center1425.auto b/src/main/deploy/pathplanner/autos/Center1425.auto
@@ -0,0 +1,93 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.242100715637207,
+      "y": 5.592960834503174
+    },
+    "rotation": -1.892991537218447e-26
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Spinup"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Arm"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Center1425.1"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Center1425.2"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Center1425.3"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Center1425.4"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": true
+}
diff --git a/src/main/deploy/pathplanner/autos/Centre1423Blue.auto b/src/main/deploy/pathplanner/autos/Centre1423Blue.auto
@@ -0,0 +1,99 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.2346843481063845,
+      "y": 5.52810525894165
+    },
+    "rotation": 2.6324873302388957e-27
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Spinup"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Arm"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Passthrough"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Centre1423Blue.1"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Passthrough"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Centre1423Blue.2"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Passthrough"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Centre1423Blue.3"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Passthrough"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "Centre1423Blue.4"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Passthrough"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": true
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -37,7 +37,7 @@ public final class Constants {
   public static final double intakeV = 0.0020349;
   public static final double intakeA = 0.00018939;
 
-  public static final double armP = 10;
+  public static final double armP = 3.15;
   public static final double armI = 0;
   public static final double armD = 0;
   public static final double armS = 0;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -21,6 +21,7 @@
 import frc.robot.subsystems.Arm.Setpoint;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.CommandSwerveDrivetrain.RotationSetpoint;
 import frc.robot.subsystems.Index;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.LED;
@@ -114,13 +115,22 @@ public Robot() {
                                 .withTimeout(2)
                                 .andThen(Commands.parallel(m_shooter.stop(), m_index.stop())),
                             m_arm.maintain()))));
-    NamedCommands.registerCommand("Arm", m_arm.goToSetpoint(Setpoint.kSpeaker));
+    NamedCommands.registerCommand(
+        "Arm",
+        Commands.deferredProxy(() -> m_arm.goToSetpointAuto(Setpoint.kSpeaker).withTimeout(1.2)));
     NamedCommands.registerCommand(
         "Intake", Commands.deferredProxy(() -> m_superstructure.intake()));
     NamedCommands.registerCommand("OTFArm", m_arm.moveToPosition(0.7528).andThen(m_arm.maintain()));
-    NamedCommands.registerCommand("Spinup", m_shooter.spinup(500).andThen(m_shooter.maintain()));
-    NamedCommands.registerCommand("Passthrough", Commands.deferredProxy(() -> m_index.shoot()));
-
+    NamedCommands.registerCommand(
+        "Spinup",
+        Commands.deferredProxy(() -> m_shooter.spinup(500).andThen(m_shooter.maintain())));
+    NamedCommands.registerCommand("Passthrough", Commands.deferredProxy(() -> m_index.shootAuto()));
+
+    // m_autoChooser.setDefaultOption("Center1425", m_drivetrain.getAutoPath("Center1425"));
+    // m_autoChooser.setDefaultOption("Centre1253", m_drivetrain.getAutoPath("Centre1423"));
+    // m_autoChooser.addOption("Center213", m_drivetrain.getAutoPath("Center213"));
+    m_autoChooser.addOption("Centre1423", m_drivetrain.getAutoPath("Centre1423"));
+    m_autoChooser.addOption("Centre1423Blue", m_drivetrain.getAutoPath("Centre1423Blue"));
     m_autoChooser.setDefaultOption("Centre1423", m_drivetrain.getAutoPath("Centre1423"));
     SmartDashboard.putData(m_autoChooser);
     SmartDashboard.putNumber("Arm", armangle);
@@ -157,11 +167,13 @@ public Robot() {
 
     m_shooter.m_atSetpoint.whileTrue(m_led.canShoot());
     m_index.m_hasNote.whileTrue(m_led.hasNote());
+    m_index.m_intaking.whileTrue(m_led.intaking());
+
     m_index.m_intaking.onTrue(
         Commands.parallel(
             m_intake.intake(5).until(m_index.m_hasNote).andThen(m_intake.stop()),
             m_index
-                .intake(-4.5)
+                .intake(-3.5)
                 .until(m_index.m_hasNote)
                 .andThen(new WaitCommand(0.5))
                 .andThen(m_index.intake(2))
@@ -170,20 +182,30 @@ public Robot() {
 
     m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
     m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
+    m_driver
+        .leftBumper()
+        .onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kAmp, m_drive, m_scheduler));
+    m_driver
+        .rightBumper()
+        .onTrue(m_drivetrain.goToSetpoint(RotationSetpoint.kSpeaker, m_drive, m_scheduler));
 
     m_codriver.leftBumper().whileTrue(m_index.shoot());
     m_codriver.rightBumper().whileTrue(m_superstructure.outake());
     m_codriver
         .leftTrigger()
         .whileTrue(
             Commands.parallel(
-                m_arm.goToSetpoint(Setpoint.kSpeaker).andThen(m_arm.maintain()),
+                m_arm
+                    .goToSetpoint(Setpoint.kSpeaker, m_codriver.leftTrigger())
+                    .andThen(m_arm.maintain()),
                 m_shooter.spinup(1500).andThen(m_shooter.maintain())));
     m_codriver
         .rightTrigger()
         .whileTrue(
             Commands.parallel(
-                m_arm.goToSetpoint(Setpoint.kAmp).andThen(m_arm.maintain()),
+                m_arm
+                    .goToSetpoint(Setpoint.kAmp, m_codriver.rightTrigger())
+                    .andThen(m_arm.maintain()),
                 m_shooter.applyVolts(8)));
 
     m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop());
@@ -196,14 +218,6 @@ public Robot() {
               }
             });
     m_codriver.y().onTrue(m_superstructure.stop());
-    // m_codriver
-    // .b()
-    // .whileTrue(
-    // m_arm
-    // .goToSetpoint(Setpoint.kShuttling)
-    // .andThen(
-    // Commands.parallel(
-    // m_arm.maintain(), m_shooter.spinup(1000).andThen(m_shooter.maintain()))));
     m_codriver.b().whileTrue(m_shooter.spinup(1500).andThen(m_shooter.maintain()));
   }
 }