Auto scoring with zones

src/main/deploy/pathplanner/ConeCubeAuto.path

@@ -0,0 +1,74 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.9,
+        "y": 4.97
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5159692897510797,
+        "y": 6.536703073406147
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.513683285179071,
+        "y": 6.259812754000508
+      },
+      "prevControl": {
+        "x": 4.508526802268566,
+        "y": 6.264969236911013
+      },
+      "nextControl": {
+        "x": 4.518839768089576,
+        "y": 6.2546562710900035
+      },
+      "holonomicAngle": 179.6021606941461,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.706650249237999,
+        "y": 4.908321691643384
+      },
+      "prevControl": {
+        "x": 6.655472288809161,
+        "y": 4.991216760707332
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/deploy/pathplanner/ConeCubeAuto2.path

@@ -0,0 +1,74 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 7.06,
+        "y": 4.5
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.06,
+        "y": 4.5
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.13,
+        "y": 4.8
+      },
+      "prevControl": {
+        "x": 2.13,
+        "y": 4.8
+      },
+      "nextControl": {
+        "x": 2.13,
+        "y": 4.8
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 1.9,
+        "y": 4.4
+      },
+      "prevControl": {
+        "x": 0.8999999999999999,
+        "y": 4.4
+      },
+      "nextControl": null,
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/kotlin/com/team4099/lib/math/Zone2d.kt

@@ -0,0 +1,39 @@
+package com.team4099.lib.math
+
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Translation2d
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.meters
+
+class Zone2d(var vertices: List<Translation2d>) {
+  fun containsPose(pose: Pose2d): Boolean {
+    var isInside = false
+    val minX = vertices.minOf { it.x }
+    val minY = vertices.minOf { it.y }
+    val maxX = vertices.maxOf { it.x }
+    val maxY = vertices.maxOf { it.y }
+
+    if ((pose.x !in minX..maxX) || (pose.y !in minY..maxY)) {
+      return false
+    }
+
+    var vBIndex = vertices.size - 1
+    for (vAIndex in vertices.indices) {
+      if ((vertices[vAIndex].y > pose.y) != (vertices[vBIndex].y > pose.y) &&
+        pose.x <
+        (
+          (vertices[vBIndex].x - vertices[vAIndex].x).inMeters *
+            (pose.y - vertices[vAIndex].y).inMeters /
+            (vertices[vBIndex].y - vertices[vAIndex].y).inMeters +
+            vertices[vAIndex].x.inMeters
+          )
+          .meters
+      ) {
+        isInside = !isInside
+      }
+      vBIndex = vAIndex
+    }
+
+    return isInside
+  }
+}

src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt

