Bring MeepMeep inline with current TrajectorSequence API

build.gradle.kts

@@ -1,6 +1,6 @@
 plugins {
     // Apply the Kotlin JVM plugin to add support for Kotlin.
-    id("org.jetbrains.kotlin.jvm") version "1.4.0"
+    id("org.jetbrains.kotlin.jvm") version "1.4.31"
 
     // Apply the java-library plugin for API and implementation separation.
     java
@@ -45,7 +45,7 @@ dependencies {
     // Use the Kotlin JUnit integration.
     testImplementation("org.jetbrains.kotlin:kotlin-test-junit5")
 
-    api("com.acmerobotics.roadrunner:core:0.5.2")
+    api("com.acmerobotics.roadrunner:core:0.5.3")
 }
 
 // Create sources Jar from main kotlin sources

gradle/wrapper/gradle-wrapper.properties

@@ -1,5 +1,5 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.6-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.7.1-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists

src/main/kotlin/com/noahbres/meepmeep/MeepMeep.kt

@@ -2,16 +2,14 @@ package com.noahbres.meepmeep
 
 import com.acmerobotics.roadrunner.geometry.Pose2d
 import com.acmerobotics.roadrunner.geometry.Vector2d
-import com.acmerobotics.roadrunner.trajectory.Trajectory
-import com.acmerobotics.roadrunner.trajectory.constraints.DriveConstraints
 import com.noahbres.meepmeep.core.colorscheme.ColorManager
 import com.noahbres.meepmeep.core.colorscheme.ColorScheme
 import com.noahbres.meepmeep.core.entity.*
 import com.noahbres.meepmeep.core.ui.WindowFrame
 import com.noahbres.meepmeep.core.util.FieldUtil
 import com.noahbres.meepmeep.core.util.LoopManager
-import com.noahbres.meepmeep.roadrunner.AddTrajectoryCallback
 import com.noahbres.meepmeep.roadrunner.AddTrajectorySequenceCallback
+import com.noahbres.meepmeep.roadrunner.Constraints
 import com.noahbres.meepmeep.roadrunner.DriveTrainType
 import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity
 import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequence
@@ -75,7 +73,7 @@ open class MeepMeep(private val windowSize: Int) {
 
         // render
         if (bg != null) {
-            if(bgAlpha < 1.0f) {
+            if (bgAlpha < 1.0f) {
                 val resetComposite = g.composite
                 val alphaComposite = AlphaComposite.getInstance(AlphaComposite.SRC_OVER, bgAlpha)
                 g.composite = alphaComposite
@@ -139,7 +137,7 @@ open class MeepMeep(private val windowSize: Int) {
         val classLoader = Thread.currentThread().contextClassLoader
 
         FONT_CMU_BOLD_LIGHT = Font.createFont(
-                Font.TRUETYPE_FONT, classLoader.getResourceAsStream("font/cmunbi.ttf")
+            Font.TRUETYPE_FONT, classLoader.getResourceAsStream("font/cmunbi.ttf")
         ).deriveFont(20f)
         FONT_CMU = Font.createFont(Font.TRUETYPE_FONT, classLoader.getResourceAsStream("font/cmunrm.ttf"))
         FONT_CMU_BOLD = Font.createFont(Font.TRUETYPE_FONT, classLoader.getResourceAsStream("font/cmunbx.ttf"))
@@ -150,7 +148,7 @@ open class MeepMeep(private val windowSize: Int) {
         DEFAULT_BOT_ENTITY = BotEntity(this, 18.0, 18.0, Pose2d(), colorManager.theme, 0.8)
         DEFAULT_AXES_ENTITY = AxesEntity(this, 0.8, colorManager.theme, FONT_CMU_BOLD_LIGHT, 20f)
         DEFAULT_COMPASS_ENTITY = CompassEntity(
-                this, colorManager.theme, 30.0, 30.0, Vector2d(-54.0, 54.0)
+            this, colorManager.theme, 30.0, 30.0, Vector2d(-54.0, 54.0)
         )
 
         // Road Runner Init
@@ -178,25 +176,24 @@ open class MeepMeep(private val windowSize: Int) {
 
         // Handle entities
         DEFAULT_ROADRUNNER_BOT_ENTITY = RoadRunnerBotEntity(
-                this,
-                DriveConstraints(
-                        30.0, 30.0, 0.0,
-                        Math.toRadians(180.0), Math.toRadians(180.0), 0.0
-                ),
-                18.0, 18.0,
-                15.0,
-                Pose2d(), colorManager.theme, 0.8)
+            this,
+            Constraints(
+                30.0, 30.0, Math.toRadians(60.0), Math.toRadians(60.0), 15.0
+            ),
+            18.0, 18.0,
+            Pose2d(), colorManager.theme, 0.8
+        )
 
         // Entities
 
         zIndexManager.setTagHierarchy(
-                "DEFAULT_BOT_ENTITY",
-                "RR_BOT_ENTITY",
-                "TURN_INDICATOR_ENTITY",
-                "MARKER_INDICATOR_ENTITY",
-                "TRAJECTORY_SEQUENCE_ENTITY",
-                "COMPASS_ENTITY",
-                "AXES_ENTITY",
+            "DEFAULT_BOT_ENTITY",
+            "RR_BOT_ENTITY",
+            "TURN_INDICATOR_ENTITY",
+            "MARKER_INDICATOR_ENTITY",
+            "TRAJECTORY_SEQUENCE_ENTITY",
+            "COMPASS_ENTITY",
+            "AXES_ENTITY",
         )
 
         //        addEntity(DEFAULT_BOT_ENTITY)
@@ -208,7 +205,7 @@ open class MeepMeep(private val windowSize: Int) {
 
     open fun start(): MeepMeep {
         // Core Start
-        if(bg == null) setBackground(Background.GRID_BLUE)
+        if (bg == null) setBackground(Background.GRID_BLUE)
         windowFrame.isVisible = true
 
         // Default added entities are initialized before color schemes are set
@@ -396,22 +393,29 @@ open class MeepMeep(private val windowSize: Int) {
         return this
     }
 
-    fun setConstraints(constraints: DriveConstraints): MeepMeep {
+    fun setConstraints(
+        maxVel: Double,
+        maxAccel: Double,
+        maxAngVel: Double,
+        maxAngAccel: Double,
+        trackWidth: Double
+    ): MeepMeep {
         if (DEFAULT_ROADRUNNER_BOT_ENTITY in entityList)
-            DEFAULT_ROADRUNNER_BOT_ENTITY.setConstraints(constraints)
-
-        return this
-    }
-
-    fun setTrackWidth(trackWidth: Double): MeepMeep {
-        if(DEFAULT_ROADRUNNER_BOT_ENTITY in entityList)
-            DEFAULT_ROADRUNNER_BOT_ENTITY.setTrackWidth(trackWidth)
+            DEFAULT_ROADRUNNER_BOT_ENTITY.setConstraints(
+                Constraints(
+                    maxVel,
+                    maxAccel,
+                    maxAngVel,
+                    maxAngAccel,
+                    trackWidth
+                )
+            )
 
         return this
     }
 
     fun setDriveTrainType(driveTrainType: DriveTrainType): MeepMeep {
-        if(DEFAULT_ROADRUNNER_BOT_ENTITY in entityList)
+        if (DEFAULT_ROADRUNNER_BOT_ENTITY in entityList)
             DEFAULT_ROADRUNNER_BOT_ENTITY.setDriveTrainType(driveTrainType)
 
         return this
@@ -420,7 +424,7 @@ open class MeepMeep(private val windowSize: Int) {
     fun followTrajectorySequence(callback: AddTrajectorySequenceCallback): MeepMeep {
         if (DEFAULT_ROADRUNNER_BOT_ENTITY in entityList)
             DEFAULT_ROADRUNNER_BOT_ENTITY.followTrajectorySequence(
-                    callback.buildTrajectorySequence(DEFAULT_ROADRUNNER_BOT_ENTITY.drive)
+                callback.buildTrajectorySequence(DEFAULT_ROADRUNNER_BOT_ENTITY.drive)
             )
 
         return this

src/main/kotlin/com/noahbres/meepmeep/core/entity/AxesEntity.kt

@@ -127,7 +127,7 @@ class AxesEntity
             )
 
             // Draw number
-            var xOffsetIn = 0.0
+            var xOffsetIn: Double
             var xOffsetPx = 0.0
             if (i > 0) {
                 xOffsetIn = X_TEXT_POSITIVE_X_OFFSET

src/main/kotlin/com/noahbres/meepmeep/roadrunner/Constraints.kt

@@ -0,0 +1,7 @@
+package com.noahbres.meepmeep.roadrunner
+
+data class Constraints(
+    val maxVel: Double, val maxAccel: Double,
+    val maxAngVel: Double, val maxAngAccel: Double,
+    val trackWidth: Double
+    )

src/main/kotlin/com/noahbres/meepmeep/roadrunner/DriveShim.kt

@@ -2,27 +2,38 @@ package com.noahbres.meepmeep.roadrunner
 
 import com.acmerobotics.roadrunner.geometry.Pose2d
 import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder
-import com.acmerobotics.roadrunner.trajectory.constraints.DriveConstraints
-import com.acmerobotics.roadrunner.trajectory.constraints.MecanumConstraints
-import com.acmerobotics.roadrunner.trajectory.constraints.TankConstraints
+import com.acmerobotics.roadrunner.trajectory.constraints.*
 import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequenceBuilder
+import java.util.*
 
-class DriveShim(driveTrainType: DriveTrainType, trajectoryConstraints: DriveConstraints, trackWidth: Double) {
-    private val constraints = when (driveTrainType) {
-        DriveTrainType.MECANUM -> MecanumConstraints(trajectoryConstraints, trackWidth)
-        DriveTrainType.TANK -> TankConstraints(trajectoryConstraints, trackWidth)
+class DriveShim(driveTrainType: DriveTrainType, private val constraints: Constraints) {
+    private val velConstraint = when (driveTrainType) {
+        DriveTrainType.MECANUM -> MinVelocityConstraint(
+            listOf(
+                AngularVelocityConstraint(constraints.maxAngVel),
+                MecanumVelocityConstraint(constraints.maxVel, constraints.trackWidth)
+            )
+        )
+        DriveTrainType.TANK -> MinVelocityConstraint(
+            listOf(
+                AngularVelocityConstraint(constraints.maxAngVel),
+                TankVelocityConstraint(constraints.maxVel, constraints.trackWidth)
+            )
+        )
     }
 
+    private val accelConstraint = ProfileAccelerationConstraint(constraints.maxAccel)
+
     fun trajectorySequenceBuilder(startPose: Pose2d): TrajectorySequenceBuilder {
-        return TrajectorySequenceBuilder(startPose, constraints)
+        return TrajectorySequenceBuilder(startPose, velConstraint, accelConstraint, constraints)
     }
 
     @JvmOverloads
     fun trajectoryBuilder(startPose: Pose2d, startHeading: Double = startPose.heading): TrajectoryBuilder {
-        return TrajectoryBuilder(startPose, startHeading, constraints)
+        return TrajectoryBuilder(startPose, startHeading, velConstraint, accelConstraint)
     }
 
     fun trajectoryBuilder(startPose: Pose2d, reversed: Boolean): TrajectoryBuilder {
-        return TrajectoryBuilder(startPose, reversed, constraints)
+        return TrajectoryBuilder(startPose, reversed, velConstraint, accelConstraint)
     }
 }

src/main/kotlin/com/noahbres/meepmeep/roadrunner/entity/RoadRunnerBotEntity.kt

@@ -1,33 +1,38 @@
 package com.noahbres.meepmeep.roadrunner.entity
 
 import com.acmerobotics.roadrunner.geometry.Pose2d
-import com.acmerobotics.roadrunner.trajectory.Trajectory
-import com.acmerobotics.roadrunner.trajectory.constraints.DriveConstraints
 import com.noahbres.meepmeep.MeepMeep
 import com.noahbres.meepmeep.core.colorscheme.ColorScheme
 import com.noahbres.meepmeep.core.entity.BotEntity
 import com.noahbres.meepmeep.core.exhaustive
 import com.noahbres.meepmeep.core.util.FieldUtil
+import com.noahbres.meepmeep.roadrunner.Constraints
 import com.noahbres.meepmeep.roadrunner.DriveShim
 import com.noahbres.meepmeep.roadrunner.DriveTrainType
 import com.noahbres.meepmeep.roadrunner.trajectorysequence.*
 import com.noahbres.meepmeep.roadrunner.ui.TrajectoryProgressSlider
 
 class RoadRunnerBotEntity(
-        meepMeep: MeepMeep,
-        private var constraints: DriveConstraints,
-        width: Double, height: Double,
-        private var trackWidth: Double,
-        pose: Pose2d,
-        private val colorScheme: ColorScheme,
-        opacity: Double
+    meepMeep: MeepMeep,
+    private var constraints: Constraints,
+
+    width: Double, height: Double,
+    pose: Pose2d,
+
+    private val colorScheme: ColorScheme,
+    opacity: Double
 ) : BotEntity(meepMeep, width, height, pose, colorScheme, opacity) {
+    companion object {
+        const val SKIP_LOOPS = 2
+        const val PROGRESS_SLIDER_HEIGHT = 20
+    }
+
     override val tag = "RR_BOT_ENTITY"
 
     override var zIndex: Int = 0
 
     private var driveTrainType = DriveTrainType.MECANUM
-    var drive = DriveShim(driveTrainType, constraints, trackWidth)
+    var drive = DriveShim(driveTrainType, constraints)
 
     var currentTrajectorySequence: TrajectorySequence? = null
 
@@ -39,10 +44,8 @@ class RoadRunnerBotEntity(
     private var trajectorySequenceElapsedTime = 0.0
     var trajectoryPaused = false
 
-    private val SKIP_LOOPS = 2
     private var skippedLoops = 0
 
-    private val PROGRESS_SLIDER_HEIGHT = 20
     private val progressSlider = TrajectoryProgressSlider(
             this,
             FieldUtil.CANVAS_WIDTH.toInt(),
@@ -91,7 +94,7 @@ class RoadRunnerBotEntity(
                     is WaitSegment -> segment.startPose
                     is TurnSegment -> segment.startPose.copy(heading = segment.motionProfile[segmentOffsetTime].x)
                     is TrajectorySegment -> segment.trajectory[segmentOffsetTime]
-                    else -> Pose2d()
+                    else -> currentTrajectorySequence!!.end()
                 }
 
                 trajectorySequenceEntity!!.markerEntityList.forEach { if (trajectorySequenceElapsedTime >= it.time) it.passed() }
@@ -142,22 +145,16 @@ class RoadRunnerBotEntity(
         meepMeep.addEntity(trajectorySequenceEntity!!)
     }
 
-    fun setTrackWidth(trackWidth: Double) {
-        this.trackWidth = trackWidth
-
-        drive = DriveShim(driveTrainType, constraints, trackWidth)
-    }
-
-    fun setConstraints(constraints: DriveConstraints) {
+    fun setConstraints(constraints: Constraints) {
         this.constraints = constraints
 
-        drive = DriveShim(driveTrainType, constraints, trackWidth)
+        drive = DriveShim(driveTrainType, constraints)
     }
 
     fun setDriveTrainType(driveTrainType: DriveTrainType) {
         this.driveTrainType = driveTrainType
 
-        drive = DriveShim(driveTrainType, constraints, trackWidth)
+        drive = DriveShim(driveTrainType, constraints)
     }
 
     override fun switchScheme(scheme: ColorScheme) {

src/main/kotlin/com/noahbres/meepmeep/roadrunner/trajectorysequence/TrajectorySequence.kt

@@ -8,6 +8,6 @@ fun TrajectorySequence.start(): Pose2d =
     if(this.isNotEmpty()) this[0].startPose else Pose2d()
 
 fun TrajectorySequence.end(): Pose2d =
-        if(this.isNotEmpty()) this.last().startPose else Pose2d()
+        if(this.isNotEmpty()) this.last().endPose else Pose2d()
 
 fun TrajectorySequence.duration(): Double = this.sumByDouble { sequenceSegment -> sequenceSegment.duration }