diff --git a/TeamCode/src/main/kotlin/pioneer/Constants.kt b/TeamCode/src/main/kotlin/pioneer/Constants.kt index a77b247..7c02940 100644 --- a/TeamCode/src/main/kotlin/pioneer/Constants.kt +++ b/TeamCode/src/main/kotlin/pioneer/Constants.kt @@ -201,7 +201,7 @@ object Constants { } object TransferData { - var allianceColor = AllianceColor.NEUTRAL + var allianceColor = AllianceColor.NONE var turretPositionTicks = 0 var spindexerPositionTicks = 0 } diff --git a/TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt b/TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt index 41888f3..8ee313c 100644 --- a/TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt +++ b/TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt @@ -85,7 +85,7 @@ object GoalTagProcessor { ?.let { GoalTag.RED } } - AllianceColor.NEUTRAL -> { + AllianceColor.NONE -> { null } } diff --git a/TeamCode/src/main/kotlin/pioneer/decode/Obelisk.kt b/TeamCode/src/main/kotlin/pioneer/decode/Obelisk.kt index 80e467e..574f470 100644 --- a/TeamCode/src/main/kotlin/pioneer/decode/Obelisk.kt +++ b/TeamCode/src/main/kotlin/pioneer/decode/Obelisk.kt @@ -23,7 +23,7 @@ object Obelisk { when (alliance) { AllianceColor.BLUE -> validTags.maxByOrNull { it.ftcPose.x }?.id AllianceColor.RED -> validTags.minByOrNull { it.ftcPose.x }?.id - AllianceColor.NEUTRAL -> null + AllianceColor.NONE -> null } return motifTagId?.let { Motif(it) } } diff --git a/TeamCode/src/main/kotlin/pioneer/decode/Points.kt b/TeamCode/src/main/kotlin/pioneer/decode/Points.kt index 9c18531..21cb640 100644 --- a/TeamCode/src/main/kotlin/pioneer/decode/Points.kt +++ b/TeamCode/src/main/kotlin/pioneer/decode/Points.kt @@ -32,7 +32,7 @@ class Points( when (c) { AllianceColor.RED -> this AllianceColor.BLUE -> Pose(-this.x, this.y, -this.theta) - AllianceColor.NEUTRAL -> this + AllianceColor.NONE -> this } // Key positions on the field diff --git a/TeamCode/src/main/kotlin/pioneer/general/AllianceColor.kt b/TeamCode/src/main/kotlin/pioneer/general/AllianceColor.kt index 41d0cdd..c38984d 100644 --- a/TeamCode/src/main/kotlin/pioneer/general/AllianceColor.kt +++ b/TeamCode/src/main/kotlin/pioneer/general/AllianceColor.kt @@ -3,5 +3,5 @@ package pioneer.general enum class AllianceColor { RED, BLUE, - NEUTRAL, + NONE, } diff --git a/TeamCode/src/main/kotlin/pioneer/general/Period.kt b/TeamCode/src/main/kotlin/pioneer/general/Period.kt new file mode 100644 index 0000000..2d67f89 --- /dev/null +++ b/TeamCode/src/main/kotlin/pioneer/general/Period.kt @@ -0,0 +1,7 @@ +package pioneer.general + +enum class Period { + AUTO, + TELEOP, + NONE, +} diff --git a/TeamCode/src/main/kotlin/pioneer/helpers/OpModeDataTransfer.kt b/TeamCode/src/main/kotlin/pioneer/helpers/OpModeDataTransfer.kt new file mode 100644 index 0000000..de56f2a --- /dev/null +++ b/TeamCode/src/main/kotlin/pioneer/helpers/OpModeDataTransfer.kt @@ -0,0 +1,36 @@ +package pioneer.helpers + +import com.google.gson.Gson +import org.firstinspires.ftc.robotcore.internal.system.AppUtil +import java.io.File +import pioneer.Bot + + +object OpModeDataTransfer { + data class OMDT( + val bot: Bot? = null, + var timestamp: Long = System.currentTimeMillis(), + val data: MutableMap = mutableMapOf() + ) + + private val gson = Gson() + private val file: File by lazy { + AppUtil.getInstance().getSettingsFile("omdt.json").apply { + parentFile?.mkdirs() + } + } + + fun save(value: OMDT) { + file.writeText(gson.toJson(value)) + } + + fun loadOrNull(): OMDT? = try { + if (file.exists()) gson.fromJson(file.readText(), OMDT::class.java) + else null + } catch (e: Exception) { + null + } + + // Deletes the OMDT file. Safe to call even if the file doesn't exist. + fun clear() = file.delete() +} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/BaseOpMode.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/BaseOpMode.kt index 4c47509..f819f27 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/BaseOpMode.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/BaseOpMode.kt @@ -1,25 +1,32 @@ package pioneer.opmodes import com.acmerobotics.dashboard.telemetry.TelemetryPacket +import com.acmerobotics.dashboard.FtcDashboard import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.eventloop.opmode.TeleOp import pioneer.Bot import pioneer.hardware.MecanumBase +import pioneer.localization.localizers.Pinpoint import pioneer.helpers.Chrono import pioneer.helpers.FileLogger -import pioneer.localization.localizers.Pinpoint +import pioneer.general.Period +import pioneer.general.AllianceColor +import pioneer.helpers.OpModeDataTransfer // Base OpMode class to be extended by all user-defined OpModes -abstract class BaseOpMode : OpMode() { - // Bot instance to be defined in subclasses +abstract class BaseOpMode( + private val period: Period = Period.NONE, + private val allianceColor: AllianceColor = AllianceColor.NONE +) : OpMode() { + // Bot instance protected lateinit var bot: Bot - + // Telemetry packet for dashboard protected var telemetryPacket = TelemetryPacket() // Dashboard instance private val dashboard = - com.acmerobotics.dashboard.FtcDashboard - .getInstance() + FtcDashboard.getInstance() // Tracker and getter for dt protected val chrono = Chrono() @@ -30,11 +37,29 @@ abstract class BaseOpMode : OpMode() { get() = getRuntime() final override fun init() { - onInit() // Call user-defined init method - bot.initAll() // Initialize bot hardware + // Auto-load bot in TELEOP period + if (period == Period.TELEOP) { + OpModeDataTransfer.loadOrNull()?.let { omdt -> + // Reinitialize bot from saved object + omdt.bot?.let { savedBot -> + bot = savedBot + // bot.initAll() // Re-initialize hardware (TODO: needs testing) + } + } + } + + // If bot wasn't loaded from OMDT, call user init if (!::bot.isInitialized) { - throw IllegalStateException("Bot not initialized. Please set 'bot' in onInit().") + onInit() // Call user-defined init method + if (!::bot.isInitialized) { + throw IllegalStateException("Bot not initialized. Please set 'bot' in onInit().") + } + bot.initAll() // Initialize bot hardware + } else { + // Bot was loaded, but still call onInit for any additional setup + onInit() } + updateTelemetry() } @@ -59,6 +84,20 @@ abstract class BaseOpMode : OpMode() { bot.mecanumBase?.stop() // Ensure motors are stopped FileLogger.flush() // Flush any logged data onStop() // Call user-defined stop method + + // Auto-save data in AUTO period, clear in TELEOP + when (period) { + Period.AUTO -> { + val omdt = OpModeDataTransfer.OMDT( + bot = bot, + ) + OpModeDataTransfer.save(omdt) + } + Period.TELEOP -> { + OpModeDataTransfer.clear() + } + else -> { /* No data transfer for NONE */ } + } } private fun updateTelemetry() { diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/other/testing/TestAutoDataTransfer.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/other/testing/TestAutoDataTransfer.kt new file mode 100644 index 0000000..fc3aa50 --- /dev/null +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/other/testing/TestAutoDataTransfer.kt @@ -0,0 +1,43 @@ +package pioneer.opmodes.other.testing + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import pioneer.Bot +import pioneer.BotType +import pioneer.general.AllianceColor +import pioneer.general.Period +import pioneer.opmodes.BaseOpMode + +@Autonomous(name = "Test Auto Data Transfer", group = "Testing") +class TestAutoDataTransfer : BaseOpMode( + period = Period.AUTO, + allianceColor = AllianceColor.RED +) { + + override fun onInit() { + bot = Bot.fromType(BotType.GOBILDA_STARTER_BOT, hardwareMap) + telemetry.addLine("Test Auto - Will save bot for teleop") + telemetry.addLine("Press START to run") + telemetry.update() + } + + override fun onLoop() { + val currentPose = bot.pinpoint?.pose + + telemetry.addLine("✓ Bot will auto-save on stop!") + telemetry.addLine() + telemetry.addData("Alliance", "RED") + telemetry.addData("Bot Type", bot.type) + if (currentPose != null) { + telemetry.addData("Current Pose", "x=%.1f, y=%.1f, θ=%.1f°", + currentPose.x, currentPose.y, currentPose.theta) + } + telemetry.addLine() + telemetry.addLine("Stop OpMode to save bot, then run Test Teleop") + telemetry.update() + } + + override fun onStop() { + telemetry.addLine("Auto stopped - Bot saved by BaseOpMode") + telemetry.update() + } +} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/other/testing/TestTeleopDataTransfer.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/other/testing/TestTeleopDataTransfer.kt new file mode 100644 index 0000000..b6bdae1 --- /dev/null +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/other/testing/TestTeleopDataTransfer.kt @@ -0,0 +1,73 @@ +package pioneer.opmodes.other.testing + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import pioneer.Bot +import pioneer.BotType +import pioneer.general.AllianceColor +import pioneer.general.Period +import pioneer.helpers.Pose +import pioneer.opmodes.BaseOpMode + +@TeleOp(name = "Test Teleop Data Transfer", group = "Testing") +class TestTeleopDataTransfer : BaseOpMode( + period = Period.TELEOP, + allianceColor = AllianceColor.RED +) { + + override fun onInit() { + // Bot is already loaded by BaseOpMode if it exists from AUTO + // Otherwise, we need to create a new one + // We can check by looking at bot.type or just always show status + + telemetry.addLine("=== BOT INITIALIZATION ===") + telemetry.addLine("Bot loaded - Check if from AUTO or new") + telemetry.addData("Bot Type", bot.type) + bot.pinpoint?.pose?.let { pose -> + telemetry.addData("Starting Pose", "x=%.1f, y=%.1f, θ=%.1f°".format( + pose.x, pose.y, pose.theta + )) + } + telemetry.addLine() + telemetry.addLine("If this is a default pose, bot was created new") + telemetry.addLine("If this is your AUTO end pose, bot was loaded!") + telemetry.addLine() + telemetry.addLine("Use left stick to drive") + telemetry.update() + } + + override fun onLoop() { + // Simple drive controls for testing + drive() + + // Display bot status + telemetry.addLine("=== BOT STATUS ===") + telemetry.addData("Bot Type", bot.type) + + bot.pinpoint?.let { pinpoint -> + telemetry.addLine() + telemetry.addLine("--- CURRENT STATE ---") + telemetry.addData("Current Pose", "x=%.1f, y=%.1f, θ=%.1f°", + pinpoint.pose.x, pinpoint.pose.y, pinpoint.pose.theta) + } + + telemetry.addLine() + telemetry.addLine("Drive: Left stick | Turn: Right stick X") + telemetry.update() + } + + private fun drive() { + val direction = Pose( + gamepad1.left_stick_x.toDouble(), + -gamepad1.left_stick_y.toDouble() + ) + bot.mecanumBase?.setDrivePower( + Pose( + vx = direction.x, + vy = direction.y, + omega = gamepad1.right_stick_x.toDouble(), + ), + 0.5, // Default power + 1000.0, // Max velocity + ) + } +} diff --git a/TeamCode/src/test/kotlin/pioneer/helpers/FileTransferTest.kt b/TeamCode/src/test/kotlin/pioneer/helpers/FileTransferTest.kt new file mode 100644 index 0000000..e308505 --- /dev/null +++ b/TeamCode/src/test/kotlin/pioneer/helpers/FileTransferTest.kt @@ -0,0 +1,178 @@ +package pioneer.helpers + +import com.google.gson.Gson +import org.junit.Test +import org.junit.Assert.* + +class FileTransferTest { + + private val gson = Gson() + + @Test + fun testDataStructureCreation() { + // Create test data with bot and custom data fields + val testData = OpModeDataTransfer.OMDT( + bot = null, // Bot can't be easily mocked in unit tests + data = mutableMapOf( + "customValue" to 42, + "motifId" to 21, + "samplesScored" to 5 + ) + ) + + // Verify fields + assertNull("Bot should be null in this test", testData.bot) + assertEquals("Custom value should match", 42, testData.data["customValue"]) + assertEquals("Motif ID should match", 21, testData.data["motifId"]) + assertEquals("Samples scored should match", 5, testData.data["samplesScored"]) + } + + @Test + fun testJsonSerialization() { + // Create test data without bot (can't serialize hardware) + val testData = OpModeDataTransfer.OMDT( + bot = null, + data = mutableMapOf( + "motifId" to 22, + "scoredSamples" to 5 + ) + ) + + // Serialize to JSON + val json = gson.toJson(testData) + + // Verify JSON contains expected data + assertTrue("JSON should contain data", json.contains("data")) + assertTrue("JSON should contain motifId", json.contains("motifId")) + assertTrue("JSON should contain timestamp", json.contains("timestamp")) + } + + @Test + fun testJsonDeserialization() { + // Create JSON string manually + val json = """ + { + "bot": null, + "timestamp": 1699800000000, + "data": { + "customField": "testValue", + "scoreCount": 42 + } + } + """.trimIndent() + + // Deserialize + val loaded = gson.fromJson(json, OpModeDataTransfer.OMDT::class.java) + + // Verify + assertNotNull("Loaded data should not be null", loaded) + assertEquals("Timestamp should match", 1699800000000L, loaded.timestamp) + assertNull("Bot should be null", loaded.bot) + assertEquals("Custom field should match", "testValue", loaded.data["customField"]) + } + + @Test + fun testRoundTripSerialization() { + // Create test data with various fields + val original = OpModeDataTransfer.OMDT( + bot = null, + data = mutableMapOf( + "motifId" to 23, + "notes" to "Test auto run", + "allianceColor" to "RED" + ) + ) + + // Serialize and deserialize + val json = gson.toJson(original) + val restored = gson.fromJson(json, OpModeDataTransfer.OMDT::class.java) + + // Verify all data survived the round trip + assertNotNull("Restored data should not be null", restored) + assertNull("Bot should be null", restored.bot) + assertEquals("Motif ID should match", 23.0, restored.data["motifId"]) + assertEquals("Notes should match", "Test auto run", restored.data["notes"]) + assertEquals("Alliance color should match", "RED", restored.data["allianceColor"]) + } + + @Test + fun testMultipleDataTypes() { + // Test various data types in the map + val testData = OpModeDataTransfer.OMDT( + bot = null, + data = mutableMapOf( + "intValue" to 42, + "doubleValue" to 3.14159, + "stringValue" to "hello", + "boolValue" to true, + "obeliskPattern" to 1 + ) + ) + + // Serialize and deserialize + val json = gson.toJson(testData) + val restored = gson.fromJson(json, OpModeDataTransfer.OMDT::class.java) + + // Verify all types + assertNotNull("Restored data should not be null", restored) + assertEquals("Int value should survive", 42.0, restored.data["intValue"]) + assertEquals("Double value should survive", 3.14159, restored.data["doubleValue"]) + assertEquals("String value should survive", "hello", restored.data["stringValue"]) + assertEquals("Bool value should survive", true, restored.data["boolValue"]) + assertEquals("Obelisk pattern should survive", 1.0, restored.data["obeliskPattern"]) + } + + @Test + fun testTimestampGeneration() { + val beforeTime = System.currentTimeMillis() + val testData = OpModeDataTransfer.OMDT() + val afterTime = System.currentTimeMillis() + + assertTrue("Timestamp should be after beforeTime", testData.timestamp >= beforeTime) + assertTrue("Timestamp should be before afterTime", testData.timestamp <= afterTime) + } + + @Test + fun testBotFieldHandling() { + // Test that bot field can be null and handled properly + val testData = OpModeDataTransfer.OMDT( + bot = null, + data = mutableMapOf( + "testField" to "testValue" + ) + ) + + // Serialize and deserialize + val json = gson.toJson(testData) + val restored = gson.fromJson(json, OpModeDataTransfer.OMDT::class.java) + + // Verify + assertNotNull("Restored data should not be null", restored) + assertNull("Bot should still be null", restored.bot) + assertEquals("Test field should survive", "testValue", restored.data["testField"]) + } + + @Test + fun testDataMapMutability() { + // Test that data map is mutable and can be modified + val testData = OpModeDataTransfer.OMDT() + + // Add items to map + testData.data["key1"] = "value1" + testData.data["key2"] = 123 + testData.data["key3"] = true + + // Verify items were added + assertEquals("Key1 should match", "value1", testData.data["key1"]) + assertEquals("Key2 should match", 123, (testData.data["key2"] as Number).toInt()) + assertEquals("Key3 should match", true, testData.data["key3"] as Boolean) + + // Serialize and check + val json = gson.toJson(testData) + val restored = gson.fromJson(json, OpModeDataTransfer.OMDT::class.java) + + assertEquals("Key1 should survive", "value1", restored.data["key1"]) + assertEquals("Key2 should survive", 123, (restored.data["key2"] as Number).toInt()) + assertEquals("Key3 should survive", true, restored.data["key3"] as Boolean) + } +} diff --git a/TeamCode/src/test/kotlin/pioneer/pathing/MotionProfileTest.kt.bak b/TeamCode/src/test/kotlin/pioneer/pathing/MotionProfileTest.kt.bak new file mode 100644 index 0000000..ce730b5 --- /dev/null +++ b/TeamCode/src/test/kotlin/pioneer/pathing/MotionProfileTest.kt.bak @@ -0,0 +1,76 @@ +import pioneer.localization.Pose +import pioneer.pathing.follower.Follower +import pioneer.pathing.paths.HermitePath +import org.junit.Test + +class MotionProfileTest { + @Test + fun test() { + val path = HermitePath.Builder() + .addPoint(Pose(0.0, 0.0)) + .addPoint(Pose(0.0, 100.0)) + .build() + val follower = Follower() + follower.path = path + val motionProfile = follower.motionProfile!! + val startState = motionProfile.start() + val endState = motionProfile.end() + val duration = motionProfile.duration() + println("Start State: $startState") + println("End State: $endState") + println("Duration: $duration") + for (i in 0..100) { + val t = i / 100.0 * duration + val state = motionProfile[t] + println("$t: $state") + } + } + + @Test + fun followerTest() { + val path = HermitePath.Builder() + .addPoint(Pose(0.0, 0.0)) + .addPoint(Pose(0.0, 100.0)) + .build() + val follower = Follower() + follower.path = path + + // Simulate update steps every 10ms + for (i in 0..1000) { + val targetState = follower.targetState!! + val pathT = targetState.x / path.getLength() + val targetPoint = path.getPoint(pathT) + val targetPointFirstDerivative = path.getTangent(pathT).normalize() + val targetPointSecondDerivative = path.getSecondDerivative(pathT) + val targetVelocity = targetPointFirstDerivative * targetState.v + val targetAcceleration = targetPointSecondDerivative * (targetState.v * targetState.v) + + targetPointFirstDerivative * targetState.a + + println("Time: ${i * 0.01}s, Target State: $targetState, Target Point: $targetPoint, " + + "Target Velocity: $targetVelocity, Target Acceleration: $targetAcceleration") + + if (pathT >= 1.0 - 1e-6) { + println("Reached end of path") + break + } + + // Simulate a delay of 10ms + Thread.sleep(10) + } + } + + @Test + fun tempTest() { + val path = HermitePath.Builder() + .addPoint(Pose(0.0, 0.0)) + .addPoint(Pose(0.0, 100.0)) + .build() + + for (i in 0..100) { + val t = i / 100.0 + val p = path.getPoint(t) + val d = path.getTangent(t) + println("$t, $p, $d") + } + } +} \ No newline at end of file