Compile time errors for conflicting waypoints (#35)

* enforce check to make sure waypoints match path frames * end command if things don't agree * rename + don't flip odowaypoint * compile time error for conflicting frame path definitions * remove path frame reference * proper naming for these constructors
team4099 · Feb 20, 2024 · ab5283f · ab5283f
1 parent db66544
commit ab5283f
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 23 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt
@@ -14,7 +14,7 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() {
     addRequirements(drivetrain)
 
     addCommands(
-      DrivePathCommand(
+      DrivePathCommand.createPathInOdometryFrame(
         drivetrain,
         {
           listOf(

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -65,9 +65,10 @@ import java.util.function.Supplier
 import kotlin.math.PI
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
-class DrivePathCommand(
+class DrivePathCommand<T : Waypoint>
+private constructor(
   val drivetrain: Drivetrain,
-  private val waypoints: Supplier<List<Waypoint>>,
+  private val waypoints: Supplier<List<T>>,
   val resetPose: Boolean = false,
   val keepTrapping: Boolean = false,
   val flipForAlliances: Boolean = true,
@@ -77,6 +78,7 @@ class DrivePathCommand(
   var stateFrame: FrameType = FrameType.ODOMETRY,
   var pathFrame: FrameType = FrameType.FIELD,
 ) : Command() {
+
   private val xPID: PIDController<Meter, Velocity<Meter>>
   private val yPID: PIDController<Meter, Velocity<Meter>>
 
@@ -211,26 +213,6 @@ class DrivePathCommand(
   }
 
   override fun initialize() {
-    val containsAllOdometryWaypoints = waypoints.get().all { it is OdometryWaypoint }
-    val containsAllFieldWaypoints = waypoints.get().all { it is FieldWaypoint }
-    Logger.recordOutput("Pathfollow/allRelative", containsAllOdometryWaypoints)
-    Logger.recordOutput("Pathfollow/allField", containsAllFieldWaypoints)
-
-    if ((containsAllOdometryWaypoints && (pathFrame == FrameType.FIELD))) {
-      errorString = "Cannot pass in Relative Waypoints when pathFrame is FrameType.FIELD"
-      end(true)
-    }
-
-    if ((containsAllFieldWaypoints && (pathFrame == FrameType.ODOMETRY))) {
-      errorString = "Cannot pass in Field Waypoints when pathFrame is FrameType.ODOMETRY"
-      end(true)
-    }
-
-    if (!(containsAllFieldWaypoints || containsAllOdometryWaypoints)) {
-      errorString = "Cannot pass in both Field Waypoints and Relative Waypoints"
-      end(true)
-    }
-
     odoTField = drivetrain.odomTField
     pathTransform =
       Transform2d(
@@ -417,4 +399,57 @@ class DrivePathCommand(
         )
     }
   }
+
+  companion object {
+    operator fun invoke() {
+      return
+    }
+    fun createPathInOdometryFrame(
+      drivetrain: Drivetrain,
+      waypoints: Supplier<List<OdometryWaypoint>>,
+      resetPose: Boolean = false,
+      keepTrapping: Boolean = false,
+      flipForAlliances: Boolean = true,
+      endPathOnceAtReference: Boolean = true,
+      leaveOutYAdjustment: Boolean = false,
+      endVelocity: Velocity2d = Velocity2d(),
+      stateFrame: FrameType = FrameType.ODOMETRY,
+    ): DrivePathCommand<OdometryWaypoint> =
+      DrivePathCommand(
+        drivetrain,
+        waypoints,
+        resetPose,
+        keepTrapping,
+        flipForAlliances,
+        endPathOnceAtReference,
+        leaveOutYAdjustment,
+        endVelocity,
+        stateFrame,
+        FrameType.ODOMETRY
+      )
+
+    fun createPathInFieldFrame(
+      drivetrain: Drivetrain,
+      waypoints: Supplier<List<FieldWaypoint>>,
+      resetPose: Boolean = false,
+      keepTrapping: Boolean = false,
+      flipForAlliances: Boolean = true,
+      endPathOnceAtReference: Boolean = true,
+      leaveOutYAdjustment: Boolean = false,
+      endVelocity: Velocity2d = Velocity2d(),
+      stateFrame: FrameType = FrameType.ODOMETRY,
+    ): DrivePathCommand<FieldWaypoint> =
+      DrivePathCommand(
+        drivetrain,
+        waypoints,
+        resetPose,
+        keepTrapping,
+        flipForAlliances,
+        endPathOnceAtReference,
+        leaveOutYAdjustment,
+        endVelocity,
+        stateFrame,
+        FrameType.FIELD
+      )
+  }
 }