@@ -10,8 +10,8 @@ import java.util.Optional
 class Waypoint {
   /** Returns the translation component of the waypoint. */
   val translation: Translation2d
-  private val driveRotation: Rotation2d?
-  private val holonomicRotation: Rotation2d?
+  val driveRotation: Rotation2d?
+  val holonomicRotation: Rotation2d?
 
   /**
    * Constructs a Waypoint with a translation, drive rotation, and holonomic rotation.

src/main/kotlin/com/team4099/robot2022/BuildConstants.kt

@@ -0,0 +1,15 @@
+package com.team4099.robot2022
+
+/**
+ * Automatically generated file containing build version information.
+ */
+const val MAVEN_GROUP = ""
+const val MAVEN_NAME = "ChargedUp-2023"
+const val VERSION = "unspecified"
+const val GIT_REVISION = 11
+const val GIT_SHA = "f62076f27df05c9da57e75b873eaa1d38d512be4"
+const val GIT_DATE = "2022-12-13T19:13:28Z"
+const val GIT_BRANCH = "autologannotation"
+const val BUILD_DATE = "2023-07-19T22:09:16Z"
+const val BUILD_UNIX_TIME = 1689822556755L
+const val DIRTY = 0

src/main/kotlin/com/team4099/robot2023/Robot.kt

@@ -86,7 +86,7 @@ object Robot : LoggedRobot() {
         Constants.Universal.POWER_DISTRIBUTION_HUB_ID, PowerDistribution.ModuleType.kRev
       )
     } else {
-      when (Constants.Universal.SIM_MODE) {
+      when (Constants.Tuning.SimType.SIM) {
         Constants.Tuning.SimType.SIM -> {
           logger.addDataReceiver(NTSafePublisher())
           logSimulationAlert.set(true)

src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt

@@ -0,0 +1,89 @@
+package com.team4099.robot2023.commands
+
+import com.team4099.lib.math.Zone2d
+import com.team4099.lib.trajectory.Waypoint
+import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
+import com.team4099.robot2023.config.constants.FieldConstants
+import com.team4099.robot2023.config.constants.GamePiece
+import com.team4099.robot2023.config.constants.NodeTier
+import com.team4099.robot2023.config.constants.WaypointConstants
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Superstructure
+import com.team4099.robot2023.util.AllianceFlipUtil
+import edu.wpi.first.wpilibj2.command.Commands.runOnce
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotation2ds
+import org.team4099.lib.units.perSecond
+
+class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstructure) :
+  SequentialCommandGroup() {
+  lateinit var drivePose: Pose2d
+  lateinit var intermediaryWaypoints: List<Waypoint>
+  var currentZone: Zone2d? = null
+  lateinit var finalPose: Pose2d
+  lateinit var postAlignPose: Pose2d
+  var heading: Angle = 0.0.degrees
+  lateinit var gamePiece: GamePiece
+  lateinit var nodeTier: NodeTier
+
+  init {
+    val setupCommand =
+      runOnce({
+        drivePose = drivetrain.odometryPose
+        heading = drivetrain.fieldVelocity.heading
+        currentZone = FieldConstants.determineZone(FieldConstants.Zones.allZones, drivePose)
+        println(currentZone)
+        intermediaryWaypoints =
+          WaypointConstants.SubstationPaths.getPath(currentZone).map {
+            AllianceFlipUtil.apply(it)
+          }
+        if (intermediaryWaypoints.isEmpty()) {
+          intermediaryWaypoints =
+            listOf<Waypoint>(
+              Waypoint(
+                drivePose.pose2d.translation,
+                if (drivetrain.fieldVelocity.magnitude.absoluteValue <
+                  0.25.meters.perSecond
+                )
+                  null
+                else heading.inRotation2ds,
+                drivePose.rotation.inRotation2ds
+              )
+            )
+        }
+
+        for (vertex in FieldConstants.Zones.farLeftLane.vertices) {
+          println("${vertex.x}, ${vertex.y}")
+        }
+        println("------")
+        for (vertex in FieldConstants.Zones.closeLeftLane.vertices) {
+          println("${vertex.x}, ${vertex.y}")
+        }
+      })
+
+    addCommands(
+      setupCommand,
+      DrivePathCommand(
+        drivetrain,
+        {
+          listOf(
+            Waypoint(
+              drivePose.pose2d.translation,
+              if (drivetrain.fieldVelocity.magnitude.absoluteValue < 0.25.meters.perSecond)
+                null
+              else heading.inRotation2ds,
+              drivePose.rotation.inRotation2ds
+            )
+          ) + intermediaryWaypoints
+        },
+        keepTrapping = true,
+        flipForAlliances = false
+      ),
+      superstructure.groundIntakeConeCommand()
+    )
+  }
+}

src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt

@@ -1,11 +1,13 @@
 package com.team4099.robot2023.commands
 
+import com.team4099.lib.math.Zone2d
 import com.team4099.lib.trajectory.Waypoint
 import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.GamePiece
 import com.team4099.robot2023.config.constants.NodeTier
+import com.team4099.robot2023.config.constants.WaypointConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
@@ -26,6 +28,8 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
   lateinit var drivePose: Pose2d
   lateinit var finalPose: Pose2d
   lateinit var postAlignPose: Pose2d
+  lateinit var intermediaryWaypoints: List<Waypoint>
+  var currentZone: Zone2d? = null
   var heading: Angle = 0.0.degrees
   lateinit var gamePiece: GamePiece
   lateinit var nodeTier: NodeTier
@@ -58,6 +62,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
             )
           )
         heading = drivetrain.fieldVelocity.heading
+
+        // Determine the current zone and calculate the trajectory
+        currentZone = FieldConstants.determineZone(FieldConstants.Zones.allZones, drivePose)
+        intermediaryWaypoints =
+          WaypointConstants.CommunityPaths.getPath(currentZone).map {
+            AllianceFlipUtil.apply(it)
+          }
+
+        println(currentZone == FieldConstants.Zones.closeCenterRightLane)
+
         gamePiece =
           if (superstructure.objective.isConeNode()) Constants.Universal.GamePiece.CONE
           else GamePiece.CUBE
@@ -78,11 +92,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
                 null
               else heading.inRotation2ds,
               drivePose.rotation.inRotation2ds
-            ),
-            Waypoint(
-              finalPose.translation.translation2d, null, finalPose.rotation.inRotation2ds
             )
-          )
+          ) +
+            intermediaryWaypoints +
+            listOf(
+              Waypoint(
+                finalPose.translation.translation2d,
+                null,
+                finalPose.rotation.inRotation2ds
+              )
+            )
         },
         keepTrapping = true,
         flipForAlliances = false