From 26713f9d9f1de11169df98ca52f2e51ee0a21557 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 12 Dec 2024 20:37:42 -0500 Subject: [PATCH] RR 1.0 --- TeamCode/build.gradle | 1 + .../ftc/teamcode/MecanumDrive.kt | 3 +- .../ftc/teamcode/constants/AutoStartPose.kt | 2 +- .../ftc/teamcode/constants/PoseStorage.kt | 4 +- .../{ => tuning}/AutomaticFeedforwardTuner.kt | 2 +- .../roadrunner/{ => tuning}/BackAndForth.kt | 2 +- .../{ => tuning}/DriveVelocityPIDTuner.kt | 2 +- .../{ => tuning}/FollowerPIDTuner.kt | 2 +- .../{ => tuning}/LocalizationTest.kt | 2 +- .../{ => tuning}/ManualFeedforwardTuner.kt | 2 +- .../{ => tuning}/MaxAngularVeloTuner.kt | 2 +- .../{ => tuning}/MaxVelocityTuner.kt | 2 +- .../{ => tuning}/MotorDirectionDebugger.kt | 2 +- .../roadrunner/{ => tuning}/SplineTest.kt | 2 +- .../roadrunner/{ => tuning}/StrafeTest.kt | 2 +- .../roadrunner/{ => tuning}/StraightTest.kt | 2 +- .../{ => tuning}/TrackWidthTuner.kt | 2 +- .../TrackingWheelForwardOffsetTuner.kt | 2 +- .../TrackingWheelLateralDistanceTuner.kt | 2 +- .../roadrunner/{ => tuning}/TurnTest.kt | 2 +- .../drive/AprilTagThreeWheelLocalizer.kt | 21 + .../utils/roadrunner/drive/DriveConstants.kt | 113 +++ .../drive/StandardTrackingWheelLocalizer.kt | 81 ++ .../EmptySequenceException.kt | 3 + .../trajectorysequence/TrajectorySequence.kt | 34 + .../TrajectorySequenceBuilder.kt | 784 +++++++++++++++++ .../TrajectorySequenceRunner.kt | 219 +++++ .../sequencesegment/SequenceSegment.kt | 7 + .../sequencesegment/TrajectorySegment.kt | 7 + .../sequencesegment/TurnSegment.kt | 16 + .../sequencesegment/WaitSegment.kt | 4 + .../util/AssetsTrajectoryManager.kt | 55 ++ .../utils/roadrunner/util/AxesSigns.kt | 8 + .../utils/roadrunner/util/BNO055IMUUtil.kt | 51 ++ .../utils/roadrunner/util/DashboardUtil.kt | 44 + .../utils/roadrunner/util/Encoder.kt | 66 ++ .../utils/roadrunner/util/LoggingUtil.kt | 47 + .../utils/roadrunner/util/LynxModuleUtil.kt | 99 +++ .../utils/roadrunner/util/RegressionUtil.kt | 131 +++ .../utils/roadrunner/util/TimedAction.kt | 86 ++ .../drive/AprilTagThreeWheelLocalizer.kt | 24 - .../utils/roadrunner/drive/DriveConstants.kt | 115 --- .../drive/StandardTrackingWheelLocalizer.kt | 89 -- .../EmptySequenceException.kt | 3 - .../trajectorysequence/TrajectorySequence.kt | 38 - .../TrajectorySequenceBuilder.kt | 809 ------------------ .../TrajectorySequenceRunner.kt | 237 ----- .../sequencesegment/SequenceSegment.kt | 10 - .../sequencesegment/TrajectorySegment.kt | 9 - .../sequencesegment/TurnSegment.kt | 21 - .../sequencesegment/WaitSegment.kt | 7 - .../util/AssetsTrajectoryManager.kt | 65 -- .../utils/roadrunner/util/AxesSigns.kt | 8 - .../utils/roadrunner/util/BNO055IMUUtil.kt | 54 -- .../utils/roadrunner/util/DashboardUtil.kt | 48 -- .../teamcode/utils/roadrunner/util/Encoder.kt | 72 -- .../utils/roadrunner/util/LoggingUtil.kt | 50 -- .../utils/roadrunner/util/LynxModuleUtil.kt | 103 --- .../utils/roadrunner/util/RegressionUtil.kt | 138 --- .../utils/roadrunner/util/TimedAction.kt | 88 -- 60 files changed, 1897 insertions(+), 2009 deletions(-) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/AutomaticFeedforwardTuner.kt (99%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/BackAndForth.kt (97%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/DriveVelocityPIDTuner.kt (99%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/FollowerPIDTuner.kt (97%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/LocalizationTest.kt (96%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/ManualFeedforwardTuner.kt (99%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/MaxAngularVeloTuner.kt (97%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/MaxVelocityTuner.kt (98%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/MotorDirectionDebugger.kt (98%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/SplineTest.kt (94%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/StrafeTest.kt (93%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/StraightTest.kt (93%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/TrackWidthTuner.kt (97%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/TrackingWheelForwardOffsetTuner.kt (98%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/TrackingWheelLateralDistanceTuner.kt (98%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/{ => tuning}/TurnTest.kt (88%) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/DriveConstants.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/EmptySequenceException.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequence.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AssetsTrajectoryManager.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AxesSigns.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/BNO055IMUUtil.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/DashboardUtil.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/Encoder.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LoggingUtil.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LynxModuleUtil.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/RegressionUtil.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/TimedAction.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 9e39ac3..b5d6f5e 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -38,6 +38,7 @@ dependencies { annotationProcessor files('lib/OpModeAnnotationProcessor.jar') implementation 'org.apache.commons:commons-math3:3.6.1' + implementation "com.acmerobotics.roadrunner:ftc:0.1.14" implementation 'com.acmerobotics.roadrunner:core:1.0.0' implementation 'com.acmerobotics.roadrunner:actions:1.0.0' implementation 'org.ftclib.ftclib:core:2.1.1' // core diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt index 53ef3c2..0f3263e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt @@ -50,7 +50,6 @@ import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage import org.firstinspires.ftc.teamcode.messages.PoseMessage -import java.util.Arrays import java.util.LinkedList import kotlin.math.ceil import kotlin.math.max @@ -102,7 +101,7 @@ class MecanumDrive(hardwareMap: HardwareMap, var pose: Pose2d) { PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel ) val defaultVelConstraint: VelConstraint = MinVelConstraint( - Arrays.asList( + listOf( kinematics.WheelVelConstraint(PARAMS.maxWheelVel), AngularVelConstraint(PARAMS.maxAngVel) ) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt index e0797f7..492b5f1 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.constants -import com.acmerobotics.roadrunner.geometry.Pose2d +import com.acmerobotics.roadrunner.Pose2d enum class AutoStartPose(val startPose: Pose2d) { BLUE_LEFT( diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt index 037f691..7bd3e0c 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.constants -import com.acmerobotics.roadrunner.geometry.Pose2d +import com.acmerobotics.roadrunner.Pose2d object PoseStorage { - var poseEstimate = Pose2d() + var poseEstimate = Pose2d(0.0, 0.0, 0.0) } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/AutomaticFeedforwardTuner.kt similarity index 99% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/AutomaticFeedforwardTuner.kt index acfbd7d..0b9741d 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/AutomaticFeedforwardTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.FtcDashboard //import com.acmerobotics.dashboard.config.Config diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/BackAndForth.kt similarity index 97% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/BackAndForth.kt index 994a146..008484d 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/BackAndForth.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.config.Config //import com.acmerobotics.roadrunner.geometry.Pose2d diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/DriveVelocityPIDTuner.kt similarity index 99% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/DriveVelocityPIDTuner.kt index 31bcaaa..f037d27 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/DriveVelocityPIDTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.FtcDashboard //import com.acmerobotics.dashboard.config.Config diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/FollowerPIDTuner.kt similarity index 97% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/FollowerPIDTuner.kt index aa5894e..6514b88 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/FollowerPIDTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.config.Config //import com.acmerobotics.roadrunner.geometry.Pose2d diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/LocalizationTest.kt similarity index 96% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/LocalizationTest.kt index ea9af50..14fb278 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/LocalizationTest.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.roadrunner.geometry.Pose2d //import com.qualcomm.robotcore.eventloop.opmode.Disabled diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/ManualFeedforwardTuner.kt similarity index 99% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/ManualFeedforwardTuner.kt index 1b1fa6a..1b0f4a4 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/ManualFeedforwardTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.FtcDashboard //import com.acmerobotics.dashboard.config.Config diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MaxAngularVeloTuner.kt similarity index 97% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MaxAngularVeloTuner.kt index 69f1d4b..14976fc 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MaxAngularVeloTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.FtcDashboard //import com.acmerobotics.dashboard.config.Config diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MaxVelocityTuner.kt similarity index 98% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MaxVelocityTuner.kt index f7ede79..56064f9 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MaxVelocityTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.FtcDashboard //import com.acmerobotics.dashboard.telemetry.MultipleTelemetry diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MotorDirectionDebugger.kt similarity index 98% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MotorDirectionDebugger.kt index 85510d7..4f1ea84 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/MotorDirectionDebugger.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.dashboard.FtcDashboard //import com.acmerobotics.dashboard.config.Config diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/SplineTest.kt similarity index 94% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/SplineTest.kt index eb3893c..2f3ca53 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/SplineTest.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning // //import com.acmerobotics.roadrunner.geometry.Pose2d //import com.acmerobotics.roadrunner.geometry.Vector2d diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/StrafeTest.kt similarity index 93% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/StrafeTest.kt index 7d70891..b9b7e29 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/StrafeTest.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning /* * This is a simple routine to test translational drive capabilities. diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/StraightTest.kt similarity index 93% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/StraightTest.kt index b08ee95..5e286a8 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/StraightTest.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning /* * This is a simple routine to test translational drive capabilities. diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackWidthTuner.kt similarity index 97% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackWidthTuner.kt index f7a9cd2..dc090d6 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackWidthTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning /* * This routine determines the effective track width. The procedure works by executing a point turn diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackingWheelForwardOffsetTuner.kt similarity index 98% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackingWheelForwardOffsetTuner.kt index 510de28..fe65d0a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackingWheelForwardOffsetTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning /** * This routine determines the effective forward offset for the lateral tracking wheel. diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackingWheelLateralDistanceTuner.kt similarity index 98% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackingWheelLateralDistanceTuner.kt index 20c5b3a..dc723dc 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TrackingWheelLateralDistanceTuner.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning /** * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TurnTest.kt similarity index 88% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TurnTest.kt index 9e36017..7ba4f74 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/tuning/TurnTest.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.legacy.roadrunner +package org.firstinspires.ftc.teamcode.legacy.roadrunner.tuning /* * This is a simple routine to test turning capabilities. diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt new file mode 100644 index 0000000..f564324 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.drive + +//class AprilTagThreeWheelLocalizer( +// private val odometry: StandardTrackingWheelLocalizer, +//) : Localizer { +// private var _poseEstimate: Pose2d = Pose2d() +// +// override var poseEstimate: Pose2d +// get() = _poseEstimate +// set(value) { +// _poseEstimate = value +// } +// +// override val poseVelocity: Pose2d? = null +// +// override fun update() { +// // Return a kalman filter that takes the pose estimate from the odometry and april tags to combine +// odometry.update() +// TODO("Not yet implemented") +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/DriveConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/DriveConstants.kt new file mode 100644 index 0000000..af5d35f --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/DriveConstants.kt @@ -0,0 +1,113 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.drive + +/* +* Constants shared between multiple drive types. +* +* Constants generated by LearnRoadRunner.com/drive-constants +* +* TODO: Tune or adjust the following constants to fit your robot. Note that the non-final +* fields may also be edited through the dashboard (connect to the robot's WiFi network and +* navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you +* adjust them in the dashboard; **config variable changes don't persist between app restarts**. +* +* These are not the only parameters; some are located in the localizer classes, drive base classes, +* and op modes themselves. +*/ + +//@Config +//object DriveConstants { +// /* +// * These are motor constants that should be listed online for your motors. +// */ +// const val TICKS_PER_REV = 383.6 +// const val MAX_RPM = 435.0 +// +// /* +// * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. +// * Set this flag to false if drive encoders are not present and an alternative localization +// * method is in use (e.g., tracking wheels). +// * +// * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients +// * from DriveVelocityPIDTuner. +// */ +// const val RUN_USING_ENCODER = false +// var MOTOR_VELO_PID = PIDFCoefficients( +// 0.0, 0.0, 0.0, +// getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV) +// ) +// +// /* +// * These are physical constants that can be determined from your robot (including the track +// * width; it will be tune empirically later although a rough estimate is important). Users are +// * free to chose whichever linear distance unit they would like so long as it is consistently +// * used. The default values were selected with inches in mind. Road runner uses radians for +// * angular distances although most angular parameters are wrapped in Math.toRadians() for +// * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. +// */ +// var WHEEL_RADIUS = 1.88976 // in` +// var GEAR_RATIO = 1.0 // output (wheel) speed / input (motor) speed +// @JvmField +// var TRACK_WIDTH = 13.94 // in +// +// /* +// * These are the feedforward parameters used to model the drive motor behavior. If you are using +// * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive +// * motor encoders or have elected not to use them for velocity control, these values should be +// * empirically tuned. +// */ +// @JvmField +// var kV = 0.0144 +// @JvmField +// var kA = 0.0015 +// @JvmField +// var kStatic = 0.012 +// +// /* +// * These values are used to generate the trajectories for you robot. To ensure proper operation, +// * the constraints should never exceed ~80% of the robot's actual capabilities. While Road +// * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start +// * small and gradually increase them later after everything is working. All distance units are +// * inches. +// */ +// /* +// * Note from LearnRoadRunner.com: +// * The velocity and acceleration constraints were calculated based on the following equation: +// * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85 +// * Resulting in 73.17330064499293 in/s. +// * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above. +// * This is capped at 85% because there are a number of variables that will preve nt your bot from actually +// * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc. +// * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically +// * max velocity. The theoretically maximum velocity is 86.08623605293286 in/s. +// * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally +// * affected if it is aiming for a velocity not actually possible. +// * +// * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on +// * actual testing. Just set it at a reasonable value and keep increasing until your path following starts +// * to degrade. As of now, it simply mirrors the velocity, resulting in 73.17330064499293 in/s/s +// * +// * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s. +// * You are free to raise this on your own if you would like. It is best determined through experimentation. +// +// */ +// @JvmField +// var MAX_VEL = 30.0 +// @JvmField +// var MAX_ACCEL = 30.0 +// @JvmField +// var MAX_ANG_VEL = Math.toRadians(170.0) +// @JvmField +// var MAX_ANG_ACCEL = Math.toRadians(170.0) +// fun encoderTicksToInches(ticks: Double): Double { +// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV +// } +// +// fun rpmToVelocity(rpm: Double): Double { +// return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0 +// } +// +// fun getMotorVelocityF(ticksPerSecond: Double): Double { +// // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx +// return 32767 / ticksPerSecond +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt new file mode 100644 index 0000000..5792eaf --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.drive + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ +//@Config +//class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWheelLocalizer( +// listOf( +// Pose2d(0.0, LATERAL_DISTANCE / 2, 0.0), // left +// Pose2d(0.0, -LATERAL_DISTANCE / 2, 0.0), // right +// Pose2d(FORWARD_OFFSET, 0.0, Math.toRadians(90.0)) // front +// ) +//) { +// private val leftEncoder: Encoder +// private val rightEncoder: Encoder +// private val strafeEncoder: Encoder +// +// init { +// leftEncoder = Encoder( +// hardwareMap.get( +// DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName +// ) +// ) +// +// +// rightEncoder = Encoder( +// hardwareMap.get( +// DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName +// ) +// ) +// strafeEncoder = Encoder( +// hardwareMap.get( +// DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName +// ) +// ) +//// strafeEncoder.direction = Encoder.Direction.REVERSE +// } +// +// override fun getWheelPositions() = listOf( +// encoderTicksToInches(leftEncoder.currentPosition.toDouble()) * X_MULTIPLIER, +// encoderTicksToInches(rightEncoder.currentPosition.toDouble()) * X_MULTIPLIER, +// encoderTicksToInches(strafeEncoder.currentPosition.toDouble()) * Y_MULTIPLIER +// ) +// +// override fun getWheelVelocities() = listOf( +// encoderTicksToInches(leftEncoder.correctedVelocity) * X_MULTIPLIER, +// encoderTicksToInches(rightEncoder.correctedVelocity) * X_MULTIPLIER, +// encoderTicksToInches(strafeEncoder.correctedVelocity) * Y_MULTIPLIER +// ) +// +// companion object { +// val TICKS_PER_REV = 2000.0 +// val WHEEL_RADIUS = 0.944882 // in +// val GEAR_RATIO = 1.0 // output (wheel) speed / input (encoder) speed +// +// @JvmField +// var LATERAL_DISTANCE = 15.45 // in; distance between the left and right wheels +// +// @JvmField +// var FORWARD_OFFSET = -2.1 // in; offset of the lateral wheel +// +// @JvmField +// var X_MULTIPLIER = 0.9944039016245 // Multiplier in the X direction +// @JvmField +// var Y_MULTIPLIER = 0.99960646480333// Multiplier in the Y direction +// fun encoderTicksToInches(ticks: Double): Double { +// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/EmptySequenceException.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/EmptySequenceException.kt new file mode 100644 index 0000000..5e3870e --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/EmptySequenceException.kt @@ -0,0 +1,3 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence + +//class EmptySequenceException : RuntimeException() \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequence.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequence.kt new file mode 100644 index 0000000..9606768 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequence.kt @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence + +//class TrajectorySequence(sequenceList: List) { +// private val sequenceList: List +// +// init { +// if (sequenceList.isEmpty()) throw EmptySequenceException() +// this.sequenceList = Collections.unmodifiableList(sequenceList) +// } +// +// fun start(): Pose2d { +// return sequenceList[0].startPose +// } +// +// fun end(): Pose2d { +// return sequenceList[sequenceList.size - 1].endPose +// } +// +// fun duration(): Double { +// var total = 0.0 +// for (segment in sequenceList) { +// total += segment.duration +// } +// return total +// } +// +// operator fun get(i: Int): SequenceSegment { +// return sequenceList[i] +// } +// +// fun size(): Int { +// return sequenceList.size +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt new file mode 100644 index 0000000..9070c01 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt @@ -0,0 +1,784 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence + +//class TrajectorySequenceBuilder( +// startPose: Pose2d, +// startTangent: Double?, +// private val baseVelConstraint: TrajectoryVelocityConstraint, +// private val baseAccelConstraint: TrajectoryAccelerationConstraint, +// baseTurnConstraintMaxAngVel: Double, +// baseTurnConstraintMaxAngAccel: Double, +//) { +// private val resolution = 0.25 +// private val baseTurnConstraintMaxAngVel: Double +// private val baseTurnConstraintMaxAngAccel: Double +// private val sequenceSegments: MutableList +// private val temporalMarkers: MutableList +// private val displacementMarkers: MutableList +// private val spatialMarkers: MutableList +// private var currentVelConstraint: TrajectoryVelocityConstraint +// private var currentAccelConstraint: TrajectoryAccelerationConstraint +// private var currentTurnConstraintMaxAngVel: Double +// private var currentTurnConstraintMaxAngAccel: Double +// private var lastPose: Pose2d +// private var tangentOffset: Double +// private var setAbsoluteTangent: Boolean +// private var absoluteTangent: Double +// private var currentTrajectoryBuilder: TrajectoryBuilder? +// private var currentDuration: Double +// private var currentDisplacement: Double +// private var lastDurationTraj: Double +// private var lastDisplacementTraj: Double +// +// init { +// currentVelConstraint = baseVelConstraint +// currentAccelConstraint = baseAccelConstraint +// this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel +// this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel +// currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel +// currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel +// sequenceSegments = ArrayList() +// temporalMarkers = ArrayList() +// displacementMarkers = ArrayList() +// spatialMarkers = ArrayList() +// lastPose = startPose +// tangentOffset = 0.0 +// setAbsoluteTangent = startTangent != null +// absoluteTangent = startTangent ?: 0.0 +// currentTrajectoryBuilder = null +// currentDuration = 0.0 +// currentDisplacement = 0.0 +// lastDurationTraj = 0.0 +// lastDisplacementTraj = 0.0 +// } +// +// constructor( +// startPose: Pose2d, +// baseVelConstraint: TrajectoryVelocityConstraint, +// baseAccelConstraint: TrajectoryAccelerationConstraint, +// baseTurnConstraintMaxAngVel: Double, +// baseTurnConstraintMaxAngAccel: Double, +// ) : this( +// startPose, null, +// baseVelConstraint, baseAccelConstraint, +// baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel +// ) +// +// fun lineTo(endPosition: Vector2d): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineTo( +// endPosition, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun lineTo( +// endPosition: Vector2d, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineTo( +// endPosition, velConstraint, accelConstraint +// ) +// } +// } +// +// fun lineToConstantHeading(endPosition: Vector2d): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineToConstantHeading( +// endPosition, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun lineToConstantHeading( +// endPosition: Vector2d, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineToConstantHeading( +// endPosition, velConstraint, accelConstraint +// ) +// } +// } +// +// fun lineToLinearHeading(endPose: Pose2d): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineToLinearHeading( +// endPose, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun lineToLinearHeading( +// endPose: Pose2d, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineToLinearHeading( +// endPose, velConstraint, accelConstraint +// ) +// } +// } +// +// fun lineToSplineHeading(endPose: Pose2d): TrajectorySequenceBuilder { +// return addPath(AddPathCallback { +// currentTrajectoryBuilder!!.lineToSplineHeading( +// endPose, currentVelConstraint, currentAccelConstraint +// ) +// }) +// } +// +// fun lineToSplineHeading( +// endPose: Pose2d, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.lineToSplineHeading( +// endPose, velConstraint, accelConstraint +// ) +// } +// } +// +// fun strafeTo(endPosition: Vector2d): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.strafeTo( +// endPosition, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun strafeTo( +// endPosition: Vector2d, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.strafeTo( +// endPosition, velConstraint, accelConstraint +// ) +// } +// } +// +// fun forward(distance: Double): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.forward( +// distance, +// currentVelConstraint, +// currentAccelConstraint +// ) +// } +// } +// +// fun forward( +// distance: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.forward( +// distance, +// velConstraint, +// accelConstraint +// ) +// } +// } +// +// fun back(distance: Double): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.back( +// distance, +// currentVelConstraint, +// currentAccelConstraint +// ) +// } +// } +// +// fun back( +// distance: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath(AddPathCallback { +// currentTrajectoryBuilder!!.back( +// distance, +// velConstraint, +// accelConstraint +// ) +// }) +// } +// +// fun strafeLeft(distance: Double): TrajectorySequenceBuilder { +// return addPath(AddPathCallback { +// currentTrajectoryBuilder!!.strafeLeft( +// distance, +// currentVelConstraint, +// currentAccelConstraint +// ) +// }) +// } +// +// fun strafeLeft( +// distance: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath(AddPathCallback { +// currentTrajectoryBuilder!!.strafeLeft( +// distance, +// velConstraint, +// accelConstraint +// ) +// }) +// } +// +// fun strafeRight(distance: Double): TrajectorySequenceBuilder { +// return addPath(AddPathCallback { +// currentTrajectoryBuilder!!.strafeRight( +// distance, +// currentVelConstraint, +// currentAccelConstraint +// ) +// }) +// } +// +// fun strafeRight( +// distance: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath(AddPathCallback { +// currentTrajectoryBuilder!!.strafeRight( +// distance, +// velConstraint, +// accelConstraint +// ) +// }) +// } +// +// fun splineTo(endPosition: Vector2d, endHeading: Double): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineTo( +// endPosition, endHeading, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun splineTo( +// endPosition: Vector2d, +// endHeading: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineTo( +// endPosition, endHeading, velConstraint, accelConstraint +// ) +// } +// } +// +// fun splineToConstantHeading( +// endPosition: Vector2d, +// endHeading: Double, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineToConstantHeading( +// endPosition, endHeading, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun splineToConstantHeading( +// endPosition: Vector2d, +// endHeading: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineToConstantHeading( +// endPosition, endHeading, velConstraint, accelConstraint +// ) +// } +// } +// +// fun splineToLinearHeading(endPose: Pose2d, endHeading: Double): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineToLinearHeading( +// endPose, endHeading, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun splineToLinearHeading( +// endPose: Pose2d, +// endHeading: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineToLinearHeading( +// endPose, endHeading, velConstraint, accelConstraint +// ) +// } +// } +// +// fun splineToSplineHeading(endPose: Pose2d, endHeading: Double): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineToSplineHeading( +// endPose, endHeading, currentVelConstraint, currentAccelConstraint +// ) +// } +// } +// +// fun splineToSplineHeading( +// endPose: Pose2d, +// endHeading: Double, +// velConstraint: TrajectoryVelocityConstraint?, +// accelConstraint: TrajectoryAccelerationConstraint?, +// ): TrajectorySequenceBuilder { +// return addPath { +// currentTrajectoryBuilder!!.splineToSplineHeading( +// endPose, endHeading, velConstraint, accelConstraint +// ) +// } +// } +// +// private fun addPath(callback: AddPathCallback): TrajectorySequenceBuilder { +// if (currentTrajectoryBuilder == null) newPath() +// try { +// callback.run() +// } catch (e: PathContinuityViolationException) { +// newPath() +// callback.run() +// } +// val builtTraj = currentTrajectoryBuilder!!.build() +// val durationDifference = builtTraj.duration() - lastDurationTraj +// val displacementDifference = builtTraj.path.length() - lastDisplacementTraj +// lastPose = builtTraj.end() +// currentDuration += durationDifference +// currentDisplacement += displacementDifference +// lastDurationTraj = builtTraj.duration() +// lastDisplacementTraj = builtTraj.path.length() +// return this +// } +// +// fun setTangent(tangent: Double): TrajectorySequenceBuilder { +// setAbsoluteTangent = true +// absoluteTangent = tangent +// pushPath() +// return this +// } +// +// private fun setTangentOffset(offset: Double): TrajectorySequenceBuilder { +// setAbsoluteTangent = false +// tangentOffset = offset +// pushPath() +// return this +// } +// +// fun setReversed(reversed: Boolean): TrajectorySequenceBuilder { +// return if (reversed) setTangentOffset(Math.toRadians(180.0)) else setTangentOffset(0.0) +// } +// +// fun setConstraints( +// velConstraint: TrajectoryVelocityConstraint, +// accelConstraint: TrajectoryAccelerationConstraint, +// ): TrajectorySequenceBuilder { +// currentVelConstraint = velConstraint +// currentAccelConstraint = accelConstraint +// return this +// } +// +// fun resetConstraints(): TrajectorySequenceBuilder { +// currentVelConstraint = baseVelConstraint +// currentAccelConstraint = baseAccelConstraint +// return this +// } +// +// fun setVelConstraint(velConstraint: TrajectoryVelocityConstraint): TrajectorySequenceBuilder { +// currentVelConstraint = velConstraint +// return this +// } +// +// fun resetVelConstraint(): TrajectorySequenceBuilder { +// currentVelConstraint = baseVelConstraint +// return this +// } +// +// fun setAccelConstraint(accelConstraint: TrajectoryAccelerationConstraint): TrajectorySequenceBuilder { +// currentAccelConstraint = accelConstraint +// return this +// } +// +// fun resetAccelConstraint(): TrajectorySequenceBuilder { +// currentAccelConstraint = baseAccelConstraint +// return this +// } +// +// fun setTurnConstraint(maxAngVel: Double, maxAngAccel: Double): TrajectorySequenceBuilder { +// currentTurnConstraintMaxAngVel = maxAngVel +// currentTurnConstraintMaxAngAccel = maxAngAccel +// return this +// } +// +// fun resetTurnConstraint(): TrajectorySequenceBuilder { +// currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel +// currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel +// return this +// } +// +// fun addTemporalMarker(callback: MarkerCallback?): TrajectorySequenceBuilder { +// return this.addTemporalMarker(currentDuration, callback) +// } +// +// fun UNSTABLE_addTemporalMarkerOffset( +// offset: Double, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// return this.addTemporalMarker(currentDuration + offset, callback) +// } +// +// fun addTemporalMarker(time: Double, callback: MarkerCallback?): TrajectorySequenceBuilder { +// return this.addTemporalMarker(0.0, time, callback) +// } +// +// fun addTemporalMarker( +// scale: Double, +// offset: Double, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// return this.addTemporalMarker({ time: Double -> scale * time + offset }, callback) +// } +// +// fun addTemporalMarker( +// time: TimeProducer?, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// temporalMarkers.add(TemporalMarker(time!!, callback!!)) +// return this +// } +// +// fun addSpatialMarker(point: Vector2d?, callback: MarkerCallback?): TrajectorySequenceBuilder { +// spatialMarkers.add(SpatialMarker(point!!, callback!!)) +// return this +// } +// +// fun addDisplacementMarker(callback: MarkerCallback?): TrajectorySequenceBuilder { +// return this.addDisplacementMarker(currentDisplacement, callback) +// } +// +// fun UNSTABLE_addDisplacementMarkerOffset( +// offset: Double, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// return this.addDisplacementMarker(currentDisplacement + offset, callback) +// } +// +// fun addDisplacementMarker( +// displacement: Double, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// return this.addDisplacementMarker(0.0, displacement, callback) +// } +// +// fun addDisplacementMarker( +// scale: Double, +// offset: Double, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// return addDisplacementMarker( +// { displacement: Double -> scale * displacement + offset }, +// callback +// ) +// } +// +// fun addDisplacementMarker( +// displacement: DisplacementProducer?, +// callback: MarkerCallback?, +// ): TrajectorySequenceBuilder { +// displacementMarkers.add(DisplacementMarker(displacement!!, callback!!)) +// return this +// } +// +// @JvmOverloads +// fun turn( +// angle: Double, +// maxAngVel: Double = currentTurnConstraintMaxAngVel, +// maxAngAccel: Double = currentTurnConstraintMaxAngAccel, +// ): TrajectorySequenceBuilder { +// pushPath() +// val turnProfile = generateSimpleMotionProfile( +// MotionState(lastPose.heading, 0.0, 0.0, 0.0), +// MotionState(lastPose.heading + angle, 0.0, 0.0, 0.0), +// maxAngVel, +// maxAngAccel +// ) +// sequenceSegments.add(TurnSegment(lastPose, angle, turnProfile, emptyList())) +// lastPose = Pose2d( +// lastPose.x, lastPose.y, +// norm(lastPose.heading + angle) +// ) +// currentDuration += turnProfile.duration() +// return this +// } +// +// fun waitSeconds(seconds: Double): TrajectorySequenceBuilder { +// pushPath() +// sequenceSegments.add(WaitSegment(lastPose, seconds, emptyList())) +// currentDuration += seconds +// return this +// } +// +// fun addTrajectory(trajectory: Trajectory): TrajectorySequenceBuilder { +// pushPath() +// sequenceSegments.add(TrajectorySegment(trajectory)) +// return this +// } +// +// private fun pushPath() { +// if (currentTrajectoryBuilder != null) { +// val builtTraj = currentTrajectoryBuilder!!.build() +// sequenceSegments.add(TrajectorySegment(builtTraj)) +// } +// currentTrajectoryBuilder = null +// } +// +// private fun newPath() { +// if (currentTrajectoryBuilder != null) pushPath() +// lastDurationTraj = 0.0 +// lastDisplacementTraj = 0.0 +// val tangent = +// if (setAbsoluteTangent) absoluteTangent else norm(lastPose.heading + tangentOffset) +// currentTrajectoryBuilder = TrajectoryBuilder( +// lastPose, +// tangent, +// currentVelConstraint, +// currentAccelConstraint, +// resolution +// ) +// } +// +// fun build(): TrajectorySequence { +// pushPath() +// val globalMarkers = convertMarkersToGlobal( +// sequenceSegments, +// temporalMarkers, displacementMarkers, spatialMarkers +// ) +// return TrajectorySequence( +// projectGlobalMarkersToLocalSegments( +// globalMarkers, +// sequenceSegments +// ) +// ) +// } +// +// private fun convertMarkersToGlobal( +// sequenceSegments: List, +// temporalMarkers: List, +// displacementMarkers: List, +// spatialMarkers: List, +// ): List { +// val trajectoryMarkers = ArrayList() +// +// // Convert temporal markers +// for ((producer, callback) in temporalMarkers) { +// trajectoryMarkers.add( +// TrajectoryMarker(producer.produce(currentDuration), callback) +// ) +// } +// +// // Convert displacement markers +// for ((producer, callback) in displacementMarkers) { +// val time = displacementToTime( +// sequenceSegments, +// producer.produce(currentDisplacement) +// ) +// trajectoryMarkers.add( +// TrajectoryMarker( +// time, +// callback +// ) +// ) +// } +// +// // Convert spatial markers +// for ((point, callback) in spatialMarkers) { +// trajectoryMarkers.add( +// TrajectoryMarker( +// pointToTime(sequenceSegments, point), +// callback +// ) +// ) +// } +// return trajectoryMarkers +// } +// +// private fun projectGlobalMarkersToLocalSegments( +// markers: List, +// sequenceSegments: MutableList, +// ): List { +// if (sequenceSegments.isEmpty()) return emptyList() +// +// var totalSequenceDuration = 0.0 +// for (segment in sequenceSegments) { +// totalSequenceDuration += segment.duration +// } +// +// for ((time, callback) in markers) { +// lateinit var segment: SequenceSegment +// var segmentIndex = 0 +// var segmentOffsetTime = 0.0 +// var currentTime = 0.0 +// +// for (i in sequenceSegments.indices) { +// val seg = sequenceSegments[i] +// val markerTime = Math.min(time, totalSequenceDuration) +// if (currentTime + seg.duration >= markerTime) { +// segment = seg +// segmentIndex = i +// segmentOffsetTime = markerTime - currentTime +// break +// } else { +// currentTime += seg.duration +// } +// } +// lateinit var newSegment: SequenceSegment +// +// when (segment) { +// is WaitSegment -> { +// val newMarkers: MutableList = ArrayList(segment.markers) +// +// newMarkers.addAll(sequenceSegments[segmentIndex].markers) +// newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback)) +// +// val thisSegment = segment +// +// newSegment = +// WaitSegment(thisSegment.startPose, thisSegment.duration, newMarkers) +// } +// +// is TurnSegment -> { +// val newMarkers: MutableList = ArrayList(segment.markers) +// +// newMarkers.addAll(sequenceSegments[segmentIndex].markers) +// newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback)) +// +// val thisSegment = segment +// +// newSegment = TurnSegment( +// thisSegment.startPose, +// thisSegment.totalRotation, +// thisSegment.motionProfile, +// newMarkers +// ) +// } +// +// is TrajectorySegment -> { +// val thisSegment = segment +// +// val newMarkers: MutableList = +// ArrayList(thisSegment.trajectory.markers) +// newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback)) +// +// newSegment = TrajectorySegment( +// Trajectory( +// thisSegment.trajectory.path, +// thisSegment.trajectory.profile, +// newMarkers +// ) +// ) +// } +// } +// +// sequenceSegments[segmentIndex] = newSegment +// } +// return sequenceSegments +// } +// +// // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private +// // note: this assumes that the profile position is monotonic increasing +// private fun motionProfileDisplacementToTime(profile: MotionProfile, s: Double): Double { +// var tLo = 0.0 +// var tHi = profile.duration() +// while (abs(tLo - tHi) >= 1e-6) { +// val tMid = 0.5 * (tLo + tHi) +// if (profile[tMid].x > s) { +// tHi = tMid +// } else { +// tLo = tMid +// } +// } +// return 0.5 * (tLo + tHi) +// } +// +// private fun displacementToTime(sequenceSegments: List, s: Double): Double { +// var currentTime = 0.0 +// var currentDisplacement = 0.0 +// for (segment in sequenceSegments) { +// if (segment is TrajectorySegment) { +// val segmentLength = segment.trajectory.path.length() +// if (currentDisplacement + segmentLength > s) { +// val target = s - currentDisplacement +// val timeInSegment = motionProfileDisplacementToTime( +// segment.trajectory.profile, +// target +// ) +// return currentTime + timeInSegment +// } else { +// currentDisplacement += segmentLength +// currentTime += segment.trajectory.duration() +// } +// } else { +// currentTime += segment.duration +// } +// } +// return 0.0 +// } +// +// private fun pointToTime(sequenceSegments: List, point: Vector2d): Double { +// class ComparingPoints( +// val distanceToPoint: Double, +// val totalDisplacement: Double, +// val thisPathDisplacement: Double, +// ) +// +// val projectedPoints: MutableList = ArrayList() +// for (segment in sequenceSegments) { +// if (segment is TrajectorySegment) { +// val thisSegment = segment +// val displacement = thisSegment.trajectory.path.project(point, 0.25) +// val projectedPoint = thisSegment.trajectory.path[displacement].vec() +// val distanceToPoint = point.minus(projectedPoint).norm() +// var totalDisplacement = 0.0 +// for (comparingPoint in projectedPoints) { +// totalDisplacement += comparingPoint.totalDisplacement +// } +// totalDisplacement += displacement +// projectedPoints.add( +// ComparingPoints( +// distanceToPoint, +// displacement, +// totalDisplacement +// ) +// ) +// } +// } +// var closestPoint: ComparingPoints? = null +// for (comparingPoint in projectedPoints) { +// if (closestPoint == null) { +// closestPoint = comparingPoint +// continue +// } +// if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint) closestPoint = +// comparingPoint +// } +// return displacementToTime(sequenceSegments, closestPoint!!.thisPathDisplacement) +// } +// +// private fun interface AddPathCallback { +// fun run() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt new file mode 100644 index 0000000..eb67981 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt @@ -0,0 +1,219 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence + +//@Config +//class TrajectorySequenceRunner( +// private val follower: TrajectoryFollower, +// headingPIDCoefficients: PIDCoefficients, +//) { +// private val turnController: PIDFController +// private val clock: NanoClock +// private val dashboard: FtcDashboard +// private val poseHistory = LinkedList() +// private var remainingMarkers: MutableList = ArrayList() +// private var currentTrajectorySequence: TrajectorySequence? = null +// private var currentSegmentStartTime = 0.0 +// private var currentSegmentIndex = 0 +// private var lastSegmentIndex = 0 +// var lastPoseError = Pose2d() +// private set +// +// init { +// turnController = PIDFController(headingPIDCoefficients) +// turnController.setInputBounds(0.0, 2 * Math.PI) +// clock = NanoClock.system() +// dashboard = FtcDashboard.getInstance() +// dashboard.telemetryTransmissionInterval = 25 +// } +// +// fun followTrajectorySequenceAsync(trajectorySequence: TrajectorySequence) { +// currentTrajectorySequence = trajectorySequence +// currentSegmentStartTime = clock.seconds() +// currentSegmentIndex = 0 +// lastSegmentIndex = -1 +// } +// +// fun update(poseEstimate: Pose2d, poseVelocity: Pose2d?): DriveSignal? { +// var targetPose: Pose2d? = null +// var driveSignal: DriveSignal? = null +// val packet = TelemetryPacket() +// val fieldOverlay = packet.fieldOverlay() +// var currentSegment: SequenceSegment? = null +// +// if (currentTrajectorySequence != null) { +// if (currentSegmentIndex >= currentTrajectorySequence!!.size()) { +// for (marker in remainingMarkers) { +// marker.callback.onMarkerReached() +// } +// +// remainingMarkers.clear() +// +// currentTrajectorySequence = null +// } +// +// if (currentTrajectorySequence == null) return DriveSignal() +// +// val now = clock.seconds() +// +// val isNewTransition = currentSegmentIndex != lastSegmentIndex +// +// currentSegment = currentTrajectorySequence!![currentSegmentIndex] +// +// if (isNewTransition) { +// currentSegmentStartTime = now +// lastSegmentIndex = currentSegmentIndex +// for (marker in remainingMarkers) { +// marker.callback.onMarkerReached() +// } +// remainingMarkers.clear() +// remainingMarkers.addAll(currentSegment.markers) +// remainingMarkers.sortWith { t1: TrajectoryMarker, t2: TrajectoryMarker -> +// t1.time.compareTo(t2.time) +// } +// } +// val deltaTime = now - currentSegmentStartTime +// +// when (currentSegment) { +// is TrajectorySegment -> { +// val currentTrajectory = currentSegment.trajectory +// if (isNewTransition) follower.followTrajectory(currentTrajectory) +// if (!follower.isFollowing()) { +// currentSegmentIndex++ +// driveSignal = DriveSignal() +// } else { +// driveSignal = follower.update(poseEstimate, poseVelocity) +// lastPoseError = follower.lastError +// } +// targetPose = currentTrajectory[deltaTime] +// } +// +// is TurnSegment -> { +// val targetState = currentSegment.motionProfile[deltaTime] +// +// turnController.targetPosition = targetState.x +// +// val correction = turnController.update(poseEstimate.heading) +// val targetOmega = targetState.v +// val targetAlpha = targetState.a +// +// lastPoseError = Pose2d(0.0, 0.0, turnController.lastError) +// +// val startPose = currentSegment.startPose +// +// targetPose = startPose.copy(startPose.x, startPose.y, targetState.x) +// +// driveSignal = DriveSignal( +// Pose2d(0.0, 0.0, targetOmega + correction), +// Pose2d(0.0, 0.0, targetAlpha) +// ) +// +// if (deltaTime >= currentSegment.duration) { +// currentSegmentIndex++ +// driveSignal = DriveSignal() +// } +// } +// +// is WaitSegment -> { +// lastPoseError = Pose2d() +// targetPose = currentSegment.startPose +// driveSignal = DriveSignal() +// if (deltaTime >= currentSegment.duration) { +// currentSegmentIndex++ +// } +// } +// } +// while (remainingMarkers.size > 0 && deltaTime > remainingMarkers[0].time) { +// remainingMarkers[0].callback.onMarkerReached() +// remainingMarkers.removeAt(0) +// } +// } +// poseHistory.add(poseEstimate) +// if (POSE_HISTORY_LIMIT > -1 && poseHistory.size > POSE_HISTORY_LIMIT) { +// poseHistory.removeFirst() +// } +// packet.put("x", poseEstimate.x) +// packet.put("y", poseEstimate.y) +// packet.put("heading (deg)", Math.toDegrees(poseEstimate.heading)) +// packet.put("xError", lastPoseError.x) +// packet.put("yError", lastPoseError.y) +// packet.put("headingError (deg)", Math.toDegrees(lastPoseError.heading)) +// draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate) +// dashboard.sendTelemetryPacket(packet) +// return driveSignal +// } +// +// private fun draw( +// fieldOverlay: Canvas, +// sequence: TrajectorySequence?, currentSegment: SequenceSegment?, +// targetPose: Pose2d?, poseEstimate: Pose2d, +// ) { +// if (sequence != null) { +// for (i in 0 until sequence.size()) { +// when (val segment = sequence[i]) { +// is TrajectorySegment -> { +// fieldOverlay.setStrokeWidth(1) +// fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY) +// DashboardUtil.drawSampledPath(fieldOverlay, segment.trajectory.path) +// } +// +// is TurnSegment -> { +// val pose = segment.startPose +// fieldOverlay.setFill(COLOR_INACTIVE_TURN) +// fieldOverlay.fillCircle(pose.x, pose.y, 2.0) +// } +// +// is WaitSegment -> { +// val pose = segment.startPose +// fieldOverlay.setStrokeWidth(1) +// fieldOverlay.setStroke(COLOR_INACTIVE_WAIT) +// fieldOverlay.strokeCircle(pose.x, pose.y, 3.0) +// } +// } +// } +// } +// if (currentSegment != null) { +// when (currentSegment) { +// is TrajectorySegment -> { +// val currentTrajectory = currentSegment.trajectory +// fieldOverlay.setStrokeWidth(1) +// fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY) +// DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.path) +// } +// +// is TurnSegment -> { +// val pose = currentSegment.startPose +// fieldOverlay.setFill(COLOR_ACTIVE_TURN) +// fieldOverlay.fillCircle(pose.x, pose.y, 3.0) +// } +// +// is WaitSegment -> { +// val pose = currentSegment.startPose +// fieldOverlay.setStrokeWidth(1) +// fieldOverlay.setStroke(COLOR_ACTIVE_WAIT) +// fieldOverlay.strokeCircle(pose.x, pose.y, 3.0) +// } +// } +// } +// if (targetPose != null) { +// fieldOverlay.setStrokeWidth(1) +// fieldOverlay.setStroke("#4CAF50") +// DashboardUtil.drawRobot(fieldOverlay, targetPose) +// } +// fieldOverlay.setStroke("#3F51B5") +// DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory) +// fieldOverlay.setStroke("#3F51B5") +// DashboardUtil.drawRobot(fieldOverlay, poseEstimate) +// } +// +// val isBusy: Boolean +// get() = currentTrajectorySequence != null +// +// companion object { +// var COLOR_INACTIVE_TRAJECTORY = "#4caf507a" +// var COLOR_INACTIVE_TURN = "#7c4dff7a" +// var COLOR_INACTIVE_WAIT = "#dd2c007a" +// var COLOR_ACTIVE_TRAJECTORY = "#4CAF50" +// var COLOR_ACTIVE_TURN = "#7c4dff" +// var COLOR_ACTIVE_WAIT = "#dd2c00" +// var POSE_HISTORY_LIMIT = -1 +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt new file mode 100644 index 0000000..1530524 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment + +//abstract class SequenceSegment protected constructor( +// val duration: Double, +// val startPose: Pose2d, val endPose: Pose2d, +// val markers: List, +//) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt new file mode 100644 index 0000000..a8ca719 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment + +//class TrajectorySegment // Note: Markers are already stored in the `Trajectory` itself. +// This class should not hold any markers +// (val trajectory: Trajectory) : org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment( +// trajectory.duration(), trajectory.start(), trajectory.end(), emptyList() +//) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt new file mode 100644 index 0000000..18fe75f --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment + +//class TurnSegment( +// startPose: Pose2d, +// val totalRotation: Double, +// val motionProfile: MotionProfile, +// markers: List, +//) : org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment( +// motionProfile.duration(), +// startPose, +// Pose2d( +// startPose.x, startPose.y, +// norm(startPose.heading + totalRotation) +// ), +// markers +//) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt new file mode 100644 index 0000000..c3eccc4 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment + +//class WaitSegment(pose: Pose2d, seconds: Double, markers: List) : +// org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment(seconds, pose, pose, markers) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AssetsTrajectoryManager.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AssetsTrajectoryManager.kt new file mode 100644 index 0000000..e8712ea --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AssetsTrajectoryManager.kt @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Set of utilities for loading trajectories from assets (the plugin save location). + */ +//object AssetsTrajectoryManager { +//// /** +//// * Loads the group config. +//// */ +// fun loadGroupConfig(): TrajectoryGroupConfig? { +// return try { +// val inputStream = AppUtil.getDefContext().assets.open( +// "trajectory/$GROUP_FILENAME" +// ) +// loadGroupConfig(inputStream) +// } catch (e: IOException) { +// null +// } +// } +// +// /** +// * Loads a trajectory config with the given name. +// */ +// fun loadConfig(name: String): TrajectoryConfig? { +// return try { +// val inputStream = AppUtil.getDefContext().assets.open( +// "trajectory/$name.yaml" +// ) +// loadConfig(inputStream) +// } catch (e: IOException) { +// null +// } +// } +// +// /** +// * Loads a trajectory builder with the given name. +// */ +// fun loadBuilder(name: String): TrajectoryBuilder? { +// val groupConfig = loadGroupConfig() +// val config = loadConfig(name) +// return if (groupConfig == null || config == null) { +// null +// } else config.toTrajectoryBuilder( +// groupConfig +// ) +// } +// +// /** +// * Loads a trajectory with the given name. +// */ +// fun load(name: String): Trajectory? { +// val builder = loadBuilder(name) ?: return null +// return builder.build() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AxesSigns.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AxesSigns.kt new file mode 100644 index 0000000..bf09e5e --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/AxesSigns.kt @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * IMU axes signs in the order XYZ (after remapping). + */ +//enum class AxesSigns(val bVal: Int) { +// PPP(0), PPN(1), PNP(2), PNN(3), NPP(4), NPN(5), NNP(6), NNN(7) +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/BNO055IMUUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/BNO055IMUUtil.kt new file mode 100644 index 0000000..4c51240 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/BNO055IMUUtil.kt @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Various utility functions for the BNO055 IMU. + */ +//object BNO055IMUUtil { +// /** +// * Remap BNO055 IMU axes and signs. For reference, the default order is [AxesOrder.ZYX]. +// * Call after [BNO055IMU.initialize]. Although this isn't +// * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion. +// * +// * +// * Adapted from [this post](https://ftcforum.usfirst.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587). +// * +// * @param imu IMU +// * @param order axes order +// * @param signs axes signs +// */ +// fun remapAxes(imu: BNO055IMU, order: AxesOrder, signs: AxesSigns) { +// try { +// // the indices correspond with the 2-bit encodings specified in the datasheet +// val indices = order.indices() +// var axisMapConfig = 0 +// axisMapConfig = axisMapConfig or (indices[0] shl 4) +// axisMapConfig = axisMapConfig or (indices[1] shl 2) +// axisMapConfig = axisMapConfig or (indices[2] shl 0) +// +// // the BNO055 driver flips the first orientation vector so we also flip here +// val axisMapSign = signs.bVal xor (4 shr indices[0]) +// +// // Enter CONFIG mode +// imu.write8( +// BNO055IMU.Register.OPR_MODE, +// BNO055IMU.SensorMode.CONFIG.bVal.toInt() and 0x0F +// ) +// Thread.sleep(100) +// +// // Write the AXIS_MAP_CONFIG register +// imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig and 0x3F) +// +// // Write the AXIS_MAP_SIGN register +// imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSign and 0x07) +// +// // Switch back to the previous mode +// imu.write8(BNO055IMU.Register.OPR_MODE, imu.parameters.mode.bVal.toInt() and 0x0F) +// Thread.sleep(100) +// } catch (e: InterruptedException) { +// Thread.currentThread().interrupt() +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/DashboardUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/DashboardUtil.kt new file mode 100644 index 0000000..7e33008 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/DashboardUtil.kt @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases. + */ +//object DashboardUtil { +// private const val DEFAULT_RESOLUTION = 2.0 // distance units; presumed inches +// private const val ROBOT_RADIUS = 9.0 // in +// fun drawPoseHistory(canvas: Canvas, poseHistory: List) { +// val xPoints = DoubleArray(poseHistory.size) +// val yPoints = DoubleArray(poseHistory.size) +// for (i in poseHistory.indices) { +// val (x, y) = poseHistory[i] +// xPoints[i] = x +// yPoints[i] = y +// } +// canvas.strokePolyline(xPoints, yPoints) +// } +// +// @JvmOverloads +// fun drawSampledPath(canvas: Canvas, path: Path, resolution: Double = DEFAULT_RESOLUTION) { +// val samples = Math.ceil(path.length() / resolution).toInt() +// val xPoints = DoubleArray(samples) +// val yPoints = DoubleArray(samples) +// val dx = path.length() / (samples - 1) +// for (i in 0 until samples) { +// val displacement = i * dx +// val (x, y) = path[displacement] +// xPoints[i] = x +// yPoints[i] = y +// } +// canvas.strokePolyline(xPoints, yPoints) +// } +// +// fun drawRobot(canvas: Canvas, pose: Pose2d) { +// canvas.strokeCircle(pose.x, pose.y, ROBOT_RADIUS) +// val (x, y) = pose.headingVec().times(ROBOT_RADIUS) +// val x1 = pose.x + x / 2 +// val y1 = pose.y + y / 2 +// val x2 = pose.x + x +// val y2 = pose.y + y +// canvas.strokeLine(x1, y1, x2, y2) +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/Encoder.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/Encoder.kt new file mode 100644 index 0000000..8c7ecb0 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/Encoder.kt @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +//class Encoder @JvmOverloads constructor( +// private val motor: DcMotorEx, +// private val clock: NanoClock = NanoClock.system(), +//) { +// /** +// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state +// * +// * @param direction either reverse or forward depending on if encoder counts should be negated +// */ +// var direction: Direction +// private var lastPosition = 0 +// private var velocityEstimate = 0.0 +// private var lastUpdateTime: Double +// +// init { +// direction = Direction.FORWARD +// lastUpdateTime = clock.seconds() +// } +// +// private val multiplier: Int +// get() = direction.multiplier * if (motor.direction == DcMotorSimple.Direction.FORWARD) 1 else -1 +// val currentPosition: Int +// get() { +// val multiplier = multiplier +// val currentPosition = motor.currentPosition * multiplier +// if (currentPosition != lastPosition) { +// val currentTime = clock.seconds() +// val dt = currentTime - lastUpdateTime +// velocityEstimate = (currentPosition - lastPosition) / dt +// lastPosition = currentPosition +// lastUpdateTime = currentTime +// } +// return currentPosition +// } +// private val rawVelocity: Double +// get() { +// val multiplier = multiplier +// return motor.velocity * multiplier +// } +// val correctedVelocity: Double +// get() = inverseOverflow( +// rawVelocity, velocityEstimate +// ) +// +// enum class Direction(val multiplier: Int) { +// FORWARD(1), REVERSE(-1) +// +// } +// +// companion object { +// private const val CPS_STEP = 0x10000 +// private fun inverseOverflow(input: Double, estimate: Double): Double { +// var real = input +// while (abs(estimate - real) > CPS_STEP / 2.0) { +// real += sign(estimate - real) * CPS_STEP +// } +// return real +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LoggingUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LoggingUtil.kt new file mode 100644 index 0000000..8553834 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LoggingUtil.kt @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Utility functions for log files. + */ +//object LoggingUtil { +// private val ROAD_RUNNER_FOLDER = File(AppUtil.ROOT_FOLDER.toString() + "/RoadRunner/") +// private const val LOG_QUOTA = (25 * 1024 * 1024 // 25MB log quota for now +// ).toLong() +// +// private fun buildLogList(logFiles: MutableList, dir: File) { +// for (file in dir.listFiles()!!) { +// if (file.isDirectory) { +// buildLogList(logFiles, file) +// } else { +// logFiles.add(file) +// } +// } +// } +// +// private fun pruneLogsIfNecessary() { +// val logFiles: MutableList = ArrayList() +// buildLogList(logFiles, ROAD_RUNNER_FOLDER) +// logFiles.sortWith { lhs: File, rhs: File -> +// lhs.lastModified().compareTo(rhs.lastModified()) +// } +// var dirSize: Long = 0 +// for (file in logFiles) { +// dirSize += file.length() +// } +// while (dirSize > LOG_QUOTA) { +// if (logFiles.size == 0) break +// val fileToRemove = logFiles.removeAt(0) +// dirSize -= fileToRemove.length() +// fileToRemove.delete() +// } +// } +// +// /** +// * Obtain a log file with the provided name +// */ +// fun getLogFile(name: String): File { +// ROAD_RUNNER_FOLDER.mkdirs() +// pruneLogsIfNecessary() +// return File(ROAD_RUNNER_FOLDER, name) +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LynxModuleUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LynxModuleUtil.kt new file mode 100644 index 0000000..3cf9a1b --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/LynxModuleUtil.kt @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Collection of utilites for interacting with Lynx modules. + */ +//object LynxModuleUtil { +// private val MIN_VERSION = LynxFirmwareVersion(1, 8, 2) +// +// /** +// * Retrieve and parse Lynx module firmware version. +// * +// * @param module Lynx module +// * @return parsed firmware version +// */ +// fun getFirmwareVersion(module: LynxModule): LynxFirmwareVersion? { +// val versionString = module.nullableFirmwareVersionString ?: return null +// val parts = +// versionString.split("[ :,]+".toRegex()).dropLastWhile { it.isEmpty() }.toTypedArray() +// return try { +// // note: for now, we ignore the hardware entry +// LynxFirmwareVersion(parts[3].toInt(), parts[5].toInt(), parts[7].toInt()) +// } catch (e: NumberFormatException) { +// null +// } +// } +// +// /** +// * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement. +// * +// * @param hardwareMap hardware map containing Lynx modules +// */ +// fun ensureMinimumFirmwareVersion(hardwareMap: HardwareMap) { +// val outdatedModules: MutableMap = HashMap() +// for (module in hardwareMap.getAll(LynxModule::class.java)) { +// val version = getFirmwareVersion(module) +// if (version == null || version < MIN_VERSION) { +// for (name in hardwareMap.getNamesOf(module)) { +// outdatedModules[name] = version +// } +// } +// } +// if (outdatedModules.isNotEmpty()) { +// val msgBuilder = StringBuilder() +// msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n") +// msgBuilder.append( +// Misc.formatInvariant( +// "Mandatory minimum firmware version for Road Runner: %s\n", +// MIN_VERSION.toString() +// ) +// ) +// for ((key, value) in outdatedModules) { +// msgBuilder.append( +// Misc.formatInvariant( +// "\t%s: %s\n", key, +// value?.toString() ?: "Unknown" +// ) +// ) +// } +// throw LynxFirmwareVersionException(msgBuilder.toString()) +// } +// } +// +// /** +// * Parsed representation of a Lynx module firmware version. +// */ +// class LynxFirmwareVersion(val major: Int, val minor: Int, val eng: Int) : +// Comparable { +// override fun equals(other: Any?): Boolean { +// return if (other is LynxFirmwareVersion) { +// major == other.major && minor == other.minor && eng == other.eng +// } else { +// false +// } +// } +// +// override fun compareTo(other: LynxFirmwareVersion): Int { +// val majorComp = major.compareTo(other.major) +// return if (majorComp == 0) { +// val minorComp = minor.compareTo(other.minor) +// if (minorComp == 0) { +// eng.compareTo(other.eng) +// } else { +// minorComp +// } +// } else { +// majorComp +// } +// } +// +// override fun toString(): String { +// return Misc.formatInvariant("%d.%d.%d", major, minor, eng) +// } +// } +// +// /** +// * Exception indicating an outdated Lynx firmware version. +// */ +// class LynxFirmwareVersionException(detailMessage: String?) : RuntimeException(detailMessage) +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/RegressionUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/RegressionUtil.kt new file mode 100644 index 0000000..03398b7 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/RegressionUtil.kt @@ -0,0 +1,131 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Various regression utilities. + */ +//object RegressionUtil { +//// /** +//// * Numerically compute dy/dx from the given x and y values. The returned list is padded to match +//// * the length of the original sequences. +//// * +//// * @param x x-values +//// * @param y y-values +//// * @return derivative values +//// */ +// private fun numericalDerivative(x: List, y: List): List { +// val deriv: MutableList = ArrayList(x.size) +// for (i in 1 until x.size - 1) { +// deriv.add( +// (y[i + 1] - y[i - 1]) / +// (x[i + 1] - x[i - 1]) +// ) +// } +// // copy endpoints to pad output +// deriv.add(0, deriv[0]) +// deriv.add(deriv[deriv.size - 1]) +// return deriv +// } +// +// /** +// * Run regression to compute velocity and static feedforward from ramp test data. +// * +// * +// * Here's the general procedure for gathering the requisite data: +// * 1. Slowly ramp the motor power/voltage and record encoder values along the way. +// * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope +// * (kV) and an optional intercept (kStatic). +// * +// * @param timeSamples time samples +// * @param positionSamples position samples +// * @param powerSamples power samples +// * @param fitStatic fit kStatic +// * @param file log file +// */ +// fun fitRampData( +// timeSamples: List, positionSamples: List, +// powerSamples: List, fitStatic: Boolean, +// file: File?, +// ): RampResult { +// if (file != null) { +// try { +// PrintWriter(file).use { pw -> +// pw.println("time,position,power") +// for (i in timeSamples.indices) { +// val time = timeSamples[i] +// val pos = positionSamples[i] +// val power = powerSamples[i] +// pw.println("$time,$pos,$power") +// } +// } +// } catch (e: FileNotFoundException) { +// // ignore +// } +// } +// val velSamples = numericalDerivative(timeSamples, positionSamples) +// val rampReg = SimpleRegression(fitStatic) +// for (i in timeSamples.indices) { +// val vel = velSamples[i] +// val power = powerSamples[i] +// rampReg.addData(vel, power) +// } +// return RampResult( +// abs(rampReg.slope), abs(rampReg.intercept), +// rampReg.rSquare +// ) +// } +// +// /** +// * Run regression to compute acceleration feedforward. +// * +// * @param timeSamples time samples +// * @param positionSamples position samples +// * @param powerSamples power samples +// * @param rampResult ramp result +// * @param file log file +// */ +// fun fitAccelData( +// timeSamples: List, positionSamples: List, +// powerSamples: List, rampResult: RampResult, +// file: File?, +// ): AccelResult { +// if (file != null) { +// try { +// PrintWriter(file).use { pw -> +// pw.println("time,position,power") +// for (i in timeSamples.indices) { +// val time = timeSamples[i] +// val pos = positionSamples[i] +// val power = powerSamples[i] +// pw.println("$time,$pos,$power") +// } +// } +// } catch (e: FileNotFoundException) { +// // ignore +// } +// } +// val velSamples = numericalDerivative(timeSamples, positionSamples) +// val accelSamples = numericalDerivative(timeSamples, velSamples) +// val accelReg = SimpleRegression(false) +// for (i in timeSamples.indices) { +// val vel = velSamples[i] +// val accel = accelSamples[i] +// val power = powerSamples[i] +// val powerFromVel = calculateMotorFeedforward( +// vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic +// ) +// val powerFromAccel = power - powerFromVel +// accelReg.addData(accel, powerFromAccel) +// } +// return AccelResult(Math.abs(accelReg.slope), accelReg.rSquare) +// } +// +// /** +// * Feedforward parameter estimates from the ramp regression and additional summary statistics +// */ +// class RampResult(val kV: Double, val kStatic: Double, val rSquare: Double) +// +// /** +// * Feedforward parameter estimates from the ramp regression and additional summary statistics +// */ +// class AccelResult(val kA: Double, val rSquare: Double) +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/TimedAction.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/TimedAction.kt new file mode 100644 index 0000000..2d8e17f --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/utils/roadrunner/util/TimedAction.kt @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner.utils.roadrunner.util + +/** + * Too many people use sleep(...) where it doesn't need to be. + * This is a simple util that runs an action until a time is reached and then performs a specified stop action. + * This is basically the same as a chain of commands with a deadline WaitCommand but without command-based. + * + * @author Jackson + */ +//class TimedAction @JvmOverloads constructor( +// onRun: Runnable, +// onEnd: Runnable, +// milliseconds: Double, +// symmetric: Boolean = false, +//) { +// private val onRun: Runnable +// private val onEnd: Runnable +// private val waitTime: Double +// private val symmetric: Boolean +// private val timer: ElapsedTime = ElapsedTime() +// private var state = State.IDLE +// /** +// * Sets up a timed action that runs two actions depending on the time +// * and is non-blocking. +// * +// * @param onRun the action to run during the time period +// * @param onEnd the action to run after the time period is up +// * @param milliseconds the wait time +// * @param symmetric true if the timed action runs symmetric +// */ +// /** +// * Sets up an asymmetric timed action. +// * +// * @param onRun the action to run during the time period +// * @param onEnd the action to run after the time period is up +// * @param milliseconds the wait time +// */ +// init { +// this.onRun = onRun +// this.onEnd = onEnd +// waitTime = milliseconds +// this.symmetric = symmetric +// } +// +// /** +// * @return true if the timed action is currently running +// */ +// fun running(): Boolean { +// return state != State.IDLE +// } +// +// /** +// * Resets the timed action to the RUNNING state +// */ +// fun reset() { +// timer.reset() +// state = State.RUNNING +// } +// +// /** +// * Runs the timed action in a non-blocking FSM +// */ +// fun run() { +// when (state) { +// State.IDLE -> {} +// State.RUNNING -> if (timer.milliseconds() <= waitTime) { +// onRun.run() +// } else { +// timer.reset() +// onEnd.run() +// state = if (symmetric) State.SYMMETRIC else State.IDLE +// } +// +// State.SYMMETRIC -> if (timer.milliseconds() <= waitTime) { +// onEnd.run() +// } else { +// timer.reset() +// state = State.IDLE +// } +// } +// } +// +// internal enum class State { +// IDLE, RUNNING, SYMMETRIC +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt deleted file mode 100644 index 8885d3d..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.drive - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.localization.Localizer - -class AprilTagThreeWheelLocalizer( - private val odometry: StandardTrackingWheelLocalizer, -) : Localizer { - private var _poseEstimate: Pose2d = Pose2d() - - override var poseEstimate: Pose2d - get() = _poseEstimate - set(value) { - _poseEstimate = value - } - - override val poseVelocity: Pose2d? = null - - override fun update() { - // Return a kalman filter that takes the pose estimate from the odometry and april tags to combine - odometry.update() - TODO("Not yet implemented") - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt deleted file mode 100644 index b73bef0..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt +++ /dev/null @@ -1,115 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.drive - -import com.acmerobotics.dashboard.config.Config -import com.qualcomm.robotcore.hardware.PIDFCoefficients - -/* -* Constants shared between multiple drive types. -* -* Constants generated by LearnRoadRunner.com/drive-constants -* -* TODO: Tune or adjust the following constants to fit your robot. Note that the non-final -* fields may also be edited through the dashboard (connect to the robot's WiFi network and -* navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you -* adjust them in the dashboard; **config variable changes don't persist between app restarts**. -* -* These are not the only parameters; some are located in the localizer classes, drive base classes, -* and op modes themselves. -*/ -@Config -object DriveConstants { - /* - * These are motor constants that should be listed online for your motors. - */ - const val TICKS_PER_REV = 383.6 - const val MAX_RPM = 435.0 - - /* - * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. - * Set this flag to false if drive encoders are not present and an alternative localization - * method is in use (e.g., tracking wheels). - * - * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients - * from DriveVelocityPIDTuner. - */ - const val RUN_USING_ENCODER = false - var MOTOR_VELO_PID = PIDFCoefficients( - 0.0, 0.0, 0.0, - getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV) - ) - - /* - * These are physical constants that can be determined from your robot (including the track - * width; it will be tune empirically later although a rough estimate is important). Users are - * free to chose whichever linear distance unit they would like so long as it is consistently - * used. The default values were selected with inches in mind. Road runner uses radians for - * angular distances although most angular parameters are wrapped in Math.toRadians() for - * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. - */ - var WHEEL_RADIUS = 1.88976 // in` - var GEAR_RATIO = 1.0 // output (wheel) speed / input (motor) speed - @JvmField - var TRACK_WIDTH = 13.94 // in - - /* - * These are the feedforward parameters used to model the drive motor behavior. If you are using - * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive - * motor encoders or have elected not to use them for velocity control, these values should be - * empirically tuned. - */ - @JvmField - var kV = 0.0144 - @JvmField - var kA = 0.0015 - @JvmField - var kStatic = 0.012 - - /* - * These values are used to generate the trajectories for you robot. To ensure proper operation, - * the constraints should never exceed ~80% of the robot's actual capabilities. While Road - * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start - * small and gradually increase them later after everything is working. All distance units are - * inches. - */ - /* - * Note from LearnRoadRunner.com: - * The velocity and acceleration constraints were calculated based on the following equation: - * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85 - * Resulting in 73.17330064499293 in/s. - * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above. - * This is capped at 85% because there are a number of variables that will preve nt your bot from actually - * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc. - * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically - * max velocity. The theoretically maximum velocity is 86.08623605293286 in/s. - * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally - * affected if it is aiming for a velocity not actually possible. - * - * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on - * actual testing. Just set it at a reasonable value and keep increasing until your path following starts - * to degrade. As of now, it simply mirrors the velocity, resulting in 73.17330064499293 in/s/s - * - * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s. - * You are free to raise this on your own if you would like. It is best determined through experimentation. - - */ - @JvmField - var MAX_VEL = 30.0 - @JvmField - var MAX_ACCEL = 30.0 - @JvmField - var MAX_ANG_VEL = Math.toRadians(170.0) - @JvmField - var MAX_ANG_ACCEL = Math.toRadians(170.0) - fun encoderTicksToInches(ticks: Double): Double { - return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV - } - - fun rpmToVelocity(rpm: Double): Double { - return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0 - } - - fun getMotorVelocityF(ticksPerSecond: Double): Double { - // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx - return 32767 / ticksPerSecond - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt deleted file mode 100644 index 5d4cb68..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt +++ /dev/null @@ -1,89 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.drive - -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.qualcomm.robotcore.hardware.HardwareMap -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.utils.roadrunner.util.Encoder - -/* - * Sample tracking wheel localizer implementation assuming the standard configuration: - - * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | - * | | - * | | - * \--------------/ - * - */ -@Config -class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWheelLocalizer( - listOf( - Pose2d(0.0, LATERAL_DISTANCE / 2, 0.0), // left - Pose2d(0.0, -LATERAL_DISTANCE / 2, 0.0), // right - Pose2d(FORWARD_OFFSET, 0.0, Math.toRadians(90.0)) // front - ) -) { - private val leftEncoder: Encoder - private val rightEncoder: Encoder - private val strafeEncoder: Encoder - - init { - leftEncoder = Encoder( - hardwareMap.get( - DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName - ) - ) - - - rightEncoder = Encoder( - hardwareMap.get( - DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName - ) - ) - strafeEncoder = Encoder( - hardwareMap.get( - DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName - ) - ) -// strafeEncoder.direction = Encoder.Direction.REVERSE - } - - override fun getWheelPositions() = listOf( - encoderTicksToInches(leftEncoder.currentPosition.toDouble()) * X_MULTIPLIER, - encoderTicksToInches(rightEncoder.currentPosition.toDouble()) * X_MULTIPLIER, - encoderTicksToInches(strafeEncoder.currentPosition.toDouble()) * Y_MULTIPLIER - ) - - override fun getWheelVelocities() = listOf( - encoderTicksToInches(leftEncoder.correctedVelocity) * X_MULTIPLIER, - encoderTicksToInches(rightEncoder.correctedVelocity) * X_MULTIPLIER, - encoderTicksToInches(strafeEncoder.correctedVelocity) * Y_MULTIPLIER - ) - - companion object { - val TICKS_PER_REV = 2000.0 - val WHEEL_RADIUS = 0.944882 // in - val GEAR_RATIO = 1.0 // output (wheel) speed / input (encoder) speed - - @JvmField - var LATERAL_DISTANCE = 15.45 // in; distance between the left and right wheels - - @JvmField - var FORWARD_OFFSET = -2.1 // in; offset of the lateral wheel - - @JvmField - var X_MULTIPLIER = 0.9944039016245 // Multiplier in the X direction - @JvmField - var Y_MULTIPLIER = 0.99960646480333// Multiplier in the Y direction - fun encoderTicksToInches(ticks: Double): Double { - return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt deleted file mode 100644 index 9f18020..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt +++ /dev/null @@ -1,3 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence - -class EmptySequenceException : RuntimeException() \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt deleted file mode 100644 index 11a191e..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence - -import com.acmerobotics.roadrunner.geometry.Pose2d -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment -import java.util.Collections - -class TrajectorySequence(sequenceList: List) { - private val sequenceList: List - - init { - if (sequenceList.isEmpty()) throw EmptySequenceException() - this.sequenceList = Collections.unmodifiableList(sequenceList) - } - - fun start(): Pose2d { - return sequenceList[0].startPose - } - - fun end(): Pose2d { - return sequenceList[sequenceList.size - 1].endPose - } - - fun duration(): Double { - var total = 0.0 - for (segment in sequenceList) { - total += segment.duration - } - return total - } - - operator fun get(i: Int): SequenceSegment { - return sequenceList[i] - } - - fun size(): Int { - return sequenceList.size - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt deleted file mode 100644 index 191cd1d..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt +++ /dev/null @@ -1,809 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.geometry.Vector2d -import com.acmerobotics.roadrunner.path.PathContinuityViolationException -import com.acmerobotics.roadrunner.profile.MotionProfile -import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimpleMotionProfile -import com.acmerobotics.roadrunner.profile.MotionState -import com.acmerobotics.roadrunner.trajectory.DisplacementMarker -import com.acmerobotics.roadrunner.trajectory.DisplacementProducer -import com.acmerobotics.roadrunner.trajectory.MarkerCallback -import com.acmerobotics.roadrunner.trajectory.SpatialMarker -import com.acmerobotics.roadrunner.trajectory.TemporalMarker -import com.acmerobotics.roadrunner.trajectory.TimeProducer -import com.acmerobotics.roadrunner.trajectory.Trajectory -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder -import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint -import com.acmerobotics.roadrunner.util.Angle.norm -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceBuilder.AddPathCallback -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TurnSegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.WaitSegment -import kotlin.math.abs - -class TrajectorySequenceBuilder( - startPose: Pose2d, - startTangent: Double?, - private val baseVelConstraint: TrajectoryVelocityConstraint, - private val baseAccelConstraint: TrajectoryAccelerationConstraint, - baseTurnConstraintMaxAngVel: Double, - baseTurnConstraintMaxAngAccel: Double, -) { - private val resolution = 0.25 - private val baseTurnConstraintMaxAngVel: Double - private val baseTurnConstraintMaxAngAccel: Double - private val sequenceSegments: MutableList - private val temporalMarkers: MutableList - private val displacementMarkers: MutableList - private val spatialMarkers: MutableList - private var currentVelConstraint: TrajectoryVelocityConstraint - private var currentAccelConstraint: TrajectoryAccelerationConstraint - private var currentTurnConstraintMaxAngVel: Double - private var currentTurnConstraintMaxAngAccel: Double - private var lastPose: Pose2d - private var tangentOffset: Double - private var setAbsoluteTangent: Boolean - private var absoluteTangent: Double - private var currentTrajectoryBuilder: TrajectoryBuilder? - private var currentDuration: Double - private var currentDisplacement: Double - private var lastDurationTraj: Double - private var lastDisplacementTraj: Double - - init { - currentVelConstraint = baseVelConstraint - currentAccelConstraint = baseAccelConstraint - this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel - this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel - currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel - currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel - sequenceSegments = ArrayList() - temporalMarkers = ArrayList() - displacementMarkers = ArrayList() - spatialMarkers = ArrayList() - lastPose = startPose - tangentOffset = 0.0 - setAbsoluteTangent = startTangent != null - absoluteTangent = startTangent ?: 0.0 - currentTrajectoryBuilder = null - currentDuration = 0.0 - currentDisplacement = 0.0 - lastDurationTraj = 0.0 - lastDisplacementTraj = 0.0 - } - - constructor( - startPose: Pose2d, - baseVelConstraint: TrajectoryVelocityConstraint, - baseAccelConstraint: TrajectoryAccelerationConstraint, - baseTurnConstraintMaxAngVel: Double, - baseTurnConstraintMaxAngAccel: Double, - ) : this( - startPose, null, - baseVelConstraint, baseAccelConstraint, - baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel - ) - - fun lineTo(endPosition: Vector2d): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineTo( - endPosition, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun lineTo( - endPosition: Vector2d, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineTo( - endPosition, velConstraint, accelConstraint - ) - } - } - - fun lineToConstantHeading(endPosition: Vector2d): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineToConstantHeading( - endPosition, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun lineToConstantHeading( - endPosition: Vector2d, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineToConstantHeading( - endPosition, velConstraint, accelConstraint - ) - } - } - - fun lineToLinearHeading(endPose: Pose2d): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineToLinearHeading( - endPose, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun lineToLinearHeading( - endPose: Pose2d, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineToLinearHeading( - endPose, velConstraint, accelConstraint - ) - } - } - - fun lineToSplineHeading(endPose: Pose2d): TrajectorySequenceBuilder { - return addPath(AddPathCallback { - currentTrajectoryBuilder!!.lineToSplineHeading( - endPose, currentVelConstraint, currentAccelConstraint - ) - }) - } - - fun lineToSplineHeading( - endPose: Pose2d, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.lineToSplineHeading( - endPose, velConstraint, accelConstraint - ) - } - } - - fun strafeTo(endPosition: Vector2d): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.strafeTo( - endPosition, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun strafeTo( - endPosition: Vector2d, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.strafeTo( - endPosition, velConstraint, accelConstraint - ) - } - } - - fun forward(distance: Double): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.forward( - distance, - currentVelConstraint, - currentAccelConstraint - ) - } - } - - fun forward( - distance: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.forward( - distance, - velConstraint, - accelConstraint - ) - } - } - - fun back(distance: Double): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.back( - distance, - currentVelConstraint, - currentAccelConstraint - ) - } - } - - fun back( - distance: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath(AddPathCallback { - currentTrajectoryBuilder!!.back( - distance, - velConstraint, - accelConstraint - ) - }) - } - - fun strafeLeft(distance: Double): TrajectorySequenceBuilder { - return addPath(AddPathCallback { - currentTrajectoryBuilder!!.strafeLeft( - distance, - currentVelConstraint, - currentAccelConstraint - ) - }) - } - - fun strafeLeft( - distance: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath(AddPathCallback { - currentTrajectoryBuilder!!.strafeLeft( - distance, - velConstraint, - accelConstraint - ) - }) - } - - fun strafeRight(distance: Double): TrajectorySequenceBuilder { - return addPath(AddPathCallback { - currentTrajectoryBuilder!!.strafeRight( - distance, - currentVelConstraint, - currentAccelConstraint - ) - }) - } - - fun strafeRight( - distance: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath(AddPathCallback { - currentTrajectoryBuilder!!.strafeRight( - distance, - velConstraint, - accelConstraint - ) - }) - } - - fun splineTo(endPosition: Vector2d, endHeading: Double): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineTo( - endPosition, endHeading, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun splineTo( - endPosition: Vector2d, - endHeading: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineTo( - endPosition, endHeading, velConstraint, accelConstraint - ) - } - } - - fun splineToConstantHeading( - endPosition: Vector2d, - endHeading: Double, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineToConstantHeading( - endPosition, endHeading, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun splineToConstantHeading( - endPosition: Vector2d, - endHeading: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineToConstantHeading( - endPosition, endHeading, velConstraint, accelConstraint - ) - } - } - - fun splineToLinearHeading(endPose: Pose2d, endHeading: Double): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineToLinearHeading( - endPose, endHeading, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun splineToLinearHeading( - endPose: Pose2d, - endHeading: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineToLinearHeading( - endPose, endHeading, velConstraint, accelConstraint - ) - } - } - - fun splineToSplineHeading(endPose: Pose2d, endHeading: Double): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineToSplineHeading( - endPose, endHeading, currentVelConstraint, currentAccelConstraint - ) - } - } - - fun splineToSplineHeading( - endPose: Pose2d, - endHeading: Double, - velConstraint: TrajectoryVelocityConstraint?, - accelConstraint: TrajectoryAccelerationConstraint?, - ): TrajectorySequenceBuilder { - return addPath { - currentTrajectoryBuilder!!.splineToSplineHeading( - endPose, endHeading, velConstraint, accelConstraint - ) - } - } - - private fun addPath(callback: AddPathCallback): TrajectorySequenceBuilder { - if (currentTrajectoryBuilder == null) newPath() - try { - callback.run() - } catch (e: PathContinuityViolationException) { - newPath() - callback.run() - } - val builtTraj = currentTrajectoryBuilder!!.build() - val durationDifference = builtTraj.duration() - lastDurationTraj - val displacementDifference = builtTraj.path.length() - lastDisplacementTraj - lastPose = builtTraj.end() - currentDuration += durationDifference - currentDisplacement += displacementDifference - lastDurationTraj = builtTraj.duration() - lastDisplacementTraj = builtTraj.path.length() - return this - } - - fun setTangent(tangent: Double): TrajectorySequenceBuilder { - setAbsoluteTangent = true - absoluteTangent = tangent - pushPath() - return this - } - - private fun setTangentOffset(offset: Double): TrajectorySequenceBuilder { - setAbsoluteTangent = false - tangentOffset = offset - pushPath() - return this - } - - fun setReversed(reversed: Boolean): TrajectorySequenceBuilder { - return if (reversed) setTangentOffset(Math.toRadians(180.0)) else setTangentOffset(0.0) - } - - fun setConstraints( - velConstraint: TrajectoryVelocityConstraint, - accelConstraint: TrajectoryAccelerationConstraint, - ): TrajectorySequenceBuilder { - currentVelConstraint = velConstraint - currentAccelConstraint = accelConstraint - return this - } - - fun resetConstraints(): TrajectorySequenceBuilder { - currentVelConstraint = baseVelConstraint - currentAccelConstraint = baseAccelConstraint - return this - } - - fun setVelConstraint(velConstraint: TrajectoryVelocityConstraint): TrajectorySequenceBuilder { - currentVelConstraint = velConstraint - return this - } - - fun resetVelConstraint(): TrajectorySequenceBuilder { - currentVelConstraint = baseVelConstraint - return this - } - - fun setAccelConstraint(accelConstraint: TrajectoryAccelerationConstraint): TrajectorySequenceBuilder { - currentAccelConstraint = accelConstraint - return this - } - - fun resetAccelConstraint(): TrajectorySequenceBuilder { - currentAccelConstraint = baseAccelConstraint - return this - } - - fun setTurnConstraint(maxAngVel: Double, maxAngAccel: Double): TrajectorySequenceBuilder { - currentTurnConstraintMaxAngVel = maxAngVel - currentTurnConstraintMaxAngAccel = maxAngAccel - return this - } - - fun resetTurnConstraint(): TrajectorySequenceBuilder { - currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel - currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel - return this - } - - fun addTemporalMarker(callback: MarkerCallback?): TrajectorySequenceBuilder { - return this.addTemporalMarker(currentDuration, callback) - } - - fun UNSTABLE_addTemporalMarkerOffset( - offset: Double, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - return this.addTemporalMarker(currentDuration + offset, callback) - } - - fun addTemporalMarker(time: Double, callback: MarkerCallback?): TrajectorySequenceBuilder { - return this.addTemporalMarker(0.0, time, callback) - } - - fun addTemporalMarker( - scale: Double, - offset: Double, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - return this.addTemporalMarker({ time: Double -> scale * time + offset }, callback) - } - - fun addTemporalMarker( - time: TimeProducer?, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - temporalMarkers.add(TemporalMarker(time!!, callback!!)) - return this - } - - fun addSpatialMarker(point: Vector2d?, callback: MarkerCallback?): TrajectorySequenceBuilder { - spatialMarkers.add(SpatialMarker(point!!, callback!!)) - return this - } - - fun addDisplacementMarker(callback: MarkerCallback?): TrajectorySequenceBuilder { - return this.addDisplacementMarker(currentDisplacement, callback) - } - - fun UNSTABLE_addDisplacementMarkerOffset( - offset: Double, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - return this.addDisplacementMarker(currentDisplacement + offset, callback) - } - - fun addDisplacementMarker( - displacement: Double, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - return this.addDisplacementMarker(0.0, displacement, callback) - } - - fun addDisplacementMarker( - scale: Double, - offset: Double, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - return addDisplacementMarker( - { displacement: Double -> scale * displacement + offset }, - callback - ) - } - - fun addDisplacementMarker( - displacement: DisplacementProducer?, - callback: MarkerCallback?, - ): TrajectorySequenceBuilder { - displacementMarkers.add(DisplacementMarker(displacement!!, callback!!)) - return this - } - - @JvmOverloads - fun turn( - angle: Double, - maxAngVel: Double = currentTurnConstraintMaxAngVel, - maxAngAccel: Double = currentTurnConstraintMaxAngAccel, - ): TrajectorySequenceBuilder { - pushPath() - val turnProfile = generateSimpleMotionProfile( - MotionState(lastPose.heading, 0.0, 0.0, 0.0), - MotionState(lastPose.heading + angle, 0.0, 0.0, 0.0), - maxAngVel, - maxAngAccel - ) - sequenceSegments.add(TurnSegment(lastPose, angle, turnProfile, emptyList())) - lastPose = Pose2d( - lastPose.x, lastPose.y, - norm(lastPose.heading + angle) - ) - currentDuration += turnProfile.duration() - return this - } - - fun waitSeconds(seconds: Double): TrajectorySequenceBuilder { - pushPath() - sequenceSegments.add(WaitSegment(lastPose, seconds, emptyList())) - currentDuration += seconds - return this - } - - fun addTrajectory(trajectory: Trajectory): TrajectorySequenceBuilder { - pushPath() - sequenceSegments.add(TrajectorySegment(trajectory)) - return this - } - - private fun pushPath() { - if (currentTrajectoryBuilder != null) { - val builtTraj = currentTrajectoryBuilder!!.build() - sequenceSegments.add(TrajectorySegment(builtTraj)) - } - currentTrajectoryBuilder = null - } - - private fun newPath() { - if (currentTrajectoryBuilder != null) pushPath() - lastDurationTraj = 0.0 - lastDisplacementTraj = 0.0 - val tangent = - if (setAbsoluteTangent) absoluteTangent else norm(lastPose.heading + tangentOffset) - currentTrajectoryBuilder = TrajectoryBuilder( - lastPose, - tangent, - currentVelConstraint, - currentAccelConstraint, - resolution - ) - } - - fun build(): TrajectorySequence { - pushPath() - val globalMarkers = convertMarkersToGlobal( - sequenceSegments, - temporalMarkers, displacementMarkers, spatialMarkers - ) - return TrajectorySequence( - projectGlobalMarkersToLocalSegments( - globalMarkers, - sequenceSegments - ) - ) - } - - private fun convertMarkersToGlobal( - sequenceSegments: List, - temporalMarkers: List, - displacementMarkers: List, - spatialMarkers: List, - ): List { - val trajectoryMarkers = ArrayList() - - // Convert temporal markers - for ((producer, callback) in temporalMarkers) { - trajectoryMarkers.add( - TrajectoryMarker(producer.produce(currentDuration), callback) - ) - } - - // Convert displacement markers - for ((producer, callback) in displacementMarkers) { - val time = displacementToTime( - sequenceSegments, - producer.produce(currentDisplacement) - ) - trajectoryMarkers.add( - TrajectoryMarker( - time, - callback - ) - ) - } - - // Convert spatial markers - for ((point, callback) in spatialMarkers) { - trajectoryMarkers.add( - TrajectoryMarker( - pointToTime(sequenceSegments, point), - callback - ) - ) - } - return trajectoryMarkers - } - - private fun projectGlobalMarkersToLocalSegments( - markers: List, - sequenceSegments: MutableList, - ): List { - if (sequenceSegments.isEmpty()) return emptyList() - - var totalSequenceDuration = 0.0 - for (segment in sequenceSegments) { - totalSequenceDuration += segment.duration - } - - for ((time, callback) in markers) { - lateinit var segment: SequenceSegment - var segmentIndex = 0 - var segmentOffsetTime = 0.0 - var currentTime = 0.0 - - for (i in sequenceSegments.indices) { - val seg = sequenceSegments[i] - val markerTime = Math.min(time, totalSequenceDuration) - if (currentTime + seg.duration >= markerTime) { - segment = seg - segmentIndex = i - segmentOffsetTime = markerTime - currentTime - break - } else { - currentTime += seg.duration - } - } - lateinit var newSegment: SequenceSegment - - when (segment) { - is WaitSegment -> { - val newMarkers: MutableList = ArrayList(segment.markers) - - newMarkers.addAll(sequenceSegments[segmentIndex].markers) - newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback)) - - val thisSegment = segment - - newSegment = - WaitSegment(thisSegment.startPose, thisSegment.duration, newMarkers) - } - - is TurnSegment -> { - val newMarkers: MutableList = ArrayList(segment.markers) - - newMarkers.addAll(sequenceSegments[segmentIndex].markers) - newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback)) - - val thisSegment = segment - - newSegment = TurnSegment( - thisSegment.startPose, - thisSegment.totalRotation, - thisSegment.motionProfile, - newMarkers - ) - } - - is TrajectorySegment -> { - val thisSegment = segment - - val newMarkers: MutableList = - ArrayList(thisSegment.trajectory.markers) - newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback)) - - newSegment = TrajectorySegment( - Trajectory( - thisSegment.trajectory.path, - thisSegment.trajectory.profile, - newMarkers - ) - ) - } - } - - sequenceSegments[segmentIndex] = newSegment - } - return sequenceSegments - } - - // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private - // note: this assumes that the profile position is monotonic increasing - private fun motionProfileDisplacementToTime(profile: MotionProfile, s: Double): Double { - var tLo = 0.0 - var tHi = profile.duration() - while (abs(tLo - tHi) >= 1e-6) { - val tMid = 0.5 * (tLo + tHi) - if (profile[tMid].x > s) { - tHi = tMid - } else { - tLo = tMid - } - } - return 0.5 * (tLo + tHi) - } - - private fun displacementToTime(sequenceSegments: List, s: Double): Double { - var currentTime = 0.0 - var currentDisplacement = 0.0 - for (segment in sequenceSegments) { - if (segment is TrajectorySegment) { - val segmentLength = segment.trajectory.path.length() - if (currentDisplacement + segmentLength > s) { - val target = s - currentDisplacement - val timeInSegment = motionProfileDisplacementToTime( - segment.trajectory.profile, - target - ) - return currentTime + timeInSegment - } else { - currentDisplacement += segmentLength - currentTime += segment.trajectory.duration() - } - } else { - currentTime += segment.duration - } - } - return 0.0 - } - - private fun pointToTime(sequenceSegments: List, point: Vector2d): Double { - class ComparingPoints( - val distanceToPoint: Double, - val totalDisplacement: Double, - val thisPathDisplacement: Double, - ) - - val projectedPoints: MutableList = ArrayList() - for (segment in sequenceSegments) { - if (segment is TrajectorySegment) { - val thisSegment = segment - val displacement = thisSegment.trajectory.path.project(point, 0.25) - val projectedPoint = thisSegment.trajectory.path[displacement].vec() - val distanceToPoint = point.minus(projectedPoint).norm() - var totalDisplacement = 0.0 - for (comparingPoint in projectedPoints) { - totalDisplacement += comparingPoint.totalDisplacement - } - totalDisplacement += displacement - projectedPoints.add( - ComparingPoints( - distanceToPoint, - displacement, - totalDisplacement - ) - ) - } - } - var closestPoint: ComparingPoints? = null - for (comparingPoint in projectedPoints) { - if (closestPoint == null) { - closestPoint = comparingPoint - continue - } - if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint) closestPoint = - comparingPoint - } - return displacementToTime(sequenceSegments, closestPoint!!.thisPathDisplacement) - } - - private fun interface AddPathCallback { - fun run() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt deleted file mode 100644 index 3311474..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt +++ /dev/null @@ -1,237 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.canvas.Canvas -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.TelemetryPacket -import com.acmerobotics.roadrunner.control.PIDCoefficients -import com.acmerobotics.roadrunner.control.PIDFController -import com.acmerobotics.roadrunner.drive.DriveSignal -import com.acmerobotics.roadrunner.followers.TrajectoryFollower -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker -import com.acmerobotics.roadrunner.util.NanoClock -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TurnSegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.WaitSegment -import org.firstinspires.ftc.teamcode.utils.roadrunner.util.DashboardUtil -import java.util.LinkedList - -@Config -class TrajectorySequenceRunner( - private val follower: TrajectoryFollower, - headingPIDCoefficients: PIDCoefficients, -) { - private val turnController: PIDFController - private val clock: NanoClock - private val dashboard: FtcDashboard - private val poseHistory = LinkedList() - private var remainingMarkers: MutableList = ArrayList() - private var currentTrajectorySequence: TrajectorySequence? = null - private var currentSegmentStartTime = 0.0 - private var currentSegmentIndex = 0 - private var lastSegmentIndex = 0 - var lastPoseError = Pose2d() - private set - - init { - turnController = PIDFController(headingPIDCoefficients) - turnController.setInputBounds(0.0, 2 * Math.PI) - clock = NanoClock.system() - dashboard = FtcDashboard.getInstance() - dashboard.telemetryTransmissionInterval = 25 - } - - fun followTrajectorySequenceAsync(trajectorySequence: TrajectorySequence) { - currentTrajectorySequence = trajectorySequence - currentSegmentStartTime = clock.seconds() - currentSegmentIndex = 0 - lastSegmentIndex = -1 - } - - fun update(poseEstimate: Pose2d, poseVelocity: Pose2d?): DriveSignal? { - var targetPose: Pose2d? = null - var driveSignal: DriveSignal? = null - val packet = TelemetryPacket() - val fieldOverlay = packet.fieldOverlay() - var currentSegment: SequenceSegment? = null - - if (currentTrajectorySequence != null) { - if (currentSegmentIndex >= currentTrajectorySequence!!.size()) { - for (marker in remainingMarkers) { - marker.callback.onMarkerReached() - } - - remainingMarkers.clear() - - currentTrajectorySequence = null - } - - if (currentTrajectorySequence == null) return DriveSignal() - - val now = clock.seconds() - - val isNewTransition = currentSegmentIndex != lastSegmentIndex - - currentSegment = currentTrajectorySequence!![currentSegmentIndex] - - if (isNewTransition) { - currentSegmentStartTime = now - lastSegmentIndex = currentSegmentIndex - for (marker in remainingMarkers) { - marker.callback.onMarkerReached() - } - remainingMarkers.clear() - remainingMarkers.addAll(currentSegment.markers) - remainingMarkers.sortWith { t1: TrajectoryMarker, t2: TrajectoryMarker -> - t1.time.compareTo(t2.time) - } - } - val deltaTime = now - currentSegmentStartTime - - when (currentSegment) { - is TrajectorySegment -> { - val currentTrajectory = currentSegment.trajectory - if (isNewTransition) follower.followTrajectory(currentTrajectory) - if (!follower.isFollowing()) { - currentSegmentIndex++ - driveSignal = DriveSignal() - } else { - driveSignal = follower.update(poseEstimate, poseVelocity) - lastPoseError = follower.lastError - } - targetPose = currentTrajectory[deltaTime] - } - - is TurnSegment -> { - val targetState = currentSegment.motionProfile[deltaTime] - - turnController.targetPosition = targetState.x - - val correction = turnController.update(poseEstimate.heading) - val targetOmega = targetState.v - val targetAlpha = targetState.a - - lastPoseError = Pose2d(0.0, 0.0, turnController.lastError) - - val startPose = currentSegment.startPose - - targetPose = startPose.copy(startPose.x, startPose.y, targetState.x) - - driveSignal = DriveSignal( - Pose2d(0.0, 0.0, targetOmega + correction), - Pose2d(0.0, 0.0, targetAlpha) - ) - - if (deltaTime >= currentSegment.duration) { - currentSegmentIndex++ - driveSignal = DriveSignal() - } - } - - is WaitSegment -> { - lastPoseError = Pose2d() - targetPose = currentSegment.startPose - driveSignal = DriveSignal() - if (deltaTime >= currentSegment.duration) { - currentSegmentIndex++ - } - } - } - while (remainingMarkers.size > 0 && deltaTime > remainingMarkers[0].time) { - remainingMarkers[0].callback.onMarkerReached() - remainingMarkers.removeAt(0) - } - } - poseHistory.add(poseEstimate) - if (POSE_HISTORY_LIMIT > -1 && poseHistory.size > POSE_HISTORY_LIMIT) { - poseHistory.removeFirst() - } - packet.put("x", poseEstimate.x) - packet.put("y", poseEstimate.y) - packet.put("heading (deg)", Math.toDegrees(poseEstimate.heading)) - packet.put("xError", lastPoseError.x) - packet.put("yError", lastPoseError.y) - packet.put("headingError (deg)", Math.toDegrees(lastPoseError.heading)) - draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate) - dashboard.sendTelemetryPacket(packet) - return driveSignal - } - - private fun draw( - fieldOverlay: Canvas, - sequence: TrajectorySequence?, currentSegment: SequenceSegment?, - targetPose: Pose2d?, poseEstimate: Pose2d, - ) { - if (sequence != null) { - for (i in 0 until sequence.size()) { - when (val segment = sequence[i]) { - is TrajectorySegment -> { - fieldOverlay.setStrokeWidth(1) - fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY) - DashboardUtil.drawSampledPath(fieldOverlay, segment.trajectory.path) - } - - is TurnSegment -> { - val pose = segment.startPose - fieldOverlay.setFill(COLOR_INACTIVE_TURN) - fieldOverlay.fillCircle(pose.x, pose.y, 2.0) - } - - is WaitSegment -> { - val pose = segment.startPose - fieldOverlay.setStrokeWidth(1) - fieldOverlay.setStroke(COLOR_INACTIVE_WAIT) - fieldOverlay.strokeCircle(pose.x, pose.y, 3.0) - } - } - } - } - if (currentSegment != null) { - when (currentSegment) { - is TrajectorySegment -> { - val currentTrajectory = currentSegment.trajectory - fieldOverlay.setStrokeWidth(1) - fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY) - DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.path) - } - - is TurnSegment -> { - val pose = currentSegment.startPose - fieldOverlay.setFill(COLOR_ACTIVE_TURN) - fieldOverlay.fillCircle(pose.x, pose.y, 3.0) - } - - is WaitSegment -> { - val pose = currentSegment.startPose - fieldOverlay.setStrokeWidth(1) - fieldOverlay.setStroke(COLOR_ACTIVE_WAIT) - fieldOverlay.strokeCircle(pose.x, pose.y, 3.0) - } - } - } - if (targetPose != null) { - fieldOverlay.setStrokeWidth(1) - fieldOverlay.setStroke("#4CAF50") - DashboardUtil.drawRobot(fieldOverlay, targetPose) - } - fieldOverlay.setStroke("#3F51B5") - DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory) - fieldOverlay.setStroke("#3F51B5") - DashboardUtil.drawRobot(fieldOverlay, poseEstimate) - } - - val isBusy: Boolean - get() = currentTrajectorySequence != null - - companion object { - var COLOR_INACTIVE_TRAJECTORY = "#4caf507a" - var COLOR_INACTIVE_TURN = "#7c4dff7a" - var COLOR_INACTIVE_WAIT = "#dd2c007a" - var COLOR_ACTIVE_TRAJECTORY = "#4CAF50" - var COLOR_ACTIVE_TURN = "#7c4dff" - var COLOR_ACTIVE_WAIT = "#dd2c00" - var POSE_HISTORY_LIMIT = -1 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt deleted file mode 100644 index 73f0fcc..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker - -abstract class SequenceSegment protected constructor( - val duration: Double, - val startPose: Pose2d, val endPose: Pose2d, - val markers: List, -) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt deleted file mode 100644 index 50c708f..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt +++ /dev/null @@ -1,9 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment - -import com.acmerobotics.roadrunner.trajectory.Trajectory - -class TrajectorySegment // Note: Markers are already stored in the `Trajectory` itself. -// This class should not hold any markers - (val trajectory: Trajectory) : SequenceSegment( - trajectory.duration(), trajectory.start(), trajectory.end(), emptyList() -) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt deleted file mode 100644 index 94326c0..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.profile.MotionProfile -import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker -import com.acmerobotics.roadrunner.util.Angle.norm - -class TurnSegment( - startPose: Pose2d, - val totalRotation: Double, - val motionProfile: MotionProfile, - markers: List, -) : SequenceSegment( - motionProfile.duration(), - startPose, - Pose2d( - startPose.x, startPose.y, - norm(startPose.heading + totalRotation) - ), - markers -) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt deleted file mode 100644 index 7d5d248..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt +++ /dev/null @@ -1,7 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker - -class WaitSegment(pose: Pose2d, seconds: Double, markers: List) : - SequenceSegment(seconds, pose, pose, markers) \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt deleted file mode 100644 index b5b6a15..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt +++ /dev/null @@ -1,65 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.acmerobotics.roadrunner.trajectory.Trajectory -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder -import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig -import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager.GROUP_FILENAME -import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager.loadConfig -import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager.loadGroupConfig -import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig -import org.firstinspires.ftc.robotcore.internal.system.AppUtil -import java.io.IOException - -/** - * Set of utilities for loading trajectories from assets (the plugin save location). - */ -object AssetsTrajectoryManager { - /** - * Loads the group config. - */ - fun loadGroupConfig(): TrajectoryGroupConfig? { - return try { - val inputStream = AppUtil.getDefContext().assets.open( - "trajectory/$GROUP_FILENAME" - ) - loadGroupConfig(inputStream) - } catch (e: IOException) { - null - } - } - - /** - * Loads a trajectory config with the given name. - */ - fun loadConfig(name: String): TrajectoryConfig? { - return try { - val inputStream = AppUtil.getDefContext().assets.open( - "trajectory/$name.yaml" - ) - loadConfig(inputStream) - } catch (e: IOException) { - null - } - } - - /** - * Loads a trajectory builder with the given name. - */ - fun loadBuilder(name: String): TrajectoryBuilder? { - val groupConfig = loadGroupConfig() - val config = loadConfig(name) - return if (groupConfig == null || config == null) { - null - } else config.toTrajectoryBuilder( - groupConfig - ) - } - - /** - * Loads a trajectory with the given name. - */ - fun load(name: String): Trajectory? { - val builder = loadBuilder(name) ?: return null - return builder.build() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt deleted file mode 100644 index 91d496c..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -/** - * IMU axes signs in the order XYZ (after remapping). - */ -enum class AxesSigns(val bVal: Int) { - PPP(0), PPN(1), PNP(2), PNN(3), NPP(4), NPN(5), NNP(6), NNN(7) -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt deleted file mode 100644 index fbeedd2..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt +++ /dev/null @@ -1,54 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.qualcomm.hardware.bosch.BNO055IMU -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder - -/** - * Various utility functions for the BNO055 IMU. - */ -object BNO055IMUUtil { - /** - * Remap BNO055 IMU axes and signs. For reference, the default order is [AxesOrder.ZYX]. - * Call after [BNO055IMU.initialize]. Although this isn't - * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion. - * - * - * Adapted from [this post](https://ftcforum.usfirst.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587). - * - * @param imu IMU - * @param order axes order - * @param signs axes signs - */ - fun remapAxes(imu: BNO055IMU, order: AxesOrder, signs: AxesSigns) { - try { - // the indices correspond with the 2-bit encodings specified in the datasheet - val indices = order.indices() - var axisMapConfig = 0 - axisMapConfig = axisMapConfig or (indices[0] shl 4) - axisMapConfig = axisMapConfig or (indices[1] shl 2) - axisMapConfig = axisMapConfig or (indices[2] shl 0) - - // the BNO055 driver flips the first orientation vector so we also flip here - val axisMapSign = signs.bVal xor (4 shr indices[0]) - - // Enter CONFIG mode - imu.write8( - BNO055IMU.Register.OPR_MODE, - BNO055IMU.SensorMode.CONFIG.bVal.toInt() and 0x0F - ) - Thread.sleep(100) - - // Write the AXIS_MAP_CONFIG register - imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig and 0x3F) - - // Write the AXIS_MAP_SIGN register - imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSign and 0x07) - - // Switch back to the previous mode - imu.write8(BNO055IMU.Register.OPR_MODE, imu.parameters.mode.bVal.toInt() and 0x0F) - Thread.sleep(100) - } catch (e: InterruptedException) { - Thread.currentThread().interrupt() - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt deleted file mode 100644 index 15f3a7a..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt +++ /dev/null @@ -1,48 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.acmerobotics.dashboard.canvas.Canvas -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.path.Path - -/** - * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases. - */ -object DashboardUtil { - private const val DEFAULT_RESOLUTION = 2.0 // distance units; presumed inches - private const val ROBOT_RADIUS = 9.0 // in - fun drawPoseHistory(canvas: Canvas, poseHistory: List) { - val xPoints = DoubleArray(poseHistory.size) - val yPoints = DoubleArray(poseHistory.size) - for (i in poseHistory.indices) { - val (x, y) = poseHistory[i] - xPoints[i] = x - yPoints[i] = y - } - canvas.strokePolyline(xPoints, yPoints) - } - - @JvmOverloads - fun drawSampledPath(canvas: Canvas, path: Path, resolution: Double = DEFAULT_RESOLUTION) { - val samples = Math.ceil(path.length() / resolution).toInt() - val xPoints = DoubleArray(samples) - val yPoints = DoubleArray(samples) - val dx = path.length() / (samples - 1) - for (i in 0 until samples) { - val displacement = i * dx - val (x, y) = path[displacement] - xPoints[i] = x - yPoints[i] = y - } - canvas.strokePolyline(xPoints, yPoints) - } - - fun drawRobot(canvas: Canvas, pose: Pose2d) { - canvas.strokeCircle(pose.x, pose.y, ROBOT_RADIUS) - val (x, y) = pose.headingVec().times(ROBOT_RADIUS) - val x1 = pose.x + x / 2 - val y1 = pose.y + y / 2 - val x2 = pose.x + x - val y2 = pose.y + y - canvas.strokeLine(x1, y1, x2, y2) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt deleted file mode 100644 index b6e4431..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt +++ /dev/null @@ -1,72 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.acmerobotics.roadrunner.util.NanoClock -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.qualcomm.robotcore.hardware.DcMotorSimple -import kotlin.math.abs -import kotlin.math.sign - -/** - * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding - * slot's motor direction - */ -class Encoder @JvmOverloads constructor( - private val motor: DcMotorEx, - private val clock: NanoClock = NanoClock.system(), -) { - /** - * Allows you to set the direction of the counts and velocity without modifying the motor's direction state - * - * @param direction either reverse or forward depending on if encoder counts should be negated - */ - var direction: Direction - private var lastPosition = 0 - private var velocityEstimate = 0.0 - private var lastUpdateTime: Double - - init { - direction = Direction.FORWARD - lastUpdateTime = clock.seconds() - } - - private val multiplier: Int - get() = direction.multiplier * if (motor.direction == DcMotorSimple.Direction.FORWARD) 1 else -1 - val currentPosition: Int - get() { - val multiplier = multiplier - val currentPosition = motor.currentPosition * multiplier - if (currentPosition != lastPosition) { - val currentTime = clock.seconds() - val dt = currentTime - lastUpdateTime - velocityEstimate = (currentPosition - lastPosition) / dt - lastPosition = currentPosition - lastUpdateTime = currentTime - } - return currentPosition - } - private val rawVelocity: Double - get() { - val multiplier = multiplier - return motor.velocity * multiplier - } - val correctedVelocity: Double - get() = inverseOverflow( - rawVelocity, velocityEstimate - ) - - enum class Direction(val multiplier: Int) { - FORWARD(1), REVERSE(-1) - - } - - companion object { - private const val CPS_STEP = 0x10000 - private fun inverseOverflow(input: Double, estimate: Double): Double { - var real = input - while (abs(estimate - real) > CPS_STEP / 2.0) { - real += sign(estimate - real) * CPS_STEP - } - return real - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt deleted file mode 100644 index e125dd6..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt +++ /dev/null @@ -1,50 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import org.firstinspires.ftc.robotcore.internal.system.AppUtil -import java.io.File - -/** - * Utility functions for log files. - */ -object LoggingUtil { - private val ROAD_RUNNER_FOLDER = File(AppUtil.ROOT_FOLDER.toString() + "/RoadRunner/") - private const val LOG_QUOTA = (25 * 1024 * 1024 // 25MB log quota for now - ).toLong() - - private fun buildLogList(logFiles: MutableList, dir: File) { - for (file in dir.listFiles()!!) { - if (file.isDirectory) { - buildLogList(logFiles, file) - } else { - logFiles.add(file) - } - } - } - - private fun pruneLogsIfNecessary() { - val logFiles: MutableList = ArrayList() - buildLogList(logFiles, ROAD_RUNNER_FOLDER) - logFiles.sortWith { lhs: File, rhs: File -> - lhs.lastModified().compareTo(rhs.lastModified()) - } - var dirSize: Long = 0 - for (file in logFiles) { - dirSize += file.length() - } - while (dirSize > LOG_QUOTA) { - if (logFiles.size == 0) break - val fileToRemove = logFiles.removeAt(0) - dirSize -= fileToRemove.length() - fileToRemove.delete() - } - } - - /** - * Obtain a log file with the provided name - */ - fun getLogFile(name: String): File { - ROAD_RUNNER_FOLDER.mkdirs() - pruneLogsIfNecessary() - return File(ROAD_RUNNER_FOLDER, name) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt deleted file mode 100644 index 4c49c45..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt +++ /dev/null @@ -1,103 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.hardware.HardwareMap -import org.firstinspires.ftc.robotcore.internal.system.Misc - -/** - * Collection of utilites for interacting with Lynx modules. - */ -object LynxModuleUtil { - private val MIN_VERSION = LynxFirmwareVersion(1, 8, 2) - - /** - * Retrieve and parse Lynx module firmware version. - * - * @param module Lynx module - * @return parsed firmware version - */ - fun getFirmwareVersion(module: LynxModule): LynxFirmwareVersion? { - val versionString = module.nullableFirmwareVersionString ?: return null - val parts = - versionString.split("[ :,]+".toRegex()).dropLastWhile { it.isEmpty() }.toTypedArray() - return try { - // note: for now, we ignore the hardware entry - LynxFirmwareVersion(parts[3].toInt(), parts[5].toInt(), parts[7].toInt()) - } catch (e: NumberFormatException) { - null - } - } - - /** - * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement. - * - * @param hardwareMap hardware map containing Lynx modules - */ - fun ensureMinimumFirmwareVersion(hardwareMap: HardwareMap) { - val outdatedModules: MutableMap = HashMap() - for (module in hardwareMap.getAll(LynxModule::class.java)) { - val version = getFirmwareVersion(module) - if (version == null || version < MIN_VERSION) { - for (name in hardwareMap.getNamesOf(module)) { - outdatedModules[name] = version - } - } - } - if (outdatedModules.isNotEmpty()) { - val msgBuilder = StringBuilder() - msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n") - msgBuilder.append( - Misc.formatInvariant( - "Mandatory minimum firmware version for Road Runner: %s\n", - MIN_VERSION.toString() - ) - ) - for ((key, value) in outdatedModules) { - msgBuilder.append( - Misc.formatInvariant( - "\t%s: %s\n", key, - value?.toString() ?: "Unknown" - ) - ) - } - throw LynxFirmwareVersionException(msgBuilder.toString()) - } - } - - /** - * Parsed representation of a Lynx module firmware version. - */ - class LynxFirmwareVersion(val major: Int, val minor: Int, val eng: Int) : - Comparable { - override fun equals(other: Any?): Boolean { - return if (other is LynxFirmwareVersion) { - major == other.major && minor == other.minor && eng == other.eng - } else { - false - } - } - - override fun compareTo(other: LynxFirmwareVersion): Int { - val majorComp = major.compareTo(other.major) - return if (majorComp == 0) { - val minorComp = minor.compareTo(other.minor) - if (minorComp == 0) { - eng.compareTo(other.eng) - } else { - minorComp - } - } else { - majorComp - } - } - - override fun toString(): String { - return Misc.formatInvariant("%d.%d.%d", major, minor, eng) - } - } - - /** - * Exception indicating an outdated Lynx firmware version. - */ - class LynxFirmwareVersionException(detailMessage: String?) : RuntimeException(detailMessage) -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt deleted file mode 100644 index 138ed16..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt +++ /dev/null @@ -1,138 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.acmerobotics.roadrunner.kinematics.Kinematics.calculateMotorFeedforward -import org.apache.commons.math3.stat.regression.SimpleRegression -import java.io.File -import java.io.FileNotFoundException -import java.io.PrintWriter -import kotlin.math.abs - -/** - * Various regression utilities. - */ -object RegressionUtil { - /** - * Numerically compute dy/dx from the given x and y values. The returned list is padded to match - * the length of the original sequences. - * - * @param x x-values - * @param y y-values - * @return derivative values - */ - private fun numericalDerivative(x: List, y: List): List { - val deriv: MutableList = ArrayList(x.size) - for (i in 1 until x.size - 1) { - deriv.add( - (y[i + 1] - y[i - 1]) / - (x[i + 1] - x[i - 1]) - ) - } - // copy endpoints to pad output - deriv.add(0, deriv[0]) - deriv.add(deriv[deriv.size - 1]) - return deriv - } - - /** - * Run regression to compute velocity and static feedforward from ramp test data. - * - * - * Here's the general procedure for gathering the requisite data: - * 1. Slowly ramp the motor power/voltage and record encoder values along the way. - * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope - * (kV) and an optional intercept (kStatic). - * - * @param timeSamples time samples - * @param positionSamples position samples - * @param powerSamples power samples - * @param fitStatic fit kStatic - * @param file log file - */ - fun fitRampData( - timeSamples: List, positionSamples: List, - powerSamples: List, fitStatic: Boolean, - file: File?, - ): RampResult { - if (file != null) { - try { - PrintWriter(file).use { pw -> - pw.println("time,position,power") - for (i in timeSamples.indices) { - val time = timeSamples[i] - val pos = positionSamples[i] - val power = powerSamples[i] - pw.println("$time,$pos,$power") - } - } - } catch (e: FileNotFoundException) { - // ignore - } - } - val velSamples = numericalDerivative(timeSamples, positionSamples) - val rampReg = SimpleRegression(fitStatic) - for (i in timeSamples.indices) { - val vel = velSamples[i] - val power = powerSamples[i] - rampReg.addData(vel, power) - } - return RampResult( - abs(rampReg.slope), abs(rampReg.intercept), - rampReg.rSquare - ) - } - - /** - * Run regression to compute acceleration feedforward. - * - * @param timeSamples time samples - * @param positionSamples position samples - * @param powerSamples power samples - * @param rampResult ramp result - * @param file log file - */ - fun fitAccelData( - timeSamples: List, positionSamples: List, - powerSamples: List, rampResult: RampResult, - file: File?, - ): AccelResult { - if (file != null) { - try { - PrintWriter(file).use { pw -> - pw.println("time,position,power") - for (i in timeSamples.indices) { - val time = timeSamples[i] - val pos = positionSamples[i] - val power = powerSamples[i] - pw.println("$time,$pos,$power") - } - } - } catch (e: FileNotFoundException) { - // ignore - } - } - val velSamples = numericalDerivative(timeSamples, positionSamples) - val accelSamples = numericalDerivative(timeSamples, velSamples) - val accelReg = SimpleRegression(false) - for (i in timeSamples.indices) { - val vel = velSamples[i] - val accel = accelSamples[i] - val power = powerSamples[i] - val powerFromVel = calculateMotorFeedforward( - vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic - ) - val powerFromAccel = power - powerFromVel - accelReg.addData(accel, powerFromAccel) - } - return AccelResult(Math.abs(accelReg.slope), accelReg.rSquare) - } - - /** - * Feedforward parameter estimates from the ramp regression and additional summary statistics - */ - class RampResult(val kV: Double, val kStatic: Double, val rSquare: Double) - - /** - * Feedforward parameter estimates from the ramp regression and additional summary statistics - */ - class AccelResult(val kA: Double, val rSquare: Double) -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt deleted file mode 100644 index 2296629..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt +++ /dev/null @@ -1,88 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.util - -import com.qualcomm.robotcore.util.ElapsedTime - -/** - * Too many people use sleep(...) where it doesn't need to be. - * This is a simple util that runs an action until a time is reached and then performs a specified stop action. - * This is basically the same as a chain of commands with a deadline WaitCommand but without command-based. - * - * @author Jackson - */ -class TimedAction @JvmOverloads constructor( - onRun: Runnable, - onEnd: Runnable, - milliseconds: Double, - symmetric: Boolean = false, -) { - private val onRun: Runnable - private val onEnd: Runnable - private val waitTime: Double - private val symmetric: Boolean - private val timer: ElapsedTime = ElapsedTime() - private var state = State.IDLE - /** - * Sets up a timed action that runs two actions depending on the time - * and is non-blocking. - * - * @param onRun the action to run during the time period - * @param onEnd the action to run after the time period is up - * @param milliseconds the wait time - * @param symmetric true if the timed action runs symmetric - */ - /** - * Sets up an asymmetric timed action. - * - * @param onRun the action to run during the time period - * @param onEnd the action to run after the time period is up - * @param milliseconds the wait time - */ - init { - this.onRun = onRun - this.onEnd = onEnd - waitTime = milliseconds - this.symmetric = symmetric - } - - /** - * @return true if the timed action is currently running - */ - fun running(): Boolean { - return state != State.IDLE - } - - /** - * Resets the timed action to the RUNNING state - */ - fun reset() { - timer.reset() - state = State.RUNNING - } - - /** - * Runs the timed action in a non-blocking FSM - */ - fun run() { - when (state) { - State.IDLE -> {} - State.RUNNING -> if (timer.milliseconds() <= waitTime) { - onRun.run() - } else { - timer.reset() - onEnd.run() - state = if (symmetric) State.SYMMETRIC else State.IDLE - } - - State.SYMMETRIC -> if (timer.milliseconds() <= waitTime) { - onEnd.run() - } else { - timer.reset() - state = State.IDLE - } - } - } - - internal enum class State { - IDLE, RUNNING, SYMMETRIC - } -} \ No newline at end of file