Auto intake

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/RobotContainer.kt

@@ -1,7 +1,7 @@
 package com.team4099.robot2023
 
 import com.team4099.robot2023.auto.AutonomousSelector
-import com.team4099.robot2023.commands.AutoScoreCommand
+import com.team4099.robot2023.commands.AutoIntakeCommand
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
@@ -202,7 +202,7 @@ object RobotContainer {
     )
 
     ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand())
-    ControlBoard.autoScore.whileTrue(AutoScoreCommand(drivetrain, superstructure))
+    ControlBoard.autoScore.whileTrue(AutoIntakeCommand(drivetrain, superstructure))
 
     ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
     //    ControlBoard.dpadDown.whileTrue(PickupFromSubstationCommand(drivetrain, superstructure))

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

@@ -0,0 +1,170 @@
+package com.team4099.robot2023.commands
+
+import com.team4099.lib.logging.LoggedTunableValue
+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.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.geometry.Translation2d
+import org.team4099.lib.units.base.inInches
+import org.team4099.lib.units.base.inches
+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
+import com.team4099.robot2023.config.constants.Constants.Universal.Substation as Substation
+
+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)
+        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
+              )
+            )
+        }
+
+        // Replace last waypoint with the correct waypoint based on operator app
+        val finalPose =
+          AllianceFlipUtil.apply(
+            when (superstructure.objective.substation) {
+              Substation.DOUBLE_SUBSTATION_LEFT -> {
+                Pose2d(
+                  FieldConstants.getTagPose(
+                    Constants.AprilTagIds.BLUE_DOUBLE_SUBSTATION_ID
+                  )!!
+                    .toPose2d()
+                    .translation +
+                    Translation2d(
+                      -doubleSubstationXoffset.get(), doubleSubstationYoffset.get()
+                    ),
+                  0.0.degrees
+                )
+              }
+              Substation.DOUBLE_SUBSTATION_RIGHT -> {
+                Pose2d(
+                  FieldConstants.getTagPose(
+                    Constants.AprilTagIds.BLUE_DOUBLE_SUBSTATION_ID
+                  )!!
+                    .toPose2d()
+                    .translation +
+                    Translation2d(
+                      -doubleSubstationXoffset.get(), -doubleSubstationYoffset.get()
+                    ),
+                  0.0.degrees
+                )
+              }
+              Substation.SINGLE_SUBSTATION -> {
+                Pose2d(
+                  FieldConstants.LoadingZone.singleSubstationTranslation +
+                    Translation2d(
+                      singleSubstationXoffset.get(), -singleSubstationYoffset.get()
+                    ),
+                  90.0.degrees
+                )
+              }
+              Substation.NONE -> {
+                Pose2d(
+                  FieldConstants.LoadingZone.singleSubstationTranslation +
+                    Translation2d(
+                      singleSubstationXoffset.get(), -singleSubstationYoffset.get()
+                    ),
+                  90.0.degrees
+                )
+              } // Return single substation by default
+            }
+          )
+
+        intermediaryWaypoints =
+          intermediaryWaypoints.mapIndexed { index, waypoint ->
+            waypoint.takeUnless { index == intermediaryWaypoints.size - 1 }
+              ?: Waypoint(
+                finalPose.pose2d.translation, holonomicRotation = finalPose.pose2d.rotation
+              )
+          }
+      })
+
+    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.intakeFromSubstationCommand()
+    )
+  }
+
+  companion object {
+    val doubleSubstationXoffset =
+      LoggedTunableValue(
+        "Drivetrain/doubleSubstationXOffsetInches",
+        1.0.meters,
+        Pair({ it.inInches }, { it.inches })
+      )
+    val doubleSubstationYoffset =
+      LoggedTunableValue(
+        "Drivetrain/doubleSubstationYOffsetInches",
+        0.735.meters,
+        Pair({ it.inInches }, { it.inches })
+      )
+
+    val singleSubstationXoffset =
+      LoggedTunableValue(
+        "Drivetrain/singleSubstationXOffset", 0.0.meters, Pair({ it.inInches }, { it.inches })
+      )
+    val singleSubstationYoffset =
+      LoggedTunableValue(
+        "Drivetrain/singleSubstationYOffset", 1.0.meters, Pair({ it.inInches }, { it.inches })
+      )
+  }
+}

src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt

@@ -12,7 +12,7 @@ typealias NodeTier = Constants.Universal.NodeTier
 object Constants {
   object Universal {
     val SIM_MODE = Tuning.SimType.SIM
-    const val REAL_FIELD = false
+    const val REAL_FIELD = true
 
     const val CTRE_CONFIG_TIMEOUT = 0
     const val EPSILON = 1E-9