Add basic autonomous points

frc2204 · Mar 6, 2024 · 4af6a0d · 4af6a0d
1 parent e235e03
commit 4af6a0d
Show file tree

Hide file tree

Showing 12 changed files with 125 additions and 125 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -91,8 +91,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "030000004c050000e60c000000000000"
     }
   ]
 }
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
diff --git a/src/main/deploy/pathplanner/paths/Amp.path b/src/main/deploy/pathplanner/paths/Amp.path
diff --git a/src/main/deploy/pathplanner/paths/Playground.path b/src/main/deploy/pathplanner/paths/Playground.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 15.916953193479339,
+        "y": 1.2159744963458083
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 14.916953193479339,
+        "y": 1.2159744963458083
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,13 +39,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 120.0,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": -59.9781887324857,
+    "rotation": -90.0,
     "velocity": 0
   },
   "useDefaultConstraints": true

diff --git a/src/main/java/org/rambots/AutoConstants.kt b/src/main/java/org/rambots/AutoConstants.kt
diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "frc-2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 8;
-  public static final String GIT_SHA = "cd46b92a3c9db0369f3c1f26dd9d6a937b0f47bd";
-  public static final String GIT_DATE = "2024-03-05 09:15:24 PST";
+  public static final int GIT_REVISION = 9;
+  public static final String GIT_SHA = "e235e0327a4c8b1b2be006c6080e1000250747ba";
+  public static final String GIT_DATE = "2024-03-05 18:20:38 PST";
   public static final String GIT_BRANCH = "james/swerve";
-  public static final String BUILD_DATE = "2024-03-05 16:08:22 PST";
-  public static final long BUILD_UNIX_TIME = 1709683702503L;
+  public static final String BUILD_DATE = "2024-03-06 15:22:24 PST";
+  public static final long BUILD_UNIX_TIME = 1709767344100L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt
@@ -14,14 +14,14 @@ package org.rambots
 
 import com.pathplanner.lib.auto.AutoBuilder
 import edu.wpi.first.math.geometry.*
+import edu.wpi.first.wpilibj.PS5Controller
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands
+import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser
-import org.rambots.commands.DriveCommands
-import org.rambots.commands.DriveToPoint
-import org.rambots.commands.PathFinderAndFollow
+import org.rambots.commands.*
 import org.rambots.subsystems.drive.*
 import org.rambots.subsystems.drive.DriveConstants.moduleConfigs
 import org.rambots.subsystems.vision.AprilTagVision
