Add Optionally Shown Window With Numerical Motion Data

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

@@ -124,6 +124,10 @@ open class MeepMeep @JvmOverloads constructor(private val windowSize: Int, fps:
         for (i in 0 until originalSize) {
             entityList[i].update(deltaTime)
         }
+        if (motionDataDisplayWindow.isVisible){
+            val robots = entityList.filterIsInstance<RoadRunnerBotEntity>()
+            motionDataDisplayWindow.updateData(robots)
+        }
     }
 
     private val loopManager = LoopManager(fps, update, render)
@@ -148,6 +152,8 @@ open class MeepMeep @JvmOverloads constructor(private val windowSize: Int, fps:
 
     private var canvasMouseX = 0
     private var canvasMouseY = 0
+    private val motionDataDisplayWindow = MotionDataDisplayWindow()
+
 
     init {
         // Core init
@@ -224,8 +230,16 @@ open class MeepMeep @JvmOverloads constructor(private val windowSize: Int, fps:
                     val clipboard = Toolkit.getDefaultToolkit().systemClipboard
                     clipboard.setContents(stringSelection, null)
                 }
+                if (e.keyCode == KeyEvent.VK_P) {
+                    if (motionDataDisplayWindow.isVisible) {
+                        motionDataDisplayWindow.isVisible = false
+                    } else {
+                        val robots = entityList.filterIsInstance<RoadRunnerBotEntity>()
+                        motionDataDisplayWindow.updateData(robots)
+                        motionDataDisplayWindow.isVisible = true
+                    }
+                }
             }
-
             override fun keyReleased(p0: KeyEvent?) {}
         })
 
@@ -482,4 +496,33 @@ open class MeepMeep @JvmOverloads constructor(private val windowSize: Int, fps:
         FIELD_CENTERSTAGE_JUICE_DARK,
         FIELD_CENTERSTAGE_JUICE_LIGHT,
     }
+    class MotionDataDisplayWindow : JFrame("Robot Trajectory Data") {
+        private val dataTextArea = JTextArea()
+
+        init {
+            setSize(500, 400)
+            defaultCloseOperation = HIDE_ON_CLOSE
+            layout = BorderLayout()
+
+            dataTextArea.font = Font("Sans", Font.PLAIN, 12)
+            dataTextArea.foreground = ColorManager.COLOR_PALETTE.GRAY_100
+            dataTextArea.background = ColorManager.COLOR_PALETTE.GRAY_800
+            dataTextArea.isEditable = false
+
+            val scrollPane = JScrollPane(dataTextArea)
+            add(scrollPane, BorderLayout.CENTER)
+        }
+
+        fun updateData(robots: List<RoadRunnerBotEntity>) {
+            val data = StringBuilder()
+            robots.forEach { robot ->
+                data.append("Robot: ${robot.name}\n")
+                data.append("Velocity:\n   X: ${robot.velocityinx()}\n   Y: ${robot.velocityiny()}\n")
+                data.append("Acceleration:\n   X: ${robot.accelerationinx()}\n   Y: ${robot.accelerationiny()}\n")
+                data.append("Jerk:\n   X: ${robot.jerkinx()}\n   Y: ${robot.jerkiny()}\n\n")
+            }
+            dataTextArea.text = data.toString()
+        }
+    }
+
 }

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

@@ -17,7 +17,8 @@ open class BotEntity(
 
     var pose: Pose2d,
     private var colorScheme: ColorScheme,
-    private val opacity: Double
+    private val opacity: Double,
+    private val name: String
 ) : ThemedEntity {
     override val tag = "DEFAULT_BOT_ENTITY"
 

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

@@ -19,6 +19,7 @@ class DefaultBotBuilder(private val meepMeep: MeepMeep) {
     private var opacity = 0.8
 
     private var driveTrainType = DriveTrainType.MECANUM
+    private var name = "RR_Bot_Default"
 
     fun setDimensions(width: Double, height: Double): DefaultBotBuilder {
         this.width = width
@@ -35,7 +36,10 @@ class DefaultBotBuilder(private val meepMeep: MeepMeep) {
 
     fun setConstraints(constraints: Constraints): DefaultBotBuilder {
         this.constraints = constraints
-
+        return this
+    }
+    fun setName(robotName: String): DefaultBotBuilder {
+        this.name = robotName
         return this
     }
 
@@ -69,7 +73,7 @@ class DefaultBotBuilder(private val meepMeep: MeepMeep) {
             constraints,
             width, height,
             startPose, colorScheme ?: meepMeep.colorManager.theme, opacity,
-            driveTrainType, false
+            driveTrainType, false, name
         )
     }
 }

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

@@ -3,6 +3,7 @@ package com.noahbres.meepmeep.roadrunner.entity
 import com.acmerobotics.roadrunner.Action
 import com.acmerobotics.roadrunner.Pose2d
 import com.acmerobotics.roadrunner.SleepAction
+import com.acmerobotics.roadrunner.Vector2d
 import com.noahbres.meepmeep.MeepMeep
 import com.noahbres.meepmeep.core.colorscheme.ColorScheme
 import com.noahbres.meepmeep.core.entity.BotEntity
@@ -12,7 +13,10 @@ import com.noahbres.meepmeep.roadrunner.Constraints
 import com.noahbres.meepmeep.roadrunner.DriveShim
 import com.noahbres.meepmeep.roadrunner.DriveTrainType
 import com.noahbres.meepmeep.roadrunner.ui.TrajectoryProgressSliderMaster
+import java.awt.Graphics2D
 import kotlin.math.min
+import kotlin.math.roundToInt
+import kotlin.math.sqrt
 
 // TODO(ryanbrott): seems like the bot should own the path entities and selectively update/render the ones
 // that need it and also update the pose (perhaps there should be another Entity interface?)
@@ -28,8 +32,9 @@ class RoadRunnerBotEntity(
 
     private var driveTrainType: DriveTrainType = DriveTrainType.MECANUM,
 
-    var listenToSwitchThemeRequest: Boolean = false
-) : BotEntity(meepMeep, width, height, pose, colorScheme, opacity), EntityEventListener {
+    var listenToSwitchThemeRequest: Boolean = false,
+    var name: String
+) : BotEntity(meepMeep, width, height, pose, colorScheme, opacity, name), EntityEventListener {
     companion object {
         const val SKIP_LOOPS = 2
     }
@@ -52,14 +57,20 @@ class RoadRunnerBotEntity(
             actionEntity?.trajectoryProgress = value
             field = value
         }
-
+    private var previousLoopTrajectorySequenceElapsedTime = 0.0
+
     var trajectoryPaused = false
 
     private var skippedLoops = 0
 
     private var sliderMaster: TrajectoryProgressSliderMaster? = null
     private var sliderMasterIndex: Int? = null
 
+    var previousPose: Pose2d = Pose2d(0.0,0.0,0.0)
+    var velocity: Pose2d = Pose2d(0.0,0.0,0.0)
+    var acceleration: Pose2d = Pose2d(0.0,0.0,0.0)
+    var jerk: Pose2d = Pose2d(0.0,0.0,0.0)
+
     override fun update(deltaTime: Long) {
         if (!running) return
 
@@ -113,8 +124,44 @@ class RoadRunnerBotEntity(
                 sliderMaster?.reportDone(sliderMasterIndex ?: -1)
             }
         }.exhaustive
+        super.update(deltaTime)
+        if (!trajectoryPaused) {
+            val newVelocity = Pose2d(Vector2d((pose.position.x - previousPose.position.x)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime), (pose.position.y - previousPose.position.y)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime)), (pose.heading - previousPose.heading)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime))
+            val newAcceleration = Pose2d(Vector2d((newVelocity.position.x - velocity.position.x)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime), (newVelocity.position.y - velocity.position.y)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime)), (newVelocity.heading - velocity.heading)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime))
+            jerk = Pose2d(Vector2d((newAcceleration.position.x - acceleration.position.x)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime), (newAcceleration.position.y - acceleration.position.y)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime)), (newAcceleration.heading - acceleration.heading)/(trajectorySequenceElapsedTime - previousLoopTrajectorySequenceElapsedTime))
+
+            velocity = newVelocity
+            acceleration = newAcceleration
+            previousPose = pose
+        }
+        previousLoopTrajectorySequenceElapsedTime = trajectorySequenceElapsedTime
+    }
+
+    fun velocityinx(): Double {
+        return String.format("%.3f", velocity.position.x).toDouble()
+    }
+
+    fun accelerationinx(): Double {
+        return String.format("%.3f", acceleration.position.x).toDouble()
+    }
+
+    fun jerkinx(): Double {
+        return String.format("%.3f", jerk.position.x).toDouble()
     }
 
+    fun velocityiny(): Double {
+        return String.format("%.3f", velocity.position.y).toDouble()
+    }
+
+    fun accelerationiny(): Double {
+        return String.format("%.3f", acceleration.position.y).toDouble()
+    }
+
+    fun jerkiny(): Double {
+        return String.format("%.3f", jerk.position.y).toDouble()
+    }
+
+
     fun start() {
         running = true
         trajectorySequenceElapsedTime = 0.0
@@ -159,7 +206,6 @@ class RoadRunnerBotEntity(
         if (listenToSwitchThemeRequest)
             super.switchScheme(scheme)
     }
-
     fun setTrajectoryProgressSliderMaster(master: TrajectoryProgressSliderMaster, index: Int) {
         sliderMaster = master
         sliderMasterIndex = index