diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 82bd397e..541c97d1 100644 --- 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 index 88c324c9..22b8e298 100644 --- 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 +private constructor( val drivetrain: Drivetrain, - private val waypoints: Supplier>, + private val waypoints: Supplier>, 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> private val yPID: PIDController> @@ -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>, + resetPose: Boolean = false, + keepTrapping: Boolean = false, + flipForAlliances: Boolean = true, + endPathOnceAtReference: Boolean = true, + leaveOutYAdjustment: Boolean = false, + endVelocity: Velocity2d = Velocity2d(), + stateFrame: FrameType = FrameType.ODOMETRY, + ): DrivePathCommand = + DrivePathCommand( + drivetrain, + waypoints, + resetPose, + keepTrapping, + flipForAlliances, + endPathOnceAtReference, + leaveOutYAdjustment, + endVelocity, + stateFrame, + FrameType.ODOMETRY + ) + + fun createPathInFieldFrame( + drivetrain: Drivetrain, + waypoints: Supplier>, + resetPose: Boolean = false, + keepTrapping: Boolean = false, + flipForAlliances: Boolean = true, + endPathOnceAtReference: Boolean = true, + leaveOutYAdjustment: Boolean = false, + endVelocity: Velocity2d = Velocity2d(), + stateFrame: FrameType = FrameType.ODOMETRY, + ): DrivePathCommand = + DrivePathCommand( + drivetrain, + waypoints, + resetPose, + keepTrapping, + flipForAlliances, + endPathOnceAtReference, + leaveOutYAdjustment, + endVelocity, + stateFrame, + FrameType.FIELD + ) + } }