@@ -46,7 +46,7 @@ object RobotContainer {
     private val driveController = DriveController()
 
     // Controller
-    private val controller = CommandXboxController(0)
+    private val controller = CommandPS5Controller(0)
 
     // Dashboard inputs
     private val autoChooser: LoggedDashboardChooser<Command>
@@ -147,11 +147,9 @@ object RobotContainer {
         // flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
         // Configure the button bindings
-        aprilTagVision.setDataInterfaces(Consumer { visionData: List<TimestampedVisionUpdate?>? ->
-            drive.addVisionData(
-                visionData
-            )
-        })
+        aprilTagVision.setDataInterfaces { visionData: List<TimestampedVisionUpdate?>? ->
+            drive.addVisionData(visionData)
+        }
         driveController.setPoseSupplier { drive.pose }
         driveController.disableHeadingControl()
         configureButtonBindings()
@@ -162,41 +160,37 @@ object RobotContainer {
      * instantiating a [GenericHID] or one of its subclasses ([ ] or [XboxController]), and then passing it to a [ ].
      */
     private fun configureButtonBindings() {
-        drive.defaultCommand = DriveCommands.joystickDrive(
-            drive,
-            driveController,
-            { -controller.leftY },
-            { -controller.leftX },
-            { -controller.rightX })
-
-        controller.leftBumper().whileTrue(Commands.runOnce({ driveController.toggleDriveMode() }))
-
-        controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType))
-
-        controller
-            .b()
-            .whileTrue(
-                Commands.startEnd(
-                    { driveController.enableHeadingControl() }, { driveController.disableHeadingControl() })
-            )
-
-        controller
-            .y()
-            .whileTrue(
-                Commands.runOnce(
-                    {
-                        drive.setAutoStartPose(
-                            Pose2d(Translation2d(4.0, 5.0), Rotation2d.fromDegrees(0.0))
-                        )
-                    })
-            )
-        controller
-            .povDown()
-            .whileTrue(
-                DriveToPoint(
-                    drive, Pose2d(Translation2d(2.954, 3.621), Rotation2d.fromRadians(2.617))
-                )
-            )
+        drive.defaultCommand = DriveCommands.joystickDrive(drive, driveController, { -controller.leftY }, { -controller.leftX }, { -controller.rightX })
+
+        controller.L1().whileTrue(AmpScoring())
+        controller.R2().whileTrue(SourceIntake())
+
+//        controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType))
+
+//        controller
+//            .b()
+//            .whileTrue(
+//                Commands.startEnd(
+//                    { driveController.enableHeadingControl() }, { driveController.disableHeadingControl() })
+//            )
+//
+//        controller
+//            .y()
+//            .whileTrue(
+//                Commands.runOnce(
+//                    {
+//                        drive.setAutoStartPose(
+//                            Pose2d(Translation2d(4.0, 5.0), Rotation2d.fromDegrees(0.0))
+//                        )
+//                    })
+//            )
+//        controller
+//            .povDown()
+//            .whileTrue(
+//                DriveToPoint(
+//                    drive, Pose2d(Translation2d(2.954, 3.621), Rotation2d.fromRadians(2.617))
+//                )
+//            )
 //
 //        controller
 //            .povUp()

diff --git a/src/main/java/org/rambots/auto/AutoConstants.kt b/src/main/java/org/rambots/auto/AutoConstants.kt
@@ -0,0 +1,28 @@
+package org.rambots.auto
+
+import com.pathplanner.lib.path.PathConstraints
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.util.Units
+import org.rambots.util.AllianceFlipUtil
+
+object AutoConstants {
+
+    val pathConstraints = PathConstraints(4.33, 8.0, Units.degreesToRadians(540.0), Units.degreesToRadians(720.0))
+
+    val ampScorePose: Pose2d get() = AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(-90.0)))
+
+    val sourceDirectIntake get() = SourcePosePositions(
+        AllianceFlipUtil.apply(Pose2d(Translation2d(15.92, 1.22), Rotation2d.fromDegrees(120.0))),
+        AllianceFlipUtil.apply(Pose2d(Translation2d(15.42, 0.93), Rotation2d.fromDegrees(120.0))),
+        AllianceFlipUtil.apply(Pose2d(Translation2d(14.95, 0.64), Rotation2d.fromDegrees(120.0)))
+    )
+
+    val sourceGroundIntake get() = SourcePosePositions(
+        AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))),
+        AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))),
+        AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0)))
+    )
+
+}
diff --git a/src/main/java/org/rambots/auto/SourcePosePositions.kt b/src/main/java/org/rambots/auto/SourcePosePositions.kt
@@ -0,0 +1,9 @@
+package org.rambots.auto
+
+import edu.wpi.first.math.geometry.Pose2d
+
+data class SourcePosePositions(
+    val left: Pose2d,
+    val center: Pose2d,
+    val right: Pose2d
+)
diff --git a/src/main/java/org/rambots/commands/AmpScoring.kt b/src/main/java/org/rambots/commands/AmpScoring.kt
@@ -1,23 +1,28 @@
 package org.rambots.commands
 
+import com.pathplanner.lib.auto.AutoBuilder
 import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
+import org.rambots.auto.AutoConstants
 
 class AmpScoring: Command() {
 
+    private val pathRun get() = AutoBuilder.pathfindToPose(AutoConstants.ampScorePose, AutoConstants.pathConstraints)
+    private var scoreCommand = Commands.sequence(pathRun)
+
     override fun initialize() {
         super.initialize()
-    }
-
-    override fun execute() {
-        super.execute()
+        scoreCommand = Commands.sequence(pathRun)
+        scoreCommand.schedule()
     }
 
     override fun end(interrupted: Boolean) {
         super.end(interrupted)
+        scoreCommand.cancel()
     }
 
     override fun isFinished(): Boolean {
-        return super.isFinished()
+        return scoreCommand.isFinished
     }
 
 }
diff --git a/src/main/java/org/rambots/commands/SourceIntake.kt b/src/main/java/org/rambots/commands/SourceIntake.kt
@@ -0,0 +1,26 @@
+package org.rambots.commands
+
+import com.pathplanner.lib.auto.AutoBuilder
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
+import org.rambots.auto.AutoConstants
+
+class SourceIntake: Command() {
+    private val pathRun get() = AutoBuilder.pathfindToPose(AutoConstants.sourceDirectIntake.left, AutoConstants.pathConstraints)
+    private var scoreCommand = Commands.sequence(pathRun)
+
+    override fun initialize() {
+        super.initialize()
+        scoreCommand = Commands.sequence(pathRun)
+        scoreCommand.schedule()
+    }
+
+    override fun end(interrupted: Boolean) {
+        super.end(interrupted)
+        scoreCommand.cancel()
+    }
+
+    override fun isFinished(): Boolean {
+        return scoreCommand.isFinished
+    }
+}
diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java
@@ -340,8 +340,7 @@ public void setAutoStartPose(Pose2d pose) {
      * @param visionPose The pose of the robot as measured by the vision camera.
      * @param timestamp  The timestamp of the vision measurement in seconds.
      */
-    public void addVisionMeasurement(
-            Pose2d visionPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {
+    public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {
         poseEstimator.addVisionMeasurement(visionPose, timestamp, visionMeasurementStdDevs);
     }