diff --git a/src/main/kotlin/com/noahbres/meepmeep/MeepMeep.kt b/src/main/kotlin/com/noahbres/meepmeep/MeepMeep.kt index 005c256..cb50739 100644 --- a/src/main/kotlin/com/noahbres/meepmeep/MeepMeep.kt +++ b/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() + 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() + 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) { + 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() + } + } + } diff --git a/src/main/kotlin/com/noahbres/meepmeep/core/entity/BotEntity.kt b/src/main/kotlin/com/noahbres/meepmeep/core/entity/BotEntity.kt index ebc96b1..e71d957 100644 --- a/src/main/kotlin/com/noahbres/meepmeep/core/entity/BotEntity.kt +++ b/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" diff --git a/src/main/kotlin/com/noahbres/meepmeep/roadrunner/DefaultBotBuilder.kt b/src/main/kotlin/com/noahbres/meepmeep/roadrunner/DefaultBotBuilder.kt index 0356d1a..ef99cb8 100644 --- a/src/main/kotlin/com/noahbres/meepmeep/roadrunner/DefaultBotBuilder.kt +++ b/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 ) } } \ No newline at end of file diff --git a/src/main/kotlin/com/noahbres/meepmeep/roadrunner/entity/RoadRunnerBotEntity.kt b/src/main/kotlin/com/noahbres/meepmeep/roadrunner/entity/RoadRunnerBotEntity.kt index e62b1f8..a62a9aa 100644 --- a/src/main/kotlin/com/noahbres/meepmeep/roadrunner/entity/RoadRunnerBotEntity.kt +++ b/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,7 +57,8 @@ class RoadRunnerBotEntity( actionEntity?.trajectoryProgress = value field = value } - + private var previousLoopTrajectorySequenceElapsedTime = 0.0 + var trajectoryPaused = false private var skippedLoops = 0 @@ -60,6 +66,11 @@ class RoadRunnerBotEntity( 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