Adds trap feature (#165)

* Adds pathfinding to closest climb spot * Starts endgame sequence command * Adds endgame sequence without drive * Adds pose target alignment to drive subsystem * Increases kP for auto rotation * Fixes format and build issues * Changes endgame poses to match * Inverts arm * Formatting fixes
team401 · Mar 19, 2024 · 8741ed9 · 8741ed9
1 parent e8543f3
commit 8741ed9
Show file tree

Hide file tree

Showing 15 changed files with 589 additions and 50 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -2,7 +2,9 @@
   "robotWidth": 0.8382,
   "robotLength": 0.9525,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Endgame"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 4.5,
   "defaultMaxAccel": 2.75,

diff --git a/simgui-ds.json b/simgui-ds.json
@@ -99,7 +99,7 @@
       "useGamepad": true
     },
     {
-      "guid": "03000000550900001472000000000000",
+      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     }
   ]

diff --git a/src/main/deploy/pathplanner/paths/FarEndgame.path b/src/main/deploy/pathplanner/paths/FarEndgame.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.804435512269952,
+        "y": 4.111991050457951
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.231331903884351,
+        "y": 4.111991050457951
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.377177122869452,
+        "y": 4.111991050457951
+      },
+      "prevControl": {
+        "x": 5.1275205233773615,
+        "y": 4.111991050457951
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 2.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Endgame",
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/LeftEndgame.path b/src/main/deploy/pathplanner/paths/LeftEndgame.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.863178241562208,
+        "y": 4.111991050457951
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.769143993136157,
+        "y": 4.274863146383425
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.613521642070118,
+        "y": 4.479133108534554
+      },
+      "prevControl": {
+        "x": 4.708978577170035,
+        "y": 4.313796847006692
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 2.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Endgame",
+  "previewStartingState": {
+    "rotation": -55.30484646876609,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/RightEndgame.path b/src/main/deploy/pathplanner/paths/RightEndgame.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.980663700146722,
+        "y": 4.156048097427144
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.801402102115242,
+        "y": 3.845557901790632
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.657578689039311,
+        "y": 3.6714205807660276
+      },
+      "prevControl": {
+        "x": 4.873538126480276,
+        "y": 4.045473298787772
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 2.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 60.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Endgame",
+  "previewStartingState": {
+    "rotation": 59.036243467926546,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -86,12 +86,23 @@ public static final class DriveConstants {
         public static final double anticipationTime = 0.01;
         public static final double minimumAnticipationVelocity = 0.0;
 
+        public static final double pathfindTransformToleranceMeters = 0.1;
+        public static final double pathfindRotationToleranceRadians = 0.1;
+
         public static final double alignToleranceRadians = 0.1;
 
         public static final double alignmentkPMax = 7.0;
         public static final double alignmentkPMin = 5.0;
         public static final double alignmentkI = 4.5;
         public static final double alignmentkD = 0.0;
+
+        public static final double vXkP = 5.0;
+        public static final double vXkI = 2.5;
+        public static final double vXkD = 0.0;
+
+        public static final double vYkP = 5.0;
+        public static final double vYkI = 2.5;
+        public static final double vYkD = 0.0;
     }
 
     public static final class FieldConstants {
@@ -494,7 +505,7 @@ public static final class ScoringConstants {
         public static final int hoodId = 17;
 
         public static final int aimEncoderPort = 0;
-        public static final double aimerEncoderOffset = 1.78;
+        public static final double aimerEncoderOffset = 1.75;
 
         public static final double aimPositionTolerance = 0.015;
 

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,9 +4,11 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.Utils;
 import com.pathplanner.lib.commands.FollowPathCommand;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -22,6 +24,7 @@
  * project.
  */
 public class Robot extends LoggedRobot {
+    private double lastTime = Utils.getCurrentTimeSeconds();
 
     private int cycle = 0;
 
@@ -72,10 +75,17 @@ public void robotInit() {
 
     @Override
     public void robotPeriodic() {
+        double startTime = Utils.getCurrentTimeSeconds() * 1000;
         CommandScheduler.getInstance().run();
 
         robotContainer.robotPeriodic();
 
+        double currentTime = Utils.getCurrentTimeSeconds() * 1000;
+        double diffTime = currentTime - startTime;
+        // lastTime = currentTime;
+
+        SmartDashboard.putNumber("Loop Time", diffTime);
+
         if (cycle % 20 == 0) {
             Logger.recordOutput("JVM/total memory", Runtime.getRuntime().totalMemory());
             Logger.recordOutput("JVM/max memory", Runtime.getRuntime().maxMemory());

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+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.button.CommandJoystick;
@@ -20,6 +21,7 @@
 import frc.robot.Constants.TunerConstants;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.commands.DriveWithJoysticks;
+import frc.robot.commands.EndgameSequence;
 import frc.robot.commands.ShootWithGamepad;
 import frc.robot.commands.WheelRadiusCharacterization;
 import frc.robot.subsystems.LED;
@@ -83,6 +85,8 @@ public class RobotContainer {
 
     LED leds;
 
+    Command endgameCommand = Commands.print("Endgame command not constructed");
+
     public RobotContainer() {
         configureSubsystems();
         configureModes();
@@ -206,6 +210,16 @@ public void configureSubsystems() {
             }
             if (FeatureFlags.runEndgame) {
                 scoringSubsystem.setElevatorPositionSupplier(endgameSubsystem::getPosition);
+
+                if (FeatureFlags.runDrive) {
+                    endgameCommand =
+                            new EndgameSequence(
+                                    scoringSubsystem,
+                                    endgameSubsystem,
+                                    drivetrain,
+                                    driveTelemetry,
+                                    () -> controller.getHID().getLeftBumper());
+                }
             }
             if (FeatureFlags.runIntake) {
                 intakeSubsystem.setScoringSupplier(scoringSubsystem::canIntake);
@@ -276,18 +290,35 @@ private void configureBindings() {
             rightJoystick.povRight()
                 .onTrue(new InstantCommand(
                     () -> drivetrain.setAlignTarget(AlignTarget.RIGHT)));
+
+            controller.x()
+                .onTrue(new InstantCommand(() -> drivetrain.driveToEndgame()))
+                .onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
         }
 
         if (FeatureFlags.runEndgame) {
-            endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.OVERRIDE);
+            // endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.OVERRIDE);
+            endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.CANCEL);
+
+            // controller.leftBumper()
+            //     .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(4.0, 0)))
+            //     .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0.0, 0)));
+
+            // controller.leftTrigger()
+            //     .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(-4.0, 0)))
+            //     .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0.0, 0)));
+
+            // controller.leftBumper()
+            //     .onTrue(new InstantCommand(() -> endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.GO_UP)));
+
+            // controller.leftTrigger()
+            //     .onTrue(new InstantCommand(() -> endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.GO_DOWN)));
 
-            controller.leftBumper()
-                .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(4.0, 0)))
-                .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0.0, 0)));
+            // controller.x()
+            //     .onTrue(/*!endgameCommand.isScheduled() ?*/ endgameCommand);
 
             controller.leftTrigger()
-                .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(-4.0, 0)))
-                .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0.0, 0)));
+                .onTrue(new InstantCommand(() -> endgameCommand.cancel()));
         }
 
         if (FeatureFlags.runIntake) {
@@ -668,6 +699,9 @@ private void setUpDriveWithJoysticks() {
                             () -> leftJoystick.getY(),
                             () -> leftJoystick.getX(),
                             () -> rightJoystick.getX(),
+                            // () -> controller.getLeftY(),
+                            // () -> controller.getLeftX(),
+                            // () -> controller.getRightX(),
                             () -> !rightJoystick.getHID().getTrigger(),
                             () -> rightJoystick.top().getAsBoolean()));
         }
@@ -720,6 +754,7 @@ public void autoInit() {
         if (drivetrain.getAutoCommand() != null) {
             drivetrain.getAutoCommand().schedule();
             drivetrain.setAlignState(AlignState.ALIGNING);
+            drivetrain.setAlignTarget(AlignTarget.SPEAKER);
             if (FeatureFlags.runScoring) {
                 scoringSubsystem.setAction(ScoringSubsystem.ScoringAction.SHOOT);
             }