From d6ba3203a1fff886d1bf2406d247ba9a70c75a94 Mon Sep 17 00:00:00 2001 From: Bi1ku <2008owenshi@gmail.com> Date: Sat, 21 Dec 2024 13:58:08 -0500 Subject: [PATCH 001/107] pedro init --- TeamCode/build.gradle | 13 - .../ftc/teamcode/CommandRobot.java | 17 +- .../ftc/teamcode/opmodes/auton/Park.java | 49 - .../opmodes/auton/blue/CloseBasket.java | 6 +- .../opmodes/auton/blue/FarBasket.java | 4 +- .../auton}/positions/blue/close-basket.json | 0 .../auton}/positions/blue/far-basket.json | 0 .../opmodes/auton}/positions/constants.txt | 0 .../auton}/positions/red/close-basket.json | 0 .../auton}/positions/red/far-basket.json | 0 .../opmodes/auton/red/CloseBasket.java | 4 +- .../teamcode/opmodes/auton/red/FarBasket.java | 4 +- .../teamcode/opmodes/test/DrivetrainTest.java | 4 +- .../teamcode/opmodes/test/LimelightTest.java | 6 - .../ftc/teamcode/pedroPathing/LOCALIZATION.md | 278 +++++ .../ftc/teamcode/pedroPathing/OVERVIEW.md | 109 ++ .../ftc/teamcode/pedroPathing/README.md | 28 + .../ftc/teamcode/pedroPathing/TUNING.md | 130 +++ .../examples/TeleOpEnhancements.java | 62 + .../follower/DriveVectorScaler.java | 172 +++ .../pedroPathing/follower/Follower.java | 1010 +++++++++++++++++ .../pedroPathing/localization/Encoder.java | 83 ++ .../localization/GoBildaPinpointDriver.java | 520 +++++++++ .../pedroPathing/localization/Localizer.java | 105 ++ .../pedroPathing/localization/Matrix.java | 273 +++++ .../pedroPathing/localization/Pose.java | 219 ++++ .../localization/PoseUpdater.java | 357 ++++++ .../localization/SparkFunOTOSCorrected.kt | 46 + .../localizers/DriveEncoderLocalizer.java | 272 +++++ .../localizers/OTOSLocalizer.java | 227 ++++ .../localizers/PinpointLocalizer.java | 220 ++++ .../RRToPedroThreeWheelLocalizer.java | 159 +++ .../localizers/RoadRunnerEncoder.java | 132 +++ .../RoadRunnerThreeWheelLocalizer.java | 123 ++ .../localizers/ThreeWheelIMULocalizer.java | 326 ++++++ .../localizers/ThreeWheelLocalizer.java | 292 +++++ .../localizers/TwoWheelLocalizer.java | 309 +++++ .../TwoWheelPinpointIMULocalizer.java | 321 ++++++ .../localization/tuning/ForwardTuner.java | 71 ++ .../localization/tuning/LateralTuner.java | 72 ++ .../localization/tuning/LocalizationTest.java | 126 ++ .../tuning}/SensorGoBildaPinpointExample.java | 64 +- .../localization/tuning/TurnTuner.java | 71 ++ .../pathGeneration/BezierCurve.java | 348 ++++++ .../BezierCurveCoefficients.java | 66 ++ .../pathGeneration/BezierLine.java | 209 ++++ .../pathGeneration/BezierPoint.java | 209 ++++ .../pathGeneration/MathFunctions.java | 323 ++++++ .../pedroPathing/pathGeneration/Path.java | 486 ++++++++ .../pathGeneration/PathBuilder.java | 249 ++++ .../pathGeneration/PathCallback.java | 78 ++ .../pathGeneration/PathChain.java | 94 ++ .../pedroPathing/pathGeneration/Point.java | 203 ++++ .../pedroPathing/pathGeneration/Vector.java | 141 +++ .../teamcode/pedroPathing/tuning/Circle.java | 74 ++ .../tuning/CurvedBackAndForth.java | 85 ++ .../tuning/FollowerConstants.java | 217 ++++ .../tuning/ForwardVelocityTuner.java | 166 +++ .../ForwardZeroPowerAccelerationTuner.java | 165 +++ .../LateralZeroPowerAccelerationTuner.java | 165 +++ .../tuning/StrafeVelocityTuner.java | 164 +++ .../tuning/StraightBackAndForth.java | 84 ++ .../util/CustomFilteredPIDFCoefficients.java | 70 ++ .../util/CustomPIDFCoefficients.java | 67 ++ .../util/DashboardPoseTracker.java | 76 ++ .../teamcode/pedroPathing/util/Drawing.java | 155 +++ .../util/FeedForwardConstant.java | 21 + .../util/FilteredPIDFController.java | 245 ++++ .../pedroPathing/util/KalmanFilter.java | 62 + .../util/KalmanFilterParameters.java | 25 + .../teamcode/pedroPathing/util/NanoTimer.java | 46 + .../pedroPathing/util/PIDFController.java | 223 ++++ .../pedroPathing/util/SingleRunAction.java | 60 + .../ftc/teamcode/pedroPathing/util/Timer.java | 46 + .../ftc/teamcode/roadrunner/Drawing.java | 23 - .../ftc/teamcode/roadrunner/Localizer.java | 8 - .../ftc/teamcode/roadrunner/MecanumDrive.java | 508 --------- .../teamcode/roadrunner/PinpointDrive.java | 165 --- .../roadrunner/SparkFunOTOSDrive.java | 148 --- .../ftc/teamcode/roadrunner/TankDrive.java | 497 -------- .../roadrunner/ThreeDeadWheelLocalizer.java | 99 -- .../roadrunner/TwoDeadWheelLocalizer.java | 129 --- .../messages/DriveCommandMessage.java | 24 - .../messages/MecanumCommandMessage.java | 19 - .../MecanumLocalizerInputsMessage.java | 30 - .../roadrunner/messages/PoseMessage.java | 18 - .../messages/TankCommandMessage.java | 15 - .../messages/TankLocalizerInputsMessage.java | 17 - .../messages/ThreeDeadWheelInputsMessage.java | 17 - .../messages/TwoDeadWheelInputsMessage.java | 35 - .../roadrunner/tuning/LocalizationTest.java | 45 - .../tuning/ManualFeedbackTuner.java | 89 -- .../roadrunner/tuning/SplineTest.java | 61 - .../roadrunner/tuning/TuningOpModes.java | 254 ----- .../tuning/otos/OTOSAngularScalar.java | 34 - .../tuning/otos/OTOSHeadingOffsetTuner.java | 30 - .../tuning/otos/OTOSPositionOffsetTuner.java | 35 - .../ftc/teamcode/subsystems/Drivetrain.java | 29 +- .../utils/commands/CommandAction.java | 34 - .../utils/commands/WaitUntilPosition.java | 21 - .../teamcode/utils/hardware/EnhancedIMU.java | 5 - build.dependencies.gradle | 2 + settings.gradle | 1 - 103 files changed, 10805 insertions(+), 2493 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton}/positions/blue/close-basket.json (100%) rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton}/positions/blue/far-basket.json (100%) rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton}/positions/constants.txt (100%) rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton}/positions/red/close-basket.json (100%) rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton}/positions/red/far-basket.json (100%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{roadrunner => pedroPathing/localization/tuning}/SensorGoBildaPinpointExample.java (78%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/PinpointDrive.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TwoDeadWheelLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSAngularScalar.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSHeadingOffsetTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSPositionOffsetTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/CommandAction.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/WaitUntilPosition.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 511928c..759edbd 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -33,25 +33,12 @@ android { repositories { maven { url = 'https://jitpack.io' } - maven { url = 'https://maven.brott.dev/' } - maven { - url = 'https://repo.dairy.foundation/releases' - } } dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - // IMPORT PATHING VARS - implementation project(path: ':PathVisualizer') - // FTCLIB implementation 'org.ftclib.ftclib:core:2.0.1' - - // ROADRUNNER V1 - implementation "page.j5155.roadrunner:ftc-otos:0.1.2+0.1.14" - implementation "com.acmerobotics.roadrunner:core:1.0.0" - implementation "com.acmerobotics.roadrunner:actions:1.0.0" - implementation "com.acmerobotics.dashboard:dashboard:0.4.16" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java index fa40095..5f6fa9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java @@ -2,7 +2,6 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.command.Command; import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.arcrobotics.ftclib.command.WaitCommand; @@ -26,7 +25,8 @@ import org.firstinspires.ftc.teamcode.commands.lift.LiftIncrement; import org.firstinspires.ftc.teamcode.commands.lift.LiftLowBasket; import org.firstinspires.ftc.teamcode.commands.lift.LiftLowRung; -import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.subsystems.Claw; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Extendo; @@ -54,12 +54,11 @@ public class CommandRobot { public static int CLAW_ACCEPT_DELAY = 450; public static int CLAW_RETRACT_DELAY = 450; - public static int LIFT_DELAY = 250; public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, OpModeCore opMode, TeleOpMode mode) { this.telemetry = telemetry; - this.drivetrain = new Drivetrain(hwMap, telemetry, new Pose2d(0, 0, 0)); + this.drivetrain = new Drivetrain(hwMap, telemetry, new Pose(0, 0, 0)); this.lift = new Lift(hwMap, telemetry); this.extendo = new Extendo(hwMap, telemetry); this.claw = new Claw(hwMap, telemetry); @@ -74,7 +73,7 @@ public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad game this.configureControls(); } - public CommandRobot(HardwareMap hwMap, Pose2d startPose, MultipleTelemetry telemetry, OpModeCore opMode) { + public CommandRobot(HardwareMap hwMap, Pose startPose, MultipleTelemetry telemetry, OpModeCore opMode) { this.telemetry = telemetry; this.lift = new Lift(hwMap, telemetry); @@ -86,10 +85,6 @@ public CommandRobot(HardwareMap hwMap, Pose2d startPose, MultipleTelemetry telem this.configureCommands(); } - public MecanumDrive getDrive() { - return this.drivetrain.getDrive(); - } - public void startThreads() { if (this.mode == TeleOpMode.OWEN || this.mode == TeleOpMode.RYAN) this.drivetrain.startThread(this.gamepad1, this.opMode); @@ -239,4 +234,8 @@ public enum TeleOpMode { RYAN, RYAN_KELLY } + + public Follower follower() { + return this.drivetrain.getFollower(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java deleted file mode 100644 index b1ec579..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes.auton; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; -import com.arcrobotics.ftclib.command.CommandScheduler; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; -import org.firstinspires.ftc.teamcode.subsystems.Claw; -import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; - -@Config -@Autonomous(name = "Park", preselectTeleOp = "Main") -public class Park extends OpModeCore { - private MecanumDrive drive; - private ElapsedTime timer; - private Claw claw; - - public static int DURATION = 3000; - - @Override - public void initialize() { - this.drive = new MecanumDrive(super.hardwareMap, new Pose2d(0, 0, 0)); - this.timer = new ElapsedTime(); - - this.claw = new Claw(super.hardwareMap, super.multipleTelemetry); - } - - @Override - public void runOpMode() { - CommandScheduler.getInstance().enable(); - this.initialize(); - super.waitForStart(); - - this.timer.reset(); - while (super.opModeIsActive()) { - CommandScheduler.getInstance().run(); - - if (this.timer.milliseconds() <= Park.DURATION) { - this.drive.setPowers(-0.25, -0.25, -0.25, -0.25); - } else { - this.drive.setPowers(0, 0, 0, 0); - } - } - - super.end(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java index 53db62d..8e2a373 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java @@ -1,20 +1,22 @@ package org.firstinspires.ftc.teamcode.opmodes.auton.blue; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; @Autonomous(name = "Blue Close Basket", preselectTeleOp = "Main") public class CloseBasket extends OpModeCore { + private Follower follower; private CommandRobot robot; @Override public void initialize() { // TODO: Input correct starting position - this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this); + this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java index 979975f..cc72f8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.opmodes.auton.blue; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; @Autonomous(name = "Blue Far Bakset", preselectTeleOp = "Main") @@ -14,7 +14,7 @@ public class FarBasket extends OpModeCore { @Override public void initialize() { // TODO: Input correct starting position - this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this); + this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); } @Override diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/close-basket.json similarity index 100% rename from PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/close-basket.json diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/far-basket.json similarity index 100% rename from PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/far-basket.json diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/constants.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/constants.txt similarity index 100% rename from PathVisualizer/src/main/java/com/example/meepmeep/positions/constants.txt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/constants.txt diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/close-basket.json similarity index 100% rename from PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/close-basket.json diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/far-basket.json similarity index 100% rename from PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/far-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java index b21976b..efb0dab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.opmodes.auton.red; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; @Autonomous(name = "Red Close Basket", preselectTeleOp = "Main") @@ -14,7 +14,7 @@ public class CloseBasket extends OpModeCore { @Override public void initialize() { // TODO: Input correct starting position - this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this); + this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java index b04b1cc..2f531c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.opmodes.auton.red; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; @Autonomous(name = "Red Far Basket", preselectTeleOp = "Main") @@ -14,7 +14,7 @@ public class FarBasket extends OpModeCore { @Override public void initialize() { // TODO: Input correct starting position - this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this); + this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java index 48b0b58..09147da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.opmodes.test; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.command.CommandScheduler; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; @@ -16,7 +16,7 @@ public class DrivetrainTest extends OpModeCore { @Override public void initialize() { this.gamepad = new GamepadEx(super.gamepad1); - this.drive = new Drivetrain(super.hardwareMap, super.multipleTelemetry, new Pose2d(0, 0, 0)); + this.drive = new Drivetrain(super.hardwareMap, super.multipleTelemetry, new Pose(0, 0, 0)); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java index 4b5c498..27769d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java @@ -1,20 +1,14 @@ package org.firstinspires.ftc.teamcode.opmodes.test; -import static org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive.PARAMS; import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.IMU; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; -import org.firstinspires.ftc.teamcode.utils.hardware.EnhancedIMU; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md new file mode 100644 index 0000000..1d431e3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md @@ -0,0 +1,278 @@ +## Overview +This is the localization system developed for the Pedro Pathing path follower. These localizers use +the pose exponential method of localization. It's basically a way of turning movements from the +robot's coordinate frame to the global coordinate frame. If you're interested in reading more about +it, then check out pages 177 - 183 of [Controls Engineering in the FIRST Robotics Competition](https://file.tavsys.net/control/controls-engineering-in-frc.pdf) +by Tyler Veness. However, the OTOS localizer uses its own onboard system for calculating localization, +which we do not know about. + +## Setting Your Localizer +Go to line `73` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` +with the localizer that applies to you: +* If you're using drive encoders, put `new DriveEncoderLocalizer(hardwareMap)` +* If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)` +* If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically + don't change it from the default +* If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)` +* If you're using OTOS, put `new OTOSLocalizer(hardwareMap)` +* If you're using Pinpoint, put `new PinpointLocalizer(hardwareMap)` + +## Tuning +To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive +encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, the three +wheel with IMU localizer, and the OTOS localizer offered in Pedro Pathing. Scroll down to the section +that applies to you and follow the directions there. + +# Drive Encoders +* First, you'll need all of your drive motors to have encoders attached. +* Then, go to `DriveEncoderLocalizer.java`. The motor names are already set, so you don't have to do + anything to change the encoder names there. +* Then, reverse the direction of any encoders so that all encoders tick up when the robot is moving forward. +* Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into + inches or radians, essentially scaling your localizer so that your numbers are accurate to the real + world. +* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +# Two Wheel Localizer +* First, you'll need a Control Hub with a working IMU, and two odometry wheels connected to motor + encoder ports on a hub. +* Then, go to `TwoWheelLocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that + of your robot's. +* Then, reverse the direction of any encoders so that the forward encoder ticks up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into + inches or radians, essentially scaling your localizer so that your numbers are accurate to the real + world. +* You actually won't need the turning tuner for this one, since the IMU in the Control Hub will take + care of the heading readings. +* First, start with the `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +# Three Wheel Localizer +* First, you'll need three odometry wheels connected to motor encoder ports on a hub. +* Then, go to `ThreeWheelLocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* Then, reverse the direction of any encoders so that the forward encoders tick up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +# Three Wheel Localizer with IMU +* First, you'll need three odometry wheels connected to motor encoder ports on a hub. +* Then, go to `ThreeWheelIMULocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that + of your robot's. +* Then, reverse the direction of any encoders so that the forward encoders tick up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* Although heading localization is done mostly through the IMU, the tracking wheels are still used for + small angle adjustments for better stability. So, you will still need to tune your turning multiplier. +* First, start with the `Turn Localizer Tuner`. Before doing any tuning, go to FTC Dashboard and find + the `ThreeWheelIMULocalizer` dropdown and deselect `useIMU`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. Make sure that `useIMU` is turned back on. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +# OTOS Localizer +* First, you'll need the OTOS connected to an I2C port on a hub. Make sure the film on the sensor is removed. +* Then, go to `OTOSLocalizer.java`. First, in the constructor, go to where it tells you to replace + the current statement with your OTOS port in the constructor. Replace the `deviceName` parameter + with the name of the port that the OTOS is connected to. +* Next, enter in the position of your OTOS relative to the center of the wheels of the robot. The + positions are in inches, so convert measurements accordingly. Use the comment above the class + declaration as well as to help you with the coordinates. +* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the angular scalar will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the scalar you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this scalar, then replace the angular scalar on line `78` in the localizer with your scalar. + Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current angular scalar. +* For this next step, since OTOS only has one linear scalar, you can run either the forward or lateral + localizer tuner and the result should be the same. So, you choose which one you want to run. +* Option 1: go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the linear scalar will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the scalar you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this scalar, then + replace the linear scalar on line `77` in the localizer with your scalar. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current scalar. +* Option 2: go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the linear scalar will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the scalar you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this scalar, then + replace the linear scalar on line `77` in the localizer with your scalar. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current scalar. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +## Pinpoint Localizer +* First you will need to plug in the pinpoint to the i2c port. Ensure that the dead wheel encoder wires are + plugged into the proper ports on the pinpoint to ensure no error within the tuning steps. +* Then, go to the `PinpointLocalier.java` file and go to where it tells you to replace + the current statement with your pinpoint port in the constructor. Replace the `deviceName` parameter + with the name of the port that the pinpoint is connected to. +* Next, follow the instructions left by the TODO: comment and enter in the odometry measurements either in + mms or inches (We have the conversion rates listed). +* First, to ensure that your pinpoint is properly connected, please run the `SensorGoBildaPinpointExample.java` + file left in the `tuning` folder located within `localization`. +* Once completed, the localizer should be properly tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +## Using Road Runner's Localizer +Of course, many teams have experience using Road Runner in the past and so have localizers from Road +Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro +Pathing localization system in Pedro Pathing, but it is commented out by default to reduce the number +of imports in gradle. + +To re-enable it, go to `RoadRunnerEncoder.java`, `RoadRunnerThreeWheelLocalizer.java`, and `RRToPedroThreeWheelLocalizer.java` +and hit `ctrl` + `a` to select everything within the files. Then, press `ctrl` + `/` to uncomment the code. + +Afterwards, go to `build.gradle` file under the `teamcode` folder and add the following dependencies: +``` +implementation 'org.apache.commons:commons-math3:3.6.1' +implementation 'com.acmerobotics.com.roadrunner:core:0.5.6' +``` + +After that, you should be good to go. If you want to use a different localizer from Road Runner, then +you can adapt it in the same process that's used for the Road Runner three wheel localizer. \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md new file mode 100644 index 0000000..ac88be4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md @@ -0,0 +1,109 @@ +## Basic Ideas +Pedro Pathing is a reactive vector based follower. What this means is that the robot dynamically +calculates a set of vectors that are required to correct error as well as to move forward and applies them. + +The robot calculates: + +* centripetal force correction +* translational correction +* heading correction +* drive vector + +These are then applied to the robot in this order until either the robot's power is maxed out or all +the vectors are applied. + +## Why Pedro Pathing? +Why use Pedro Pathing? Why not something else like Road Runner or Pure Pursuit? + +* Why not Pure Pursuit? + * Pure Pursuit searches for the farthest point on the path that's within a certain radius from the robot. Pure Pursuit will then go in a straight line to that point. This poses several problems, as a small search radius will cause some oscillations on corners, and a large search radius will cut corners on paths, which makes the paths inaccurate to real life. + * Pedro Pathing instead corrects to the closest point on the path while still following the path. This ensures that the follower will stay on the path while still being able to move forward along the path without cutting corners or encountering oscillation issues. +* Why not Road Runner? + * Road Runner is a motion profile based follower, which means that a set of instructions for motor powers are calculated for each path beforehand and then run. During this motion profile, Road Runner can struggle to correct. This can be sufficient for many situations, but if the robot encounters an obstacle or wheel slippage, it may be unable to correct in time. + * Pedro Pathing instead dynamically corrects throughout the path. The movement vectors are calculated at every point along the path, and because of this, the path can even be changed midway through and Pedro Pathing will still be able to correct. Since correction occurs throughout the path, the error correction isn't concentrated on the end of the path and therefore the robot is able to better minimize error. + +## How Does Pedro Path? +As mentioned in the *Basic Ideas* section, Pedro Pathing calculates a set of vectors to move the +robot along a path, which is defined with Bezier curves. Here, we'll go more in-depth on how these +vectors are calculated and used in path following. + +### The Hierarchy +While following paths, sometimes all these motion vectors are demanding more power from the robot +than it actually has. How do we deal with this? + +Our motion vectors are applied in order of importance, which is the order they appear in within the +list in *Basic Ideas*. The centripetal force vector is the highest importance, since it ensures the +robot sticks to the path. If the robot is far off the path, then the robot will not drive along the +path, and so the centripetal force vector will be reduced in magnitude. This is why ranking the +centripetal force correction above the translational correction doesn't produce issues. The next +highest, of course, is the translational correction. This corrects strictly the robot's position to +the closest point on the path. The reasoning behind this is that it is usually much more important +that the robot be on the path, and so avoid potential obstacles, rather than facing the correct +direction. The third highest important vector is the heading correction, which turns the robot to +the correct angle. This is higher than the drive vector since we don't want to drive forward if the +robot isn't facing the correct direction. Finally, the drive vector is applied. This ensures that +the robot only continues on the path when there aren't any major issues with the following. + +As each vector is applied, the robot checks to see if the sum of the applied vectors is greater than +the power that the drivetrain can produce, which would be 1 motor power. If the magnitude of the +combined vectors is greater than 1, then the most recently added vector is scaled down until the +combined vectors' magnitude is equal to 1. If all vectors are able to be applied without exceeding +the power limit, then all the vectors can just be applied without issue. + +### Centripetal Force Correction +Have you ever noticed that your robot seems to want to swing outwards when taking corners? This is +due to a lack of centripetal force correction. In order to take curves effectively, your robot must +accelerate towards the inside of the curve. If we can approximate the region of the path the robot +is at with a circle, then we can use the formula for centripetal force to calculate how much power +we need to allocate to approximate a centripetal force. + +Because paths are defined with Bezier curves, we can easily take the first and second derivative of +the path, expressed as vectors. We can use that to calculate the curvature of the path, which is the +inverse of the length of the radius of the circle we can use to approximate the path. The actual +formula for the calculations of the curvature is the cross product of the first derivative and +second derivative, divided by the magnitude of the first derivative raised to the power of 3. + +With this, along with the weight of the robot and the velocity of the robot along the path, we can +calculate the force necessary to keep the robot on the path, and then tune a scaling factor to turn +that force into a corresponding power for the robot. + +### Translational Correction +This is as simple as it sounds: this corrects error in the robot's position only. The robot's translational +error is corrected with a PID control. The translational correction does not act along the path the +robot takes, but instead moves the robot back to the closest point on the path. + +### Heading Correction +The heading correction operates very similarly to the translational correction, except this corrects +the direction the robot is facing. The heading correction will turn in the closest direction from the +robot's current heading to the target heading. + +### Drive Vector +The drive vector points in the direction of the tangent of the path and it is responsible for moving +the robot along the path. Using basic kinematics equations, we can use the velocity of the robot +along the path, the length of path left, and a specified target rate of deceleration to calculate +the velocity we should be moving at. Additionally, after finding out the rate of deceleration of the +robot under 0 power, we can compensate for that with another kinematics equation. Combining these +two lets us control our velocity to both move along the path quickly and brake without overshooting. + +## Additional Capabilities +In addition to following paths, Pedro Pathing can be used for a few other purposes. + +### Holding Points +Pedro Pathing is also capable of holding a specified position and direction. This can be useful for +improving on the end accuracy of paths, providing additional correction time if possible. It can +also be useful in cases where contact with other robots might occur. For instance, in the 2022-2023 +FTC season challenge, Power Play, robots might come into contact when scoring on a contested middle +junction. Pedro Pathing would be able to recover and correct from a robot collision, allowing for +more consistent scoring. + +### TeleOp Enhancements +Finally, Pedro Pathing can be used in TeleOp to enhance driving. With regular mecanum drive, robots +will tend to swing out when taking corners. Pedro Pathing can account for that, allowing the robot +to take corners more smoothly and efficiently. Using the same localizer as is used in autonomous, a +first and second derivative can be estimated from previous positions. Then, with a modified version +of the curvature formula, we can estimate a centripetal force correction and apply it under driver +control. + +## Questions? +If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com` or +within our discord linked here(https://discord.gg/2GfC4qBP5s) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md new file mode 100644 index 0000000..0b23ad5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -0,0 +1,28 @@ +## Welcome! +This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots with Logan +Nash in the 2023-2024 Centerstage season. + +## Installation +The quickest way to get started is with the quickstart [here](https://github.com/AnyiLin/Pedro-Pathing-Quickstart). + +Otherwise, take the `pedroPathing` folder and put it under the `teamcode` folder in your project. +You can do this from either downloading the project from the above quickstart link or the 10158 +CENTERSTAGE repository [here](https://github.com/AnyiLin/10158-Centerstage). + +For this version of Pedro Pathing, the localizer used is the Road Runner localizer. To install its +dependencies: +1. Find `build.dependencies.gradle` in the main folder of your project. +2. Add the following code to the end of the `repositories` block: +``` +maven { url = 'https://maven.brott.dev/' } +``` +3. Then, add the following code to the end of your `dependencies` block: +``` +implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' +``` +4. Find the `build.gradle` file under the `teamcode` folder. +5. In this gradle file, add the following dependency: +``` +implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' +implementation 'org.jetbrains.kotlin:kotlin-stdlib:1.4.21' +``` \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md new file mode 100644 index 0000000..cf39512 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -0,0 +1,130 @@ +## Prerequisites +Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work +with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. +You must also have a localizer of some sort. Pedro Pathing has a drive encoder, a two tracking wheel, +and a three tracking wheel localizer. You will need to have your localizer tuned before starting to +tune PedroPathing. Check out the tuning guide under the localization tab if you're planning on using one of the +localizers available in Pedro Pathing. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) +will help a lot in tuning. Team 16166 Watt'S Up made a path visualizer linked [here](https://pedro-path-generator.vercel.app). +The old Desmos visualizer is [here](https://www.desmos.com/calculator/3so1zx0hcd), but the one by +Watt'S Up is honestly a lot better. +One last thing to note is that Pedro Pathing operates in inches and radians. You can use centimeters +instead of inches, but you'll have to input all your measurement in centimeters, and any distances +that the tuners require you to push the robot or the tuners output will say "inches" when the actual +measurements will be in centimeters. + +## Tuning +* First, make sure that your motor names and directions, located at the top of `FollowerConstants`, + are correct. + +* After that, we need the mass of the robot in kg. This is used for the centripetal force correction, + and the mass, with the variable name `mass`, should be put on line `92` in the `FollowerConstants` + class under the `tuning` package. + +* Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a + 45 degree angle from the forward direction, but the actual direction the force is output is actually + closer to forward. Before running any OpModes, make sure your motors are reversed properly in the + `Follower` class constructor. To find the direction your wheels will go, you will need to run the + `Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full + power for 40 inches forward and to the right, respectively. The distance can be changed through FTC + Dashboard under the dropdown for each respective class, but higher distances work better. After the + distance has finished running, the end velocity will be output to telemetry. The robot may continue + to drift a little bit after the robot has finished running the distance, so make sure you have + plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `39` in + the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `40` in the + `FollowerConstants` class. The variable names should be `xMovement` and `yMovement`, respectively. + +* The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These + find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to + get a more accurate estimation of the drive vector. To find this, you will need to run the + `Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes. + These will run your robot until it hits a velocity of 30 inches/second forward and to the right, + respectively. The velocity can be changed through FTC Dashboard under the dropdown for each + respective class, but higher velocities work better. After the velocity has been reached, power will + be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at + which point it will display the deceleration in telemetry. This robot will need to drift to a stop + to properly work, and the higher the velocity the greater the drift distance, so make sure you have + enough room. Once you're done, put the zero power acceleration for the + `Forward Zero Power Acceleration Tuner` on line `100` in the `FollowerConstants` class and the zero + power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `104` in the + `FollowerConstants` class. The variable names should be `forwardZeroPowerAcceleration` and + `lateralZeroPowerAcceleration`, respectively. + +* After this, we will want to tune the translational PID. Go to FTC Dashboard and disable all but + the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. + Make sure you disable the timer on autonomous OpModes. You will notice in telemetry a message saying + that the robot will travel a distance forward and backward, this will not happen until later, so for + now you can ignore this message. The robot should not move when you run the opmode initally. Instead, + it should correct when you push it away from its starting position. Note that this correction should + happen regardless of the robot's rotation, and that the robot should not rotate itself (if it does, + disable `useHeading` as mentioned prior). Also note that the robot will only correct to an imaginary line + that runs straight forward from the robot's starting position, meaning that it will not correct in the + (original) forward direction. The PID for the translational error is called `translationalPIDF`. + If you need to add a feedforward value, use the `translationalPIDFFeedForward` since that will add + the feedforward in the direction the robot is trying to move, rather than the feedforward in the + PIDF itself, since those will only add the feedforward one way. You can change the PIDF constants + and feedforward values, under the `FollowerConstants` tab in FTC Dashboard. + To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides + you push to reduce field wear and tear as well as push with varying power and distance. I would + recommend tuning the PID so that it is capable of correcting while minimizing oscillations and still + achieving a satisfactory level of accuracy. Overall, try to tune for fewer oscillations rather than + higher speeds or perfect accuracy, since this will make the robot run more smoothly under actual + pathing conditions. + +* Next, we will tune the heading PID. The process is essentially the same as above, except you will + want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from + opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with + "translational" in the name, you will instead want to look for stuff with "heading" in the name. + Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this. + +* Afterwards, we will tune the drive PID. Before we continue, we will need to set the + `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor + of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but + what works best for you is most important. Higher numbers will cause a faster brake, but increase + oscillations at the end. Lower numbers will do the opposite. This can be found on line `113` in + `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference, + my P values were in the hundredths and thousandths place values, and my D values were in the hundred + thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and + `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth` + and don't forget to turn off the timer on the OpMode. Then, tune the PID following the tips from + earlier. For this, it is very important to try to reduce oscillations. Additionally, I would + absolutely not recommend using the I, or integral, part of the PID for this. Using integral in + drivetrain PIDs is already not ideal, but it will continuously build up error in this PID, causing + major issues when it gets too strong. Don't use I; P and D are enough. In the versions of Pedro Pathing + from after late July 2024, there is a Kalman filter on the drive error and the drive PID has a + filter as well. These smooth out the drive error and PID response so that there is not as much + oscillation during the braking portion of each path. The Kalman filter is tuned to have 6 for the + model covariance and 1 for the data covariance. These values should work, but if you feel inclined + to tune the Kalman filter yourself, a higher ratio of model covariance to data covariance means that + the filter will rely more on its previous output rather than the data, and the opposite ratio will + mean that the filter will rely more so on the data input (the raw drive error) rather than the model. + The filtered PID functions like a normal PID, except the derivative term is a weighted average of the + current derivative and the previous derivative. Currently, the weighting, or time constant for the + drive filtered PID is 0.6, so the derivative output is 0.6 times the previous derivative plus 0.4 + times the current derivative. Feel free to mess around with these values and find what works best + for your robot! + +* Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open + up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` + and turn off its timer. If you notice the robot is correcting towards the inside of the curve + as/after running a path, then increase `centripetalScaling`, which can be found on line `95` of + `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease + `centripetalScaling`. + +* Once you've found satisfactory tunings for everything, run the robot around in + `StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also + `Circle`, but that's more so for fun than anything else. If you notice something could be improved, + feel free to mess around more with your PIDs. That should be all! If you have any more questions, + refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) + +## Note About the PIDs +In versions of Pedro Pathing before early August 2024, there were 2 PIDs used in the translational, +heading, and drive control. However, now there is only one main PID. The old system can still be used. +Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `163` to `165` +to true. They should be named `useSecondaryTranslationalPID`, `useSecondaryHeadingPID`, and `useSecondaryDrivePID`. +This will enable the two PID system that Pedro Pathing originally used. From there, scroll +down and all the values pertaining to the secondary PIDs will be there. The two PID system works with +a PID that handles larger errors (the main PID) and a second PID to handle smaller errors (the +secondary PID). The main PID should be tuned to move the error within the secondary PID's range +without providing too much momentum that could cause an overshoot. The secondary PID should be tuned +to correct within its range quickly and accurately while minimizing oscillations. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java new file mode 100644 index 0000000..9f35ef4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.examples; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; + +/** + * This is the TeleOpEnhancements OpMode. It is an example usage of the TeleOp enhancements that + * Pedro Pathing is capable of. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/21/2024 + */ +@TeleOp(name = "Pedro Pathing TeleOp Enhancements", group = "Test") +public class TeleOpEnhancements extends OpMode { + private Follower follower; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + + /** + * This initializes the drive motors as well as the Follower and motion Vectors. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + follower.startTeleopDrive(); + } + + /** + * This runs the OpMode. This is only drive control with Pedro Pathing live centripetal force + * correction. + */ + @Override + public void loop() { + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); + follower.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java new file mode 100644 index 0000000..a6ed238 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java @@ -0,0 +1,172 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.follower; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the DriveVectorScaler class. This class takes in inputs Vectors for driving, heading + * correction, and translational/centripetal correction and returns an array with wheel powers. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +public class DriveVectorScaler { + // This is ordered left front, left back, right front, right back. These are also normalized. + private Vector[] mecanumVectors; + private double maxPowerScaling = 1; + + /** + * This creates a new DriveVectorScaler, which takes in various movement vectors and outputs + * the wheel drive powers necessary to move in the intended direction, given the true movement + * vector for the front left mecanum wheel. + * + * @param frontLeftVector this is the front left mecanum wheel's preferred drive vector. + */ + public DriveVectorScaler(Vector frontLeftVector) { + Vector copiedFrontLeftVector = MathFunctions.normalizeVector(frontLeftVector); + mecanumVectors = new Vector[]{ + new Vector(copiedFrontLeftVector.getMagnitude(), copiedFrontLeftVector.getTheta()), + new Vector(copiedFrontLeftVector.getMagnitude(), 2*Math.PI-copiedFrontLeftVector.getTheta()), + new Vector(copiedFrontLeftVector.getMagnitude(), 2*Math.PI-copiedFrontLeftVector.getTheta()), + new Vector(copiedFrontLeftVector.getMagnitude(), copiedFrontLeftVector.getTheta())}; + } + + /** + * This takes in vectors for corrective power, heading power, and pathing power and outputs + * an Array of four doubles, one for each wheel's motor power. + * + * IMPORTANT NOTE: all vector inputs are clamped between 0 and 1 inclusive in magnitude. + * + * @param correctivePower this Vector includes the centrifugal force scaling Vector as well as a + * translational power Vector to correct onto the Bezier curve the Follower + * is following. + * @param headingPower this Vector points in the direction of the robot's current heaing, and + * the magnitude tells the robot how much it should turn and in which + * direction. + * @param pathingPower this Vector points in the direction the robot needs to go to continue along + * the Path. + * @param robotHeading this is the current heading of the robot, which is used to calculate how + * much power to allocate to each wheel. + * @return this returns an Array of doubles with a length of 4, which contains the wheel powers. + */ + public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { + // clamps down the magnitudes of the input vectors + if (correctivePower.getMagnitude() > maxPowerScaling) correctivePower.setMagnitude(maxPowerScaling); + if (headingPower.getMagnitude() > maxPowerScaling) headingPower.setMagnitude(maxPowerScaling); + if (pathingPower.getMagnitude() > maxPowerScaling) pathingPower.setMagnitude(maxPowerScaling); + + // the powers for the wheel vectors + double [] wheelPowers = new double[4]; + + // This contains a copy of the mecanum wheel vectors + Vector[] mecanumVectorsCopy = new Vector[4]; + + // this contains the pathing vectors, one for each side (heading control requires 2) + Vector[] truePathingVectors = new Vector[2]; + + if (correctivePower.getMagnitude() == maxPowerScaling) { + // checks for corrective power equal to max power scaling in magnitude. if equal, then set pathing power to that + truePathingVectors[0] = MathFunctions.copyVector(correctivePower); + truePathingVectors[1] = MathFunctions.copyVector(correctivePower); + } else { + // corrective power did not take up all the power, so add on heading power + Vector leftSideVector = MathFunctions.subtractVectors(correctivePower, headingPower); + Vector rightSideVector = MathFunctions.addVectors(correctivePower, headingPower); + + if (leftSideVector.getMagnitude() > maxPowerScaling || rightSideVector.getMagnitude() > maxPowerScaling) { + //if the combined corrective and heading power is greater than 1, then scale down heading power + double headingScalingFactor = Math.min(findNormalizingScaling(correctivePower, headingPower), findNormalizingScaling(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, -1))); + truePathingVectors[0] = MathFunctions.subtractVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor)); + truePathingVectors[1] = MathFunctions.addVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor)); + } else { + // if we're here then we can add on some drive power but scaled down to 1 + Vector leftSideVectorWithPathing = MathFunctions.addVectors(leftSideVector, pathingPower); + Vector rightSideVectorWithPathing = MathFunctions.addVectors(rightSideVector, pathingPower); + + if (leftSideVectorWithPathing.getMagnitude() > maxPowerScaling || rightSideVectorWithPathing.getMagnitude() > maxPowerScaling) { + // too much power now, so we scale down the pathing vector + double pathingScalingFactor = Math.min(findNormalizingScaling(leftSideVector, pathingPower), findNormalizingScaling(rightSideVector, pathingPower)); + truePathingVectors[0] = MathFunctions.addVectors(leftSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor)); + truePathingVectors[1] = MathFunctions.addVectors(rightSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor)); + } else { + // just add the vectors together and you get the final vector + truePathingVectors[0] = MathFunctions.copyVector(leftSideVectorWithPathing); + truePathingVectors[1] = MathFunctions.copyVector(rightSideVectorWithPathing); + } + } + } + + truePathingVectors[0] = MathFunctions.scalarMultiplyVector(truePathingVectors[0], 2.0); + truePathingVectors[1] = MathFunctions.scalarMultiplyVector(truePathingVectors[1], 2.0); + + for (int i = 0; i < mecanumVectorsCopy.length; i++) { + // this copies the vectors from mecanumVectors but creates new references for them + mecanumVectorsCopy[i] = MathFunctions.copyVector(mecanumVectors[i]); + + mecanumVectorsCopy[i].rotateVector(robotHeading); + } + + wheelPowers[0] = (mecanumVectorsCopy[1].getXComponent()*truePathingVectors[0].getYComponent() - truePathingVectors[0].getXComponent()*mecanumVectorsCopy[1].getYComponent()) / (mecanumVectorsCopy[1].getXComponent()*mecanumVectorsCopy[0].getYComponent() - mecanumVectorsCopy[0].getXComponent()*mecanumVectorsCopy[1].getYComponent()); + wheelPowers[1] = (mecanumVectorsCopy[0].getXComponent()*truePathingVectors[0].getYComponent() - truePathingVectors[0].getXComponent()*mecanumVectorsCopy[0].getYComponent()) / (mecanumVectorsCopy[0].getXComponent()*mecanumVectorsCopy[1].getYComponent() - mecanumVectorsCopy[1].getXComponent()*mecanumVectorsCopy[0].getYComponent()); + wheelPowers[2] = (mecanumVectorsCopy[3].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[3].getYComponent()) / (mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent() - mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent()); + wheelPowers[3] = (mecanumVectorsCopy[2].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[2].getYComponent()) / (mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent() - mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent()); + + double wheelPowerMax = Math.max(Math.max(Math.abs(wheelPowers[0]), Math.abs(wheelPowers[1])), Math.max(Math.abs(wheelPowers[2]), Math.abs(wheelPowers[3]))); + if (wheelPowerMax > maxPowerScaling) { + wheelPowers[0] /= wheelPowerMax; + wheelPowers[1] /= wheelPowerMax; + wheelPowers[2] /= wheelPowerMax; + wheelPowers[3] /= wheelPowerMax; + } + + return wheelPowers; + } + + /** + * This takes in two Vectors, one static and one variable, and returns the scaling factor that, + * when multiplied to the variable Vector, results in magnitude of the sum of the static Vector + * and the scaled variable Vector being the max power scaling. + * + * IMPORTANT NOTE: I did not intend for this to be used for anything other than the method above + * this one in this class, so there will be errors if you input Vectors of length greater than maxPowerScaling, + * and it will scale up the variable Vector if the magnitude of the sum of the two input Vectors + * isn't greater than maxPowerScaling. So, just don't use this elsewhere. There's gotta be a better way to do + * whatever you're trying to do. + * + * I know that this is used outside of this class, however, I created this method so I get to + * use it if I want to. Also, it's only used once outside of the DriveVectorScaler class, and + * it's used to scale Vectors, as intended. + * + * @param staticVector the Vector that is held constant. + * @param variableVector the Vector getting scaled to make the sum of the input Vectors have a + * magnitude of maxPowerScaling. + * @return returns the scaling factor for the variable Vector. + */ + public double findNormalizingScaling(Vector staticVector, Vector variableVector) { + double a = Math.pow(variableVector.getXComponent(), 2) + Math.pow(variableVector.getYComponent(), 2); + double b = staticVector.getXComponent() * variableVector.getXComponent() + staticVector.getYComponent() * variableVector.getYComponent(); + double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - Math.pow(maxPowerScaling, 2); + return (-b + Math.sqrt(Math.pow(b, 2) - a*c))/(a); + + } + + /** + * Sets the maximum power that can be used by the drive vector scaler. Clamped between 0 and 1. + * + * @param maxPowerScaling setting the max power scaling + */ + public void setMaxPowerScaling(double maxPowerScaling) { + this.maxPowerScaling = MathFunctions.clamp(maxPowerScaling, 0, 1); + } + + /** + * Gets the maximum power that can be used by the drive vector scaler. Ranges between 0 and 1. + * + * @return returns the max power scaling + */ + public double getMaxPowerScaling() { + return maxPowerScaling; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java new file mode 100644 index 0000000..0a45eb8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -0,0 +1,1010 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.follower; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryTranslationalPID; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathCallback; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; +import org.firstinspires.ftc.teamcode.pedroPathing.util.FilteredPIDFController; +import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilter; +import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the Follower class. It handles the actual following of the paths and all the on-the-fly + * calculations that are relevant for movement. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +@Config +public class Follower { + private HardwareMap hardwareMap; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private DriveVectorScaler driveVectorScaler; + + private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; + + private Pose closestPose; + + private Path currentPath; + + private PathChain currentPathChain; + + private final int BEZIER_CURVE_BINARY_STEP_LIMIT = FollowerConstants.BEZIER_CURVE_BINARY_STEP_LIMIT; + private final int AVERAGED_VELOCITY_SAMPLE_NUMBER = FollowerConstants.AVERAGED_VELOCITY_SAMPLE_NUMBER; + + private int chainIndex; + + private long[] pathStartTimes; + + private boolean followingPathChain; + private boolean holdingPosition; + private boolean isBusy; + private boolean reachedParametricPathEnd; + private boolean holdPositionAtEnd; + private boolean teleopDrive; + + private double previousSecondaryTranslationalIntegral; + private double previousTranslationalIntegral; + private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling; + private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling; + public double driveError; + public double headingError; + + private long reachedParametricPathEndTime; + + private double[] drivePowers; + private double[] teleopDriveValues; + + private ArrayList velocities = new ArrayList<>(); + private ArrayList accelerations = new ArrayList<>(); + + private Vector averageVelocity; + private Vector averagePreviousVelocity; + private Vector averageAcceleration; + private Vector secondaryTranslationalIntegralVector; + private Vector translationalIntegralVector; + private Vector teleopDriveVector; + private Vector teleopHeadingVector; + public Vector driveVector; + public Vector headingVector; + public Vector translationalVector; + public Vector centripetalVector; + public Vector correctiveVector; + + private PIDFController secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients); + private PIDFController secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral); + private PIDFController translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients); + private PIDFController translationalIntegral = new PIDFController(FollowerConstants.translationalIntegral); + private PIDFController secondaryHeadingPIDF = new PIDFController(FollowerConstants.secondaryHeadingPIDFCoefficients); + private PIDFController headingPIDF = new PIDFController(FollowerConstants.headingPIDFCoefficients); + private FilteredPIDFController secondaryDrivePIDF = new FilteredPIDFController(FollowerConstants.secondaryDrivePIDFCoefficients); + private FilteredPIDFController drivePIDF = new FilteredPIDFController(FollowerConstants.drivePIDFCoefficients); + + private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters); + private double[] driveErrors; + private double rawDriveError; + private double previousRawDriveError; + + public static boolean drawOnDashboard = true; + public static boolean useTranslational = true; + public static boolean useCentripetal = true; + public static boolean useHeading = true; + public static boolean useDrive = true; + + /** + * This creates a new Follower given a HardwareMap. + * + * @param hardwareMap HardwareMap required + */ + public Follower(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + initialize(); + } + + /** + * This initializes the follower. + * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are + * initialized and their behavior is set, and the variables involved in approximating first and + * second derivatives for teleop are set. + */ + public void initialize() { + driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector); + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + breakFollowing(); + } + + /** + * This sets the maximum power the motors are allowed to use. + * + * @param set This caps the motor power from [0, 1]. + */ + public void setMaxPower(double set) { + driveVectorScaler.setMaxPowerScaling(set); + } + + /** + * This gets a Point from the current Path from a specified t-value. + * + * @return returns the Point. + */ + public Point getPointFromPath(double t) { + if (currentPath != null) { + return currentPath.getPoint(t); + } else { + return null; + } + } + + /** + * This returns the current pose from the PoseUpdater. + * + * @return returns the pose + */ + public Pose getPose() { + return poseUpdater.getPose(); + } + + /** + * This sets the current pose in the PoseUpdater without using offsets. + * + * @param pose The pose to set the current pose to. + */ + public void setPose(Pose pose) { + poseUpdater.setPose(pose); + } + + /** + * This returns the current velocity of the robot as a Vector. + * + * @return returns the current velocity as a Vector. + */ + public Vector getVelocity() { + return poseUpdater.getVelocity(); + } + + /** + * This returns the current acceleration of the robot as a Vector. + * + * @return returns the current acceleration as a Vector. + */ + public Vector getAcceleration() { + return poseUpdater.getAcceleration(); + } + + /** + * This returns the magnitude of the current velocity. For when you only need the magnitude. + * + * @return returns the magnitude of the current velocity. + */ + public double getVelocityMagnitude() { + return poseUpdater.getVelocity().getMagnitude(); + } + + /** + * This sets the starting pose. Do not run this after moving at all. + * + * @param pose the pose to set the starting pose to. + */ + public void setStartingPose(Pose pose) { + poseUpdater.setStartingPose(pose); + } + + /** + * This sets the current pose, using offsets so no reset time delay. This is better than the + * Road Runner reset, in general. Think of using offsets as setting trim in an aircraft. This can + * be reset as well, so beware of using the resetOffset() method. + * + * @param set The pose to set the current pose to. + */ + public void setCurrentPoseWithOffset(Pose set) { + poseUpdater.setCurrentPoseWithOffset(set); + } + + /** + * This sets the offset for only the x position. + * + * @param xOffset This sets the offset. + */ + public void setXOffset(double xOffset) { + poseUpdater.setXOffset(xOffset); + } + + /** + * This sets the offset for only the y position. + * + * @param yOffset This sets the offset. + */ + public void setYOffset(double yOffset) { + poseUpdater.setYOffset(yOffset); + } + + /** + * This sets the offset for only the heading. + * + * @param headingOffset This sets the offset. + */ + public void setHeadingOffset(double headingOffset) { + poseUpdater.setHeadingOffset(headingOffset); + } + + /** + * This returns the x offset. + * + * @return returns the x offset. + */ + public double getXOffset() { + return poseUpdater.getXOffset(); + } + + /** + * This returns the y offset. + * + * @return returns the y offset. + */ + public double getYOffset() { + return poseUpdater.getYOffset(); + } + + /** + * This returns the heading offset. + * + * @return returns the heading offset. + */ + public double getHeadingOffset() { + return poseUpdater.getHeadingOffset(); + } + + /** + * This resets all offsets set to the PoseUpdater. If you have reset your pose using the + * setCurrentPoseUsingOffset(Pose set) method, then your pose will be returned to what the + * PoseUpdater thinks your pose would be, not the pose you reset to. + */ + public void resetOffset() { + poseUpdater.resetOffset(); + } + + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(BezierPoint point, double heading) { + breakFollowing(); + holdingPosition = true; + isBusy = false; + followingPathChain = false; + currentPath = new Path(point); + currentPath.setConstantHeadingInterpolation(heading); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); + } + + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(Point point, double heading) { + holdPoint(new BezierPoint(point), heading); + } + + /** + * This holds a Point. + * + * @param pose the Point (as a Pose) to stay at. + */ + public void holdPoint(Pose pose) { + holdPoint(new Point(pose), pose.getHeading()); + } + + /** + * This follows a Path. + * This also makes the Follower hold the last Point on the Path. + * + * @param path the Path to follow. + */ + public void followPath(Path path, boolean holdEnd) { + breakFollowing(); + holdPositionAtEnd = holdEnd; + isBusy = true; + followingPathChain = false; + currentPath = path; + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } + + /** + * This follows a Path. + * + * @param path the Path to follow. + */ + public void followPath(Path path) { + followPath(path, false); + } + + /** + * This follows a PathChain. Drive vector projection is only done on the last Path. + * This also makes the Follower hold the last Point on the PathChain. + * + * @param pathChain the PathChain to follow. + */ + public void followPath(PathChain pathChain, boolean holdEnd) { + breakFollowing(); + holdPositionAtEnd = holdEnd; + pathStartTimes = new long[pathChain.size()]; + pathStartTimes[0] = System.currentTimeMillis(); + isBusy = true; + followingPathChain = true; + chainIndex = 0; + currentPathChain = pathChain; + currentPath = pathChain.getPath(chainIndex); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } + + /** + * This follows a PathChain. Drive vector projection is only done on the last Path. + * + * @param pathChain the PathChain to follow. + */ + public void followPath(PathChain pathChain) { + followPath(pathChain, false); + } + + /** + * This starts teleop drive control. + */ + public void startTeleopDrive() { + breakFollowing(); + teleopDrive = true; + } + + /** + * Calls an update to the PoseUpdater, which updates the robot's current position estimate. + */ + public void updatePose() { + poseUpdater.update(); + + if (drawOnDashboard) { + dashboardPoseTracker.update(); + } + } + + /** + * This calls an update to the PoseUpdater, which updates the robot's current position estimate. + * This also updates all the Follower's PIDFs, which updates the motor powers. + */ + public void update() { + updatePose(); + + if (!teleopDrive) { + if (currentPath != null) { + if (holdingPosition) { + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); + + drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading()); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } else { + if (isBusy) { + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + + if (followingPathChain) updateCallbacks(); + + drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading()); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } + if (currentPath.isAtParametricEnd()) { + if (followingPathChain && chainIndex < currentPathChain.size() - 1) { + // Not at last path, keep going + breakFollowing(); + pathStartTimes[chainIndex] = System.currentTimeMillis(); + isBusy = true; + followingPathChain = true; + chainIndex++; + currentPath = currentPathChain.getPath(chainIndex); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } else { + // At last path, run some end detection stuff + // set isBusy to false if at end + if (!reachedParametricPathEnd) { + reachedParametricPathEnd = true; + reachedParametricPathEndTime = System.currentTimeMillis(); + } + + if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) { + if (holdPositionAtEnd) { + holdPositionAtEnd = false; + holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1)); + } else { + breakFollowing(); + } + } + } + } + } + } + } else { + velocities.add(poseUpdater.getVelocity()); + velocities.remove(velocities.get(velocities.size() - 1)); + + calculateAveragedVelocityAndAcceleration(); + + drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading()); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } + } + + /** + * This sets the teleop drive vectors. This defaults to robot centric. + * + * @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric + * movement, this is the x-axis. + * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric + * movement, this is the y-axis. + * @param heading determines the heading vector for the robot in teleop. + */ + public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { + setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true); + } + + /** + * This sets the teleop drive vectors. + * + * @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric + * movement, this is the x-axis. + * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric + * movement, this is the y-axis. + * @param heading determines the heading vector for the robot in teleop. + * @param robotCentric sets if the movement will be field or robot centric + */ + public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) { + teleopDriveValues[0] = MathFunctions.clamp(forwardDrive, -1, 1); + teleopDriveValues[1] = MathFunctions.clamp(lateralDrive, -1, 1); + teleopDriveValues[2] = MathFunctions.clamp(heading, -1, 1); + teleopDriveVector.setOrthogonalComponents(teleopDriveValues[0], teleopDriveValues[1]); + teleopDriveVector.setMagnitude(MathFunctions.clamp(teleopDriveVector.getMagnitude(), 0, 1)); + + if (robotCentric) { + teleopDriveVector.rotateVector(getPose().getHeading()); + } + + teleopHeadingVector.setComponents(teleopDriveValues[2], getPose().getHeading()); + } + + /** + * This calculates an averaged approximate velocity and acceleration. This is used for a + * real-time correction of centripetal force, which is used in teleop. + */ + public void calculateAveragedVelocityAndAcceleration() { + averageVelocity = new Vector(); + averagePreviousVelocity = new Vector(); + + for (int i = 0; i < velocities.size() / 2; i++) { + averageVelocity = MathFunctions.addVectors(averageVelocity, velocities.get(i)); + } + averageVelocity = MathFunctions.scalarMultiplyVector(averageVelocity, 1.0 / ((double) velocities.size() / 2)); + + for (int i = velocities.size() / 2; i < velocities.size(); i++) { + averagePreviousVelocity = MathFunctions.addVectors(averagePreviousVelocity, velocities.get(i)); + } + averagePreviousVelocity = MathFunctions.scalarMultiplyVector(averagePreviousVelocity, 1.0 / ((double) velocities.size() / 2)); + + accelerations.add(MathFunctions.subtractVectors(averageVelocity, averagePreviousVelocity)); + accelerations.remove(accelerations.size() - 1); + + averageAcceleration = new Vector(); + + for (int i = 0; i < accelerations.size(); i++) { + averageAcceleration = MathFunctions.addVectors(averageAcceleration, accelerations.get(i)); + } + averageAcceleration = MathFunctions.scalarMultiplyVector(averageAcceleration, 1.0 / accelerations.size()); + } + + /** + * This checks if any PathCallbacks should be run right now, and runs them if applicable. + */ + public void updateCallbacks() { + for (PathCallback callback : currentPathChain.getCallbacks()) { + if (!callback.hasBeenRun()) { + if (callback.getType() == PathCallback.PARAMETRIC) { + // parametric call back + if (chainIndex == callback.getIndex() && (getCurrentTValue() >= callback.getStartCondition() || MathFunctions.roughlyEquals(getCurrentTValue(), callback.getStartCondition()))) { + callback.run(); + } + } else { + // time based call back + if (chainIndex >= callback.getIndex() && System.currentTimeMillis() - pathStartTimes[callback.getIndex()] > callback.getStartCondition()) { + callback.run(); + } + + } + } + } + } + + /** + * This resets the PIDFs and stops following the current Path. + */ + public void breakFollowing() { + teleopDrive = false; + holdingPosition = false; + isBusy = false; + reachedParametricPathEnd = false; + secondaryDrivePIDF.reset(); + drivePIDF.reset(); + secondaryHeadingPIDF.reset(); + headingPIDF.reset(); + secondaryTranslationalPIDF.reset(); + secondaryTranslationalIntegral.reset(); + secondaryTranslationalIntegralVector = new Vector(); + previousSecondaryTranslationalIntegral = 0; + translationalPIDF.reset(); + translationalIntegral.reset(); + translationalIntegralVector = new Vector(); + previousTranslationalIntegral = 0; + driveVector = new Vector(); + headingVector = new Vector(); + translationalVector = new Vector(); + centripetalVector = new Vector(); + correctiveVector = new Vector(); + driveError = 0; + headingError = 0; + rawDriveError = 0; + previousRawDriveError = 0; + driveErrors = new double[2]; + for (int i = 0; i < driveErrors.length; i++) { + driveErrors[i] = 0; + } + driveKalmanFilter.reset(); + + for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) { + velocities.add(new Vector()); + } + for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) { + accelerations.add(new Vector()); + } + calculateAveragedVelocityAndAcceleration(); + teleopDriveValues = new double[3]; + teleopDriveVector = new Vector(); + teleopHeadingVector = new Vector(); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(0); + } + } + + /** + * This returns if the Follower is currently following a Path or a PathChain. + * + * @return returns if the Follower is busy. + */ + public boolean isBusy() { + return isBusy; + } + + /** + * This returns a Vector in the direction the robot must go to move along the path. This Vector + * takes into account the projected position of the robot to calculate how much power is needed. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the drive vector. + */ + public Vector getDriveVector() { + if (!useDrive) return new Vector(); + if (followingPathChain && chainIndex < currentPathChain.size() - 1) { + return new Vector(driveVectorScaler.getMaxPowerScaling(), currentPath.getClosestPointTangentVector().getTheta()); + } + + driveError = getDriveVelocityError(); + + if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) { + secondaryDrivePIDF.updateError(driveError); + driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); + return MathFunctions.copyVector(driveVector); + } + + drivePIDF.updateError(driveError); + driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); + return MathFunctions.copyVector(driveVector); + } + + /** + * This returns the velocity the robot needs to be at to make it to the end of the Path + * at some specified deceleration (well technically just some negative acceleration). + * + * @return returns the projected velocity. + */ + public double getDriveVelocityError() { + double distanceToGoal; + if (!currentPath.isAtParametricEnd()) { + distanceToGoal = currentPath.length() * (1 - currentPath.getClosestPointTValue()); + } else { + Vector offset = new Vector(); + offset.setOrthogonalComponents(getPose().getX() - currentPath.getLastControlPoint().getX(), getPose().getY() - currentPath.getLastControlPoint().getY()); + distanceToGoal = MathFunctions.dotProduct(currentPath.getEndTangent(), offset); + } + + Vector distanceToGoalVector = MathFunctions.scalarMultiplyVector(MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector()), distanceToGoal); + Vector velocity = new Vector(MathFunctions.dotProduct(getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()); + + Vector forwardHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading()); + double forwardVelocity = MathFunctions.dotProduct(forwardHeadingVector, velocity); + double forwardDistanceToGoal = MathFunctions.dotProduct(forwardHeadingVector, distanceToGoalVector); + double forwardVelocityGoal = MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * forwardZeroPowerAcceleration * forwardDistanceToGoal)); + double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * forwardDistanceToGoal)); + + Vector lateralHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); + double lateralVelocity = MathFunctions.dotProduct(lateralHeadingVector, velocity); + double lateralDistanceToGoal = MathFunctions.dotProduct(lateralHeadingVector, distanceToGoalVector); + double lateralVelocityGoal = MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * lateralZeroPowerAcceleration * lateralDistanceToGoal)); + double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * lateralDistanceToGoal)); + + Vector forwardVelocityError = new Vector(forwardVelocityGoal - forwardVelocityZeroPowerDecay - forwardVelocity, forwardHeadingVector.getTheta()); + Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); + Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); + + previousRawDriveError = rawDriveError; + rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + + double projection = 2 * driveErrors[1] - driveErrors[0]; + + driveKalmanFilter.update(rawDriveError - previousRawDriveError, projection); + + for (int i = 0; i < driveErrors.length - 1; i++) { + driveErrors[i] = driveErrors[i + 1]; + } + driveErrors[1] = driveKalmanFilter.getState(); + + return driveKalmanFilter.getState(); + } + + /** + * This returns a Vector in the direction of the robot that contains the heading correction + * as its magnitude. Positive heading correction turns the robot counter-clockwise, and negative + * heading correction values turn the robot clockwise. So basically, Pedro Pathing uses a right- + * handed coordinate system. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the heading vector. + */ + public Vector getHeadingVector() { + if (!useHeading) return new Vector(); + headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); + if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) { + secondaryHeadingPIDF.updateError(headingError); + headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); + return MathFunctions.copyVector(headingVector); + } + headingPIDF.updateError(headingError); + headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); + return MathFunctions.copyVector(headingVector); + } + + /** + * This returns a combined Vector in the direction the robot must go to correct both translational + * error as well as centripetal force. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the corrective vector. + */ + public Vector getCorrectiveVector() { + Vector centripetal = getCentripetalForceCorrection(); + Vector translational = getTranslationalCorrection(); + Vector corrective = MathFunctions.addVectors(centripetal, translational); + + if (corrective.getMagnitude() > driveVectorScaler.getMaxPowerScaling()) { + return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational))); + } + + correctiveVector = MathFunctions.copyVector(corrective); + + return corrective; + } + + /** + * This returns a Vector in the direction the robot must go to account for only translational + * error. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the translational correction vector. + */ + public Vector getTranslationalCorrection() { + if (!useTranslational) return new Vector(); + Vector translationalVector = new Vector(); + double x = closestPose.getX() - poseUpdater.getPose().getX(); + double y = closestPose.getY() - poseUpdater.getPose().getY(); + translationalVector.setOrthogonalComponents(x, y); + + if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) { + translationalVector = MathFunctions.subtractVectors(translationalVector, new Vector(MathFunctions.dotProduct(translationalVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + + secondaryTranslationalIntegralVector = MathFunctions.subtractVectors(secondaryTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(secondaryTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + translationalIntegralVector = MathFunctions.subtractVectors(translationalIntegralVector, new Vector(MathFunctions.dotProduct(translationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + } + + if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch && useSecondaryTranslationalPID) { + secondaryTranslationalIntegral.updateError(translationalVector.getMagnitude()); + secondaryTranslationalIntegralVector = MathFunctions.addVectors(secondaryTranslationalIntegralVector, new Vector(secondaryTranslationalIntegral.runPIDF() - previousSecondaryTranslationalIntegral, translationalVector.getTheta())); + previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.runPIDF(); + + secondaryTranslationalPIDF.updateError(translationalVector.getMagnitude()); + translationalVector.setMagnitude(secondaryTranslationalPIDF.runPIDF() + secondaryTranslationalPIDFFeedForward); + translationalVector = MathFunctions.addVectors(translationalVector, secondaryTranslationalIntegralVector); + } else { + translationalIntegral.updateError(translationalVector.getMagnitude()); + translationalIntegralVector = MathFunctions.addVectors(translationalIntegralVector, new Vector(translationalIntegral.runPIDF() - previousTranslationalIntegral, translationalVector.getTheta())); + previousTranslationalIntegral = translationalIntegral.runPIDF(); + + translationalPIDF.updateError(translationalVector.getMagnitude()); + translationalVector.setMagnitude(translationalPIDF.runPIDF() + translationalPIDFFeedForward); + translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector); + } + + translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, driveVectorScaler.getMaxPowerScaling())); + + this.translationalVector = MathFunctions.copyVector(translationalVector); + + return translationalVector; + } + + /** + * This returns the raw translational error, or how far off the closest point the robot is. + * + * @return This returns the raw translational error as a Vector. + */ + public Vector getTranslationalError() { + Vector error = new Vector(); + double x = closestPose.getX() - poseUpdater.getPose().getX(); + double y = closestPose.getY() - poseUpdater.getPose().getY(); + error.setOrthogonalComponents(x, y); + return error; + } + + /** + * This returns a Vector in the direction the robot must go to account for only centripetal + * force. + *

+ * Note: This vector is clamped to be between [0, 1] in magnitude. + * + * @return returns the centripetal force correction vector. + */ + public Vector getCentripetalForceCorrection() { + if (!useCentripetal) return new Vector(); + double curvature; + if (!teleopDrive) { + curvature = currentPath.getClosestPointCurvature(); + } else { + double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); + double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); + curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); + } + if (Double.isNaN(curvature)) return new Vector(); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + return centripetalVector; + } + + /** + * This returns the closest pose to the robot on the Path the Follower is currently following. + * This closest pose is calculated through a binary search method with some specified number of + * steps to search. By default, 10 steps are used, which should be more than enough. + * + * @return returns the closest pose. + */ + public Pose getClosestPose() { + return closestPose; + } + + /** + * This returns whether the follower is at the parametric end of its current Path. + * The parametric end is determined by if the closest Point t-value is greater than some specified + * end t-value. + * If running a PathChain, this returns true only if at parametric end of last Path in the PathChain. + * + * @return returns whether the Follower is at the parametric end of its Path. + */ + public boolean atParametricEnd() { + if (followingPathChain) { + if (chainIndex == currentPathChain.size() - 1) return currentPath.isAtParametricEnd(); + return false; + } + return currentPath.isAtParametricEnd(); + } + + /** + * This returns the t value of the closest point on the current Path to the robot + * In the absence of a current Path, it returns 1.0. + * + * @return returns the current t value. + */ + public double getCurrentTValue() { + if (isBusy) return currentPath.getClosestPointTValue(); + return 1.0; + } + + /** + * This returns the current path number. For following Paths, this will return 0. For PathChains, + * this will return the current path number. For holding Points, this will also return 0. + * + * @return returns the current path number. + */ + public double getCurrentPathNumber() { + if (!followingPathChain) return 0; + return chainIndex; + } + + /** + * This returns a new PathBuilder object for easily building PathChains. + * + * @return returns a new PathBuilder object. + */ + public PathBuilder pathBuilder() { + return new PathBuilder(); + } + + /** + * This writes out information about the various motion Vectors to the Telemetry specified. + * + * @param telemetry this is an instance of Telemetry or the FTC Dashboard telemetry that this + * method will use to output the debug data. + */ + public void telemetryDebug(MultipleTelemetry telemetry) { + telemetry.addData("follower busy", isBusy()); + telemetry.addData("heading error", headingError); + telemetry.addData("heading vector magnitude", headingVector.getMagnitude()); + telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude()); + telemetry.addData("corrective vector heading", correctiveVector.getTheta()); + telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude()); + telemetry.addData("translational error direction", getTranslationalError().getTheta()); + telemetry.addData("translational vector magnitude", translationalVector.getMagnitude()); + telemetry.addData("translational vector heading", translationalVector.getMagnitude()); + telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude()); + telemetry.addData("centripetal vector heading", centripetalVector.getTheta()); + telemetry.addData("drive error", driveError); + telemetry.addData("drive vector magnitude", driveVector.getMagnitude()); + telemetry.addData("drive vector heading", driveVector.getTheta()); + telemetry.addData("x", getPose().getX()); + telemetry.addData("y", getPose().getY()); + telemetry.addData("heading", getPose().getHeading()); + telemetry.addData("total heading", poseUpdater.getTotalHeading()); + telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); + telemetry.addData("velocity heading", getVelocity().getTheta()); + driveKalmanFilter.debug(telemetry); + telemetry.update(); + if (drawOnDashboard) { + Drawing.drawDebug(this); + } + } + + /** + * This writes out information about the various motion Vectors to the Telemetry specified. + * + * @param telemetry this is an instance of Telemetry or the FTC Dashboard telemetry that this + * method will use to output the debug data. + */ + public void telemetryDebug(Telemetry telemetry) { + telemetryDebug(new MultipleTelemetry(telemetry)); + } + + /** + * This returns the total number of radians the robot has turned. + * + * @return the total heading. + */ + public double getTotalHeading() { + return poseUpdater.getTotalHeading(); + } + + /** + * This returns the current Path the Follower is following. This can be null. + * + * @return returns the current Path. + */ + public Path getCurrentPath() { + return currentPath; + } + + /** + * This returns the pose tracker for the robot to draw on the Dashboard. + * + * @return returns the pose tracker + */ + public DashboardPoseTracker getDashboardPoseTracker() { + return dashboardPoseTracker; + } + + /** + * This resets the IMU, if applicable. + */ + private void resetIMU() throws InterruptedException { + poseUpdater.resetIMU(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java new file mode 100644 index 0000000..573c4cf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java @@ -0,0 +1,83 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * This is the Encoder class. This tracks the position of a motor of class DcMotorEx. The motor + * must have an encoder attached. It can also get changes in position. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Encoder { + private DcMotorEx motor; + private double previousPosition; + private double currentPosition; + private double multiplier; + + public final static double FORWARD = 1, REVERSE = -1; + + /** + * This creates a new Encoder from a DcMotorEx. + * + * @param setMotor the motor this will be tracking + */ + public Encoder(DcMotorEx setMotor) { + motor = setMotor; + multiplier = FORWARD; + reset(); + } + + /** + * This sets the direction/multiplier of the Encoder. Setting 1 or -1 will make the Encoder track + * forward or in reverse, respectively. Any multiple of either one will scale the Encoder's output + * by that amount. + * + * @param setMultiplier the multiplier/direction to set + */ + public void setDirection(double setMultiplier) { + multiplier = setMultiplier; + } + + /** + * This resets the Encoder's position and the current and previous position in the code. + */ + public void reset() { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + previousPosition = motor.getCurrentPosition(); + currentPosition = motor.getCurrentPosition(); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + /** + * This updates the Encoder's tracked current position and previous position. + */ + public void update() { + previousPosition = currentPosition; + currentPosition = motor.getCurrentPosition(); + } + + /** + * This returns the multiplier/direction of the Encoder. + * + * @return returns the multiplier + */ + public double getMultiplier() { + return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * This returns the change in position from the previous position to the current position. One + * important thing to note is that this encoder does not track velocity, only change in position. + * This is because I am using a pose exponential method of localization, which doesn't need the + * velocity of the encoders. Velocity of the robot is calculated in the localizer using an elapsed + * time timer there. + * + * @return returns the change in position of the Encoder + */ + public double getDeltaPosition() { + return getMultiplier() * (currentPosition - previousPosition); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java new file mode 100644 index 0000000..882ed56 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java @@ -0,0 +1,520 @@ + +/* MIT License + * Copyright (c) [2024] [Base 10 Assets, LLC] + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; + +import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; +import com.qualcomm.robotcore.hardware.I2cAddr; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; +import com.qualcomm.robotcore.util.TypeConversion; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.Arrays; + + +@I2cDeviceType +@DeviceProperties( + name = "goBILDA® Pinpoint Odometry Computer", + xmlTag = "goBILDAPinpoint", + description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" +) + +public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { + + private int deviceStatus = 0; + private int loopTime = 0; + private int xEncoderValue = 0; + private int yEncoderValue = 0; + private float xPosition = 0; + private float yPosition = 0; + private float hOrientation = 0; + private float xVelocity = 0; + private float yVelocity = 0; + private float hVelocity = 0; + + private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod + private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod + + //i2c address of the device + public static final byte DEFAULT_ADDRESS = 0x31; + + public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { + super(deviceClient, deviceClientIsOwned); + + this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); + super.registerArmingStateCallback(false); + } + + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.Other; + } + + @Override + protected synchronized boolean doInitialize() { + ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); + return true; + } + + @Override + public String getDeviceName() { + return "goBILDA® Pinpoint Odometry Computer"; + } + + + //Register map of the i2c device + private enum Register { + DEVICE_ID (1), + DEVICE_VERSION (2), + DEVICE_STATUS (3), + DEVICE_CONTROL (4), + LOOP_TIME (5), + X_ENCODER_VALUE (6), + Y_ENCODER_VALUE (7), + X_POSITION (8), + Y_POSITION (9), + H_ORIENTATION (10), + X_VELOCITY (11), + Y_VELOCITY (12), + H_VELOCITY (13), + MM_PER_TICK (14), + X_POD_OFFSET (15), + Y_POD_OFFSET (16), + YAW_SCALAR (17), + BULK_READ (18); + + private final int bVal; + + Register(int bVal){ + this.bVal = bVal; + } + } + + //Device Status enum that captures the current fault condition of the device + public enum DeviceStatus{ + NOT_READY (0), + READY (1), + CALIBRATING (1 << 1), + FAULT_X_POD_NOT_DETECTED (1 << 2), + FAULT_Y_POD_NOT_DETECTED (1 << 3), + FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), + FAULT_IMU_RUNAWAY (1 << 4); + + private final int status; + + DeviceStatus(int status){ + this.status = status; + } + } + + //enum that captures the direction the encoders are set to + public enum EncoderDirection{ + FORWARD, + REVERSED; + } + + //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used + public enum GoBildaOdometryPods { + goBILDA_SWINGARM_POD, + goBILDA_4_BAR_POD; + } + //enum that captures a limited scope of read data. More options may be added in future update + public enum readData { + ONLY_UPDATE_HEADING, + } + + + /** Writes an int to the i2c device + @param reg the register to write the int to + @param i the integer to write to the register + */ + private void writeInt(final Register reg, int i){ + deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN)); + } + + /** + * Reads an int from a register of the i2c device + * @param reg the register to read from + * @return returns an int that contains the value stored in the read register + */ + private int readInt(Register reg){ + return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN); + } + + /** + * Converts a byte array to a float value + * @param byteArray byte array to transform + * @param byteOrder order of byte array to convert + * @return the float value stored by the byte array + */ + private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ + return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); + } + /** + * Reads a float from a register + * @param reg the register to read + * @return the float value stored in that register + */ + + private float readFloat(Register reg){ + return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); + } + + + /** + * Converts a float to a byte array + * @param value the float array to convert + * @return the byte array converted from the float + */ + private byte [] floatToByteArray (float value, ByteOrder byteOrder) { + return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); + } + + /** + * Writes a byte array to a register on the i2c device + * @param reg the register to write to + * @param bytes the byte array to write + */ + private void writeByteArray (Register reg, byte[] bytes){ + deviceClient.write(reg.bVal,bytes); + } + + /** + * Writes a float to a register on the i2c device + * @param reg the register to write to + * @param f the float to write + */ + private void writeFloat (Register reg, float f){ + byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array(); + deviceClient.write(reg.bVal,bytes); + } + + /** + * Looks up the DeviceStatus enum corresponding with an int value + * @param s int to lookup + * @return the Odometry Computer state + */ + private DeviceStatus lookupStatus (int s){ + if ((s & DeviceStatus.CALIBRATING.status) != 0){ + return DeviceStatus.CALIBRATING; + } + boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; + boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; + + if(!xPodDetected && !yPodDetected){ + return DeviceStatus.FAULT_NO_PODS_DETECTED; + } + if (!xPodDetected){ + return DeviceStatus.FAULT_X_POD_NOT_DETECTED; + } + if (!yPodDetected){ + return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; + } + if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ + return DeviceStatus.FAULT_IMU_RUNAWAY; + } + if ((s & DeviceStatus.READY.status) != 0){ + return DeviceStatus.READY; + } + else { + return DeviceStatus.NOT_READY; + } + } + + /** + * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. + */ + public void update(){ + byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); + deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); + loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); + xEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN); + yEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 12,16), ByteOrder.LITTLE_ENDIAN); + xPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN); + yPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN); + hOrientation = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN); + xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); + yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); + hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); + } + + /** + * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function + * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING + * is supported. + * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING + */ + public void update(readData data) { + if (data == readData.ONLY_UPDATE_HEADING) { + hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); + } + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + */ + public void setOffsets(double xOffset, double yOffset){ + writeFloat(Register.X_POD_OFFSET, (float) xOffset); + writeFloat(Register.Y_POD_OFFSET, (float) yOffset); + } + + /** + * Recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} + + /** + * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} + + /** + * Can reverse the direction of each encoder. + * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward + * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left + */ + public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ + if (xEncoder == EncoderDirection.FORWARD){ + writeInt(Register.DEVICE_CONTROL,1<<5); + } + if (xEncoder == EncoderDirection.REVERSED) { + writeInt(Register.DEVICE_CONTROL,1<<4); + } + + if (yEncoder == EncoderDirection.FORWARD){ + writeInt(Register.DEVICE_CONTROL,1<<3); + } + if (yEncoder == EncoderDirection.REVERSED){ + writeInt(Register.DEVICE_CONTROL,1<<2); + } + } + + /** + * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

+ * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD + */ + public void setEncoderResolution(GoBildaOdometryPods pods){ + if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { + writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); + } + if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); + } + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + */ + public void setEncoderResolution(double ticks_per_mm){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Tuning this value should be unnecessary.
+ * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

+ * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

+ * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. + * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

+ * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com + * @param yawOffset A scalar for the robot's heading. + */ + public void setYawScalar(double yawOffset){ + writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. You can use this to + * update the estimated position of your robot with new external sensor data, or to run a robot + * in field coordinates.

+ * This overrides the current position.

+ * Using this feature to track your robot's position in field coordinates:
+ * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
+ * Say you're on the red alliance, your robot is against the wall and closer to the audience side, + * and the front of your robot is pointing towards the center of the field. + * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always + * keep track of how far away from the center of the field you are.

+ * Using this feature to update your position with additional sensors:
+ * Some robots have a secondary way to locate their robot on the field. This is commonly + * Apriltag localization in FTC, but it can also be something like a distance sensor. + * Often these external sensors are absolute (meaning they measure something about the field) + * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific + * position on the field to use them. In that case, spend most of your time relying on the Pinpoint + * to determine your location. Then when you pull a new position from your secondary sensor, + * send a setPosition command with the new position. The Pinpoint will then track your movement + * relative to that new, more accurate position. + * @param pos a Pose2D describing the robot's new position. + */ + public Pose2D setPosition(Pose2D pos){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); + return pos; + } + + /** + * Checks the deviceID of the Odometry Computer. Should return 1. + * @return 1 if device is functional. + */ + public int getDeviceID(){return readInt(Register.DEVICE_ID);} + + /** + * @return the firmware version of the Odometry Computer + */ + public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } + + public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } + + /** + * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: + * @return one of the following states:
+ * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
+ * READY - The device is currently functioning as normal. GREEN LED
+ * CALIBRATING - The device is currently recalibrating the gyro. RED LED
+ * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
+ * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
+ * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
+ */ + public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } + + /** + * Checks the Odometry Computer's most recent loop time.

+ * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return loop time in microseconds (1/1,000,000 seconds) + */ + public int getLoopTime(){return loopTime; } + + /** + * Checks the Odometry Computer's most recent loop frequency.

+ * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return Pinpoint Frequency in Hz (loops per second), + */ + public double getFrequency(){ + if (loopTime != 0){ + return 1000000.0/loopTime; + } + else { + return 0; + } + } + + /** + * @return the raw value of the X (forward) encoder in ticks + */ + public int getEncoderX(){return xEncoderValue; } + + /** + * @return the raw value of the Y (strafe) encoder in ticks + */ + public int getEncoderY(){return yEncoderValue; } + + /** + * @return the estimated X (forward) position of the robot in mm + */ + public double getPosX(){return xPosition; } + + /** + * @return the estimated Y (Strafe) position of the robot in mm + */ + public double getPosY(){return yPosition; } + + /** + * @return the estimated H (heading) position of the robot in Radians + */ + public double getHeading(){return hOrientation;} + + /** + * @return the estimated X (forward) velocity of the robot in mm/sec + */ + public double getVelX(){return xVelocity; } + + /** + * @return the estimated Y (strafe) velocity of the robot in mm/sec + */ + public double getVelY(){return yVelocity; } + + /** + * @return the estimated H (heading) velocity of the robot in radians/sec + */ + public double getHeadingVelocity(){return hVelocity; } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the X (forward) pod + */ + public float getXOffset(){return readFloat(Register.X_POD_OFFSET);} + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the Y (strafe) pod + */ + public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);} + + /** + * @return a Pose2D containing the estimated position of the robot + */ + public Pose2D getPosition(){ + return new Pose2D(DistanceUnit.MM, + xPosition, + yPosition, + AngleUnit.RADIANS, + hOrientation); + } + + + + /** + * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second + */ + public Pose2D getVelocity(){ + return new Pose2D(DistanceUnit.MM, + xVelocity, + yVelocity, + AngleUnit.RADIANS, + hVelocity); + } + + + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java new file mode 100644 index 0000000..64710ac --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -0,0 +1,105 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Localizer class. It is an abstract superclass of all localizers used in Pedro Pathing, + * so it contains abstract methods that will have a concrete implementation in the subclasses. Any + * method that all localizers will need will be in this class. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public abstract class Localizer { + + /** + * This returns the current pose estimate from the Localizer. + * + * @return returns the pose as a Pose object. + */ + public abstract Pose getPose(); + + /** + * This returns the current velocity estimate from the Localizer. + * + * @return returns the velocity as a Pose object. + */ + public abstract Pose getVelocity(); + + /** + * This returns the current velocity estimate from the Localizer as a Vector. + * + * @return returns the velocity as a Vector. + */ + public abstract Vector getVelocityVector(); + + /** + * This sets the start pose of the Localizer. Changing the start pose should move the robot as if + * all its previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + public abstract void setStartPose(Pose setStart); + + /** + * This sets the current pose estimate of the Localizer. Changing this should just change the + * robot's current pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + public abstract void setPose(Pose setPose); + + /** + * This calls an update to the Localizer, updating the current pose estimate and current velocity + * estimate. + */ + public abstract void update(); + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public abstract double getTotalHeading(); + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public abstract double getForwardMultiplier(); + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public abstract double getLateralMultiplier(); + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public abstract double getTurningMultiplier(); + + /** + * This resets the IMU of the localizer, if applicable. + */ + public abstract void resetIMU() throws InterruptedException; + + /** + * This is overridden to return the IMU, if there is one. + * + * @return returns the IMU if it exists + */ + public IMU getIMU() { + return null; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java new file mode 100644 index 0000000..c93ab7e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java @@ -0,0 +1,273 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import java.util.Arrays; + +/** + * This is the Matrix class. This defines matrices, primarily for use in the localizers. However, if + * matrices and matrix operations are necessary, this class as well as some operations in the + * MathFunctions class can absolutely be used there as well. It's similar to Mats in OpenCV if you've + * used them before, but with more limited functionality. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Matrix { + private double[][] matrix; + + /** + * This creates a new Matrix of width and height 0. + */ + public Matrix() { + matrix = new double[0][0]; + } + + /** + * This creates a new Matrix with a specified width and height. + * + * @param rows the number of rows, or height + * @param columns the number of columns, or width + */ + public Matrix(int rows, int columns) { + matrix = new double[rows][columns]; + } + + /** + * This creates a new Matrix from a 2D matrix of doubles. Please only enter rectangular 2D + * Arrays of doubles or else things mess up. + * + * @param setMatrix the 2D Array of doubles + */ + public Matrix(double[][] setMatrix) { + setMatrix(setMatrix); + } + + /** + * This creates a new Matrix from another Matrix. + * + * @param setMatrix the Matrix input. + */ + public Matrix(Matrix setMatrix) { + setMatrix(setMatrix); + } + + /** + * This creates a copy of a 2D Array of doubles that references entirely new memory locations + * from the original 2D Array of doubles, so no issues with mutability. + * + * @param copyMatrix the 2D Array of doubles to copy + * @return returns a deep copy of the input Array + */ + public static double[][] deepCopy(double[][] copyMatrix) { + double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length]; + for (int i = 0; i < copyMatrix.length; i++) { + returnMatrix[i] = Arrays.copyOf(copyMatrix[i], copyMatrix[i].length); + } + return returnMatrix; + } + + /** + * This returns a deep copy of the 2D Array that this Matrix is based on. + * + * @return returns the 2D Array of doubles this Matrix is built on + */ + public double[][] getMatrix() { + return deepCopy(matrix); + } + + /** + * This returns a specified row of the Matrix in the form of an Array of doubles. + * + * @param row the index of the row to return + * @return returns the row of the Matrix specified + */ + public double[] get(int row) { + return Arrays.copyOf(matrix[row], matrix[row].length); + } + + /** + * This returns a specified element of the Matrix as a double. + * + * @param row the index of the row of the element + * @param column the index of the column of the element + * @return returns the element of the Matrix specified + */ + public double get(int row, int column) { + return get(row)[column]; + } + + /** + * This returns the number of rows of the Matrix, as determined by the length of the 2D Array. + * If the Matrix/2D Array is not rectangular, issues arise. + * + * @return returns the number of rows in the Matrix + */ + public int getRows() { + return matrix.length; + } + + /** + * This returns the number of columns of the Matrix, as determined by the length of the first Array + * in the 2D Array. If the Matrix/2D Array is not rectangular, issues arise. + * + * @return returns the number of columns in the Matrix + */ + public int getColumns() { + return matrix[0].length; + } + + /** + * This sets the 2D Array of this Matrix to a copy of the 2D Array of another Matrix. + * + * @param setMatrix the Matrix to copy from + * @return returns if the operation was successful + */ + public boolean setMatrix(Matrix setMatrix) { + return setMatrix(setMatrix.getMatrix()); + } + + /** + * This sets the 2D Array of this Matrix to a copy of a specified 2D Array. + * + * @param setMatrix the 2D Array to copy from + * @return returns if the operation was successful + */ + public boolean setMatrix(double[][] setMatrix) { + int columns = setMatrix[0].length; + for (int i = 0; i < setMatrix.length; i++) { + if (setMatrix[i].length != columns) { + return false; + } + } + matrix = deepCopy(setMatrix); + return true; + } + + /** + * This sets a row of the Matrix to a copy of a specified Array of doubles. + * + * @param row the row to be written over + * @param input the Array input + * @return returns if the operation was successful + */ + public boolean set(int row, double[] input) { + if (input.length != getColumns()) { + return false; + } + matrix[row] = Arrays.copyOf(input, input.length); + return true; + } + + /** + * This sets a specified element of the Matrix to an input value. + * + * @param row the index of the row of the specified element + * @param column the index of the column of the specified element + * @param input the input value + * @return returns if the operation was successful + */ + public boolean set(int row, int column, double input) { + matrix[row][column] = input; + return true; + } + + /** + * This adds a Matrix to this Matrix. + * + * @param input the Matrix to add to this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ + public boolean add(Matrix input) { + if (input.getRows() == getRows() && input.getColumns() == getColumns()) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, get(i,j) + input.get(i,j)); + } + } + return true; + } + return false; + } + + /** + * This subtracts a Matrix from this Matrix. + * + * @param input the Matrix to subtract from this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ + public boolean subtract(Matrix input) { + if (input.getRows() == getRows() && input.getColumns() == getColumns()) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, get(i,j) - input.get(i,j)); + } + } + return true; + } + return false; + } + + /** + * This multiplies this Matrix with a scalar. + * + * @param scalar the scalar number + * @return returns if the operation was successful + */ + public boolean scalarMultiply(double scalar) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, scalar * get(i,j)); + } + } + return true; + } + + /** + * This multiplies the Matrix by -1, flipping the signs of all the elements within. + * + * @return returns if the operation was successful + */ + public boolean flipSigns() { + return scalarMultiply(-1); + } + + /** + * This multiplies a Matrix to this Matrix. + * + * @param input the Matrix to multiply to this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ + public boolean multiply(Matrix input) { + if (getColumns() == input.getRows()) { + Matrix product = new Matrix(getRows(), input.getColumns()); + for (int i = 0; i < product.getRows(); i++) { + for (int j = 0; j < product.getColumns(); j++) { + double value = 0; + for (int k = 0; k < get(i).length; k++) { + value += get(i, k) * input.get(k, j); + } + product.set(i, j, value); + } + } + setMatrix(product); + return true; + } + return false; + } + + /** + * This multiplies a Matrix to another Matrix. This will not change any data in the two input + * Matrices. + * + * @param one the first Matrix to multiply. + * @param two the second Matrix to multiply + * @return returns if the operation was successful + */ + public static Matrix multiply(Matrix one, Matrix two) { + Matrix returnMatrix = new Matrix(one); + if (returnMatrix.multiply(two)) { + return returnMatrix; + } else { + return new Matrix(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java new file mode 100644 index 0000000..d784c09 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -0,0 +1,219 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import androidx.annotation.NonNull; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Pose class. It defines poses in 2D space, like the Pose2D class in Road Runner except + * in the Pedro Pathing code so I don't have to import the Road Runner library. A Pose consists of + * two coordinates defining a position and a third value for the heading, so basically just defining + * any position and orientation the robot can be at, unless your robot can fly for whatever reason. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Pose { + private double x; + private double y; + private double heading; + + /** + * This creates a new Pose from a x, y, and heading inputs. + * + * @param setX the initial x value + * @param setY the initial y value + * @param setHeading the initial heading value + */ + public Pose(double setX, double setY, double setHeading) { + setX(setX); + setY(setY); + setHeading(setHeading); + } + + /** + * This creates a new Pose from x and y inputs. The heading is set to 0. + * + * @param setX the initial x value + * @param setY the initial y value + */ + public Pose(double setX, double setY) { + this(setX, setY, 0); + } + + /** + * This creates a new Pose with no inputs and 0 for all values. + */ + public Pose() { + this(0, 0, 0); + } + + /** + * This sets the x value. + * + * @param set the x value + */ + public void setX(double set) { + x = set; + } + + /** + * This sets the y value. + * + * @param set the y value + */ + public void setY(double set) { + y = set; + } + + /** + * This sets the heading value. + * + * @param set the heading value + */ + public void setHeading(double set) { + heading = MathFunctions.normalizeAngle(set); + } + + /** + * This returns the x value. + * + * @return returns the x value + */ + public double getX() { + return x; + } + + /** + * This returns the y value. + * + * @return returns the y value + */ + public double getY() { + return y; + } + + /** + * This returns the heading value. + * + * @return returns the heading value + */ + public double getHeading() { + return heading; + } + + /** + * This returns the Pose as a Vector. Naturally, the heading data in the Pose cannot be included + * in the Vector. + * + * @return returns the Pose as a Vector + */ + public Vector getVector() { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(x, y); + return returnVector; + } + + /** + * This returns a new Vector with magnitude 1 pointing in the direction of the heading. + * + * @return returns a unit Vector in the direction of the heading + */ + public Vector getHeadingVector() { + return new Vector(1, heading); + } + + /** + * This adds all the values of an input Pose to this Pose. The input Pose's data will not be + * changed. + * + * @param pose the input Pose + */ + public void add(Pose pose) { + setX(x + pose.getX()); + setY(y + pose.getY()); + setHeading(heading + pose.getHeading()); + } + + /** + * This subtracts all the values of an input Pose from this Pose. The input Pose's data will not + * be changed. + * + * @param pose the input Pose + */ + public void subtract(Pose pose) { + setX(x - pose.getX()); + setY(y - pose.getY()); + setHeading(heading - pose.getHeading()); + } + + /** + * This multiplies all the values of this Pose by a scalar. + * + * @param scalar the scalar + */ + public void scalarMultiply(double scalar) { + setX(x * scalar); + setY(y * scalar); + setHeading(heading * scalar); + } + + /** + * This divides all the values of this Pose by a scalar. + * + * @param scalar the scalar + */ + public void scalarDivide(double scalar) { + setX(x / scalar); + setY(y / scalar); + setHeading(heading / scalar); + } + + /** + * This flips the signs of all values in this Pose by multiplying them by -1. Heading values are + * still normalized to be between 0 and 2 * pi in value. + */ + public void flipSigns() { + setX(-x); + setY(-y); + setHeading(-heading); + } + + /** + * This returns if a Pose is within a specified accuracy of this Pose in terms of x position, + * y position, and heading. + * + * @param pose the input Pose to check + * @param accuracy the specified accuracy necessary to return true + * @return returns if the input Pose is within the specified accuracy of this Pose + */ + public boolean roughlyEquals(Pose pose, double accuracy) { + return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy); + } + + /** + * This checks if the input Pose is within 0.0001 in all values to this Pose. + * + * @param pose the input Pose + * @return returns if the input Pose is within 0.0001 of this Pose + */ + public boolean roughlyEquals(Pose pose) { + return roughlyEquals(pose, 0.0001); + } + + /** + * This creates a copy of this Pose that points to a new memory location. + * + * @return returns a deep copy of this Pose + */ + public Pose copy() { + return new Pose(getX(), getY(), getHeading()); + } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")"; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java new file mode 100644 index 0000000..b718320 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -0,0 +1,357 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the PoseUpdater class. This class handles getting pose data from the localizer and returning + * the information in a useful way to the Follower. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +public class PoseUpdater { + private HardwareMap hardwareMap; + + private IMU imu; + + private Localizer localizer; + + private Pose startingPose = new Pose(0,0,0); + + private Pose currentPose = startingPose; + + private Pose previousPose = startingPose; + + private Vector currentVelocity = new Vector(); + + private Vector previousVelocity = new Vector(); + + private Vector currentAcceleration = new Vector(); + + private double xOffset = 0; + private double yOffset = 0; + private double headingOffset = 0; + + private long previousPoseTime; + private long currentPoseTime; + + /** + * Creates a new PoseUpdater from a HardwareMap and a Localizer. + * + * @param hardwareMap the HardwareMap + * @param localizer the Localizer + */ + public PoseUpdater(HardwareMap hardwareMap, Localizer localizer) { + this.hardwareMap = hardwareMap; + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + this.localizer = localizer; + imu = localizer.getIMU(); + } + + /** + * Creates a new PoseUpdater from a HardwareMap. + * + * @param hardwareMap the HardwareMap + */ + public PoseUpdater(HardwareMap hardwareMap) { + // TODO: replace the second argument with your preferred localizer + this(hardwareMap, new ThreeWheelLocalizer(hardwareMap)); + } + + /** + * This updates the robot's pose, as well as updating the previous pose, velocity, and + * acceleration. The cache for the current pose, velocity, and acceleration is cleared, and + * the time stamps are updated as well. + */ + public void update() { + previousVelocity = getVelocity(); + previousPose = applyOffset(getRawPose()); + currentPose = null; + currentVelocity = null; + currentAcceleration = null; + previousPoseTime = currentPoseTime; + currentPoseTime = System.nanoTime(); + localizer.update(); + } + + /** + * This sets the starting pose. Do not run this after moving at all. + * + * @param set the Pose to set the starting pose to. + */ + public void setStartingPose(Pose set) { + startingPose = set; + previousPose = startingPose; + previousPoseTime = System.nanoTime(); + currentPoseTime = System.nanoTime(); + localizer.setStartPose(set); + } + + /** + * This sets the current pose, using offsets. Think of using offsets as setting trim in an + * aircraft. This can be reset as well, so beware of using the resetOffset() method. + * + * + * @param set The pose to set the current pose to. + */ + public void setCurrentPoseWithOffset(Pose set) { + Pose currentPose = getRawPose(); + setXOffset(set.getX() - currentPose.getX()); + setYOffset(set.getY() - currentPose.getY()); + setHeadingOffset(MathFunctions.getTurnDirection(currentPose.getHeading(), set.getHeading()) * MathFunctions.getSmallestAngleDifference(currentPose.getHeading(), set.getHeading())); + } + + /** + * This sets the offset for only the x position. + * + * @param offset This sets the offset. + */ + public void setXOffset(double offset) { + xOffset = offset; + } + + /** + * This sets the offset for only the y position. + * + * @param offset This sets the offset. + */ + public void setYOffset(double offset) { + yOffset = offset; + } + + /** + * This sets the offset for only the heading. + * + * @param offset This sets the offset. + */ + public void setHeadingOffset(double offset) { + headingOffset = offset; + } + + /** + * This returns the x offset. + * + * @return returns the x offset. + */ + public double getXOffset() { + return xOffset; + } + + /** + * This returns the y offset. + * + * @return returns the y offset. + */ + public double getYOffset() { + return yOffset; + } + + /** + * This returns the heading offset. + * + * @return returns the heading offset. + */ + public double getHeadingOffset() { + return headingOffset; + } + + /** + * This applies the offset to a specified Pose. + * + * @param pose The pose to be offset. + * @return This returns a new Pose with the offset applied. + */ + public Pose applyOffset(Pose pose) { + return new Pose(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset); + } + + /** + * This resets all offsets set to the PoseUpdater. If you have reset your pose using the + * setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the + * PoseUpdater thinks your pose would be, not the pose you reset to. + */ + public void resetOffset() { + setXOffset(0); + setYOffset(0); + setHeadingOffset(0); + } + + /** + * This returns the current pose, with offsets applied. If this is called multiple times in + * a single update, the current pose is cached so that subsequent calls don't have to repeat + * localizer calls or calculations. + * + * @return returns the current pose. + */ + public Pose getPose() { + if (currentPose == null) { + currentPose = localizer.getPose(); + return applyOffset(currentPose); + } else { + return applyOffset(currentPose); + } + } + + /** + * This returns the current raw pose, without any offsets applied. If this is called multiple times in + * a single update, the current pose is cached so that subsequent calls don't have to repeat + * localizer calls or calculations. + * + * @return returns the raw pose. + */ + public Pose getRawPose() { + if (currentPose == null) { + currentPose = localizer.getPose(); + return currentPose; + } else { + return currentPose; + } + } + + /** + * This sets the current pose without using resettable offsets. + * + * @param set the pose to set the current pose to. + */ + public void setPose(Pose set) { + resetOffset(); + localizer.setPose(set); + } + + /** + * Returns the robot's pose from the previous update. + * + * @return returns the robot's previous pose. + */ + public Pose getPreviousPose() { + return previousPose; + } + + /** + * Returns the robot's change in pose from the previous update. + * + * @return returns the robot's delta pose. + */ + public Pose getDeltaPose() { + Pose returnPose = getPose(); + returnPose.subtract(previousPose); + return returnPose; + } + + /** + * This returns the velocity of the robot as a Vector. If this is called multiple times in + * a single update, the velocity Vector is cached so that subsequent calls don't have to repeat + * localizer calls or calculations. + * + * @return returns the velocity of the robot. + */ + public Vector getVelocity() { + if (currentVelocity == null) { +// currentVelocity = new Vector(); +// currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); +// currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); + currentVelocity = localizer.getVelocityVector(); + return MathFunctions.copyVector(currentVelocity); + } else { + return MathFunctions.copyVector(currentVelocity); + } + } + + /** + * This returns the angular velocity of the robot as a double. + * + * @return returns the angular velocity of the robot. + */ + public double getAngularVelocity() { + return MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(getPose().getHeading(), previousPose.getHeading()) / ((currentPoseTime-previousPoseTime)/Math.pow(10.0, 9)); + } + + /** + * This returns the acceleration of the robot as a Vector. If this is called multiple times in + * a single update, the acceleration Vector is cached so that subsequent calls don't have to + * repeat localizer calls or calculations. + * + * @return returns the acceleration of the robot. + */ + public Vector getAcceleration() { + if (currentAcceleration == null) { + currentAcceleration = MathFunctions.subtractVectors(getVelocity(), previousVelocity); + currentAcceleration.setMagnitude(currentAcceleration.getMagnitude() / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); + return MathFunctions.copyVector(currentAcceleration); + } else { + return MathFunctions.copyVector(currentAcceleration); + } + } + + /** + * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. + */ + public void resetHeadingToIMU() { + if (imu != null) { + localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } + } + + /** + * This resets the heading of the robot to the IMU's heading, using offsets instead of Road + * Runner's pose reset. This way, it's faster, but this can be wiped with the resetOffsets() + * method. + */ + public void resetHeadingToIMUWithOffsets() { + if (imu != null) { + setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } + } + + /** + * This returns the IMU heading normalized to be between [0, 2 PI] radians. + * + * @return returns the normalized IMU heading. + */ + public double getNormalizedIMUHeading() { + if (imu != null) { + return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + } + return 0; + } + + /** + * This returns the total number of radians the robot has turned. + * + * @return the total heading. + */ + public double getTotalHeading() { + return localizer.getTotalHeading(); + } + + /** + * This returns the Localizer. + * + * @return the Localizer + */ + public Localizer getLocalizer() { + return localizer; + } + + /** + * + */ + public void resetIMU() throws InterruptedException { + localizer.resetIMU(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt new file mode 100644 index 0000000..76d825c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt @@ -0,0 +1,46 @@ +package com.acmerobotics.roadrunner.ftc + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS +import com.qualcomm.robotcore.hardware.I2cDeviceSynch +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType +import java.util.* +@I2cDeviceType +@DeviceProperties( + name = "SparkFun OTOS Corrected", + xmlTag = "SparkFunOTOS2", + description = "SparkFun Qwiic Optical Tracking Odometry Sensor Corrected" +) +class SparkFunOTOSCorrected(deviceClient: I2cDeviceSynch) : SparkFunOTOS(deviceClient) { + /** + * Gets only the position and velocity measured by the + * OTOS in a single burst read + * @param pos Position measured by the OTOS + * @param vel Velocity measured by the OTOS + */ + fun getPosVel(pos: Pose2D, vel: Pose2D) { + // Read all pose registers + val rawData = deviceClient.read(REG_POS_XL.toInt(), 12) + + // Convert raw data to pose units + pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD)) + vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS)) + } + + // Modified version of poseToRegs to fix pose setting issue + // see https://discord.com/channels/225450307654647808/1246977443030368349/1271702497659977760 + override fun poseToRegs(rawData: ByteArray, pose: Pose2D, xyToRaw: Double, hToRaw: Double) { + // Convert pose units to raw data + val rawX = (_distanceUnit.toMeters(pose.x) * xyToRaw).toInt().toShort() + val rawY = (_distanceUnit.toMeters(pose.y) * xyToRaw).toInt().toShort() + val rawH = (_angularUnit.toRadians(pose.h) * hToRaw).toInt().toShort() + + // Store raw data in buffer + rawData[0] = (rawX.toInt() and 0xFF).toByte() + rawData[1] = ((rawX.toInt() shr 8) and 0xFF).toByte() + rawData[2] = (rawY.toInt() and 0xFF).toByte() + rawData[3] = ((rawY.toInt() shr 8) and 0xFF).toByte() + rawData[4] = (rawH.toInt() and 0xFF).toByte() + rawData[5] = ((rawH.toInt() shr 8) and 0xFF).toByte() + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java new file mode 100644 index 0000000..5f3290a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -0,0 +1,272 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the drive encoder set up. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +@Config +public class DriveEncoderLocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder leftFront; + private Encoder rightFront; + private Encoder leftRear; + private Encoder rightRear; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 1; + public static double STRAFE_TICKS_TO_INCHES = 1; + public static double TURN_TICKS_TO_RADIANS = 1; + public static double ROBOT_WIDTH = 1; + public static double ROBOT_LENGTH = 1; + + /** + * This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public DriveEncoderLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { + hardwareMap = map; + + leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName)); + leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName)); + rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName)); + rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName)); + + // TODO: reverse any encoders necessary + leftFront.setDirection(Encoder.REVERSE); + leftRear.setDirection(Encoder.REVERSE); + rightFront.setDirection(Encoder.FORWARD); + rightRear.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders. + */ + public void updateEncoders() { + leftFront.update(); + rightFront.update(); + leftRear.update(); + rightRear.update(); + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + leftFront.reset(); + rightFront.reset(); + leftRear.reset(); + rightRear.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition())); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition())); + // theta/turning + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH))); + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; + } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java new file mode 100644 index 0000000..0f309a5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -0,0 +1,227 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the SparkFun OTOS. The diagram below, which is modified from + * Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/20/2024 + */ +public class OTOSLocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private SparkFunOTOS otos; + private SparkFunOTOS.Pose2D otosPose; + private SparkFunOTOS.Pose2D otosVel; + private SparkFunOTOS.Pose2D otosAcc; + private double previousHeading; + private double totalHeading; + + /** + * This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public OTOSLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public OTOSLocalizer(HardwareMap map, Pose setStartPose) { + hardwareMap = map; + + /* + TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the + 'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a + "SparkFunOTOS Corrected" in your robot config + */ + // TODO: replace this with your OTOS port + otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); + + otos.setLinearUnit(DistanceUnit.INCH); + otos.setAngularUnit(AngleUnit.RADIANS); + + // TODO: replace this with your OTOS offset from the center of the robot + // For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being + // positive y and forward being positive x. PI/2 radians is facing forward, and clockwise + // rotation is negative rotation. + otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2)); + + // TODO: replace these with your tuned multipliers + otos.setLinearScalar(1.0); + otos.setAngularScalar(1.0); + + otos.calibrateImu(); + otos.resetTracking(); + + setStartPose(setStartPose); + otosPose = new SparkFunOTOS.Pose2D(); + otosVel = new SparkFunOTOS.Pose2D(); + otosAcc = new SparkFunOTOS.Pose2D(); + totalHeading = 0; + previousHeading = startPose.getHeading(); + + resetOTOS(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.h); + + Vector vec = pose.getVector(); + vec.rotateVector(startPose.getHeading()); + + return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return new Pose(otosVel.x, otosVel.y, otosVel.h); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return getVelocity().getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + resetOTOS(); + Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose); + otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading())); + } + + /** + * This updates the total heading of the robot. The OTOS handles all other updates itself. + */ + @Override + public void update() { + otos.getPosVelAcc(otosPose,otosVel,otosAcc); + totalHeading += MathFunctions.getSmallestAngleDifference(otosPose.h, previousHeading); + previousHeading = otosPose.h; + } + + /** + * This resets the OTOS. + */ + public void resetOTOS() { + otos.resetTracking(); + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from OTOS + * ticks to inches. For the OTOS, this value is the same as the lateral multiplier. + * This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return otos.getLinearScalar(); + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier. + * This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return otos.getLinearScalar(); + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from OTOS ticks + * to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return otos.getAngularScalar(); + } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java new file mode 100644 index 0000000..73272b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -0,0 +1,220 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; +// +// +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +//import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; +//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +// +///** +// * This is the Pinpoint class. This class extends the Localizer superclass and is a +// * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading +// * readings. The diagram below, which is modified from Road Runner, shows a typical set up. +// * +// * The view is from the top of the robot looking downwards. +// * +// * left on robot is the y positive direction +// * +// * forward on robot is the x positive direction +// * +// * forward (x positive) +// * △ +// * | +// * | +// * /--------------\ +// * | | +// * | | +// * | || | +// * left (y positive) <--- | || | +// * | ____ | +// * | ---- | +// * \--------------/ +// * With the pinpoint your readings will be used in mm +// * to use inches ensure to divide your mm value by 25.4 +// * @author Logan Nash +// * @author Havish Sripada 12808 - RevAmped Robotics +// * @author Ethan Doak - Gobilda +// * @version 1.0, 10/2/2024 +// */ +//public class PinpointLocalizer extends Localizer { +// private HardwareMap hardwareMap; +// private GoBildaPinpointDriver odo; +// private double previousHeading; +// private double totalHeading; +// +// /** +// * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) +// * facing 0 heading. +// * +// * @param map the HardwareMap +// */ +// public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} +// +// /** +// * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose +// * specifying the starting pose of the localizer. +// * +// * @param map the HardwareMap +// * @param setStartPose the Pose to start from +// */ +// public PinpointLocalizer(HardwareMap map, Pose setStartPose){ +// hardwareMap = map; +// // TODO: replace this with your Pinpoint port +// odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); +// +// //This uses mm, to use inches divide these numbers by 25.4 +// odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 +// //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here +// // odo.setYawScalar(1.0); +// //TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included. +// //TODO: If you would like to use your own odometry pods input the ticks per mm in the commented part below +// odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); +// //odo.setEncoderResolution(13.26291192); +// //TODO: Set encoder directions +// odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); +// +// resetPinpoint();; + +// setStartPose(setStartPose); +// totalHeading = 0; +// previousHeading = setStartPose.getHeading(); +// } +// +// /** +// * This returns the current pose estimate. +// * +// * @return returns the current pose estimate as a Pose +// */ +// @Override +// public Pose getPose() { +// Pose2D rawPose = odo.getPosition(); +// return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); +// } +// +// /** +// * This returns the current velocity estimate. +// * +// * @return returns the current velocity estimate as a Pose +// */ +// @Override +// public Pose getVelocity() { +// Pose2D pose = odo.getVelocity(); +// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), odo.getHeadingVelocity()); +// } +// +// /** +// * This returns the current velocity estimate. +// * +// * @return returns the current velocity estimate as a Vector +// */ +// @Override +// public Vector getVelocityVector() { +// Pose2D pose = odo.getVelocity(); +// Vector returnVector = new Vector(); +// returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH)); +// return returnVector; +// } +// +// /** +// * This sets the start pose. Since nobody should be using this after the robot has begun moving, +// * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). +// * +// * @param setStart the new start pose +// */ +// @Override +// public void setStartPose(Pose setStart) { +// odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading())); +// } +// +// /** +// * This sets the current pose estimate. Changing this should just change the robot's current +// * pose estimate, not anything to do with the start pose. +// * +// * @param setPose the new current pose estimate +// */ +// @Override +// public void setPose(Pose setPose) { +// odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); +// } +// +// /** +// * This updates the total heading of the robot. The Pinpoint handles all other updates itself. +// */ +// @Override +// public void update() { +// odo.update(); +// totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading); +// previousHeading = MathFunctions.normalizeAngle(odo.getHeading()); +// } +// +// /** +// * This returns how far the robot has turned in radians, in a number not clamped between 0 and +// * 2 * pi radians. This is used for some tuning things and nothing actually within the following. +// * +// * @return returns how far the robot has turned in total, in radians. +// */ +// @Override +// public double getTotalHeading() { +// return totalHeading; +// } +// +// /** +// * This returns the Y encoder value as none of the odometry tuners are required for this localizer +// * @return returns the Y encoder value +// */ +// @Override +// public double getForwardMultiplier() { +// return odo.getEncoderY(); +// } +// +// /** +// * This returns the X encoder value as none of the odometry tuners are required for this localizer +// * @return returns the X encoder value +// */ +// @Override +// public double getLateralMultiplier() { +// return odo.getEncoderX(); +// } +// +// /** +// * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. +// * @return returns the yaw scalar +// */ +// @Override +// public double getTurningMultiplier() { +// return odo.getYawScalar(); +// } +// +// /** +// * This resets the IMU. Note: This does not change the estimated heading orientation. +// */ +// @Override +// public void resetIMU() throws InterruptedException { +// odo.recalibrateIMU(); +// +// try { +// Thread.sleep(300); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } +// } +// +// /** +// * This resets the pinpoint. +// */ +// private void resetPinpoint() { +// odo.resetPosAndIMU(); +// +// try { +// Thread.sleep(300); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java new file mode 100644 index 0000000..15275f0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java @@ -0,0 +1,159 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +// +//import java.util.ArrayList; +//import java.util.List; +// +///** +// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and +// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing +// * localizer system. +// * +// * @author Anyi Lin - 10158 Scott's Bots +// * @version 1.0, 5/9/2024 +// */ +//public class RRToPedroThreeWheelLocalizer extends Localizer { +// private RoadRunnerThreeWheelLocalizer localizer; +// private double totalHeading; +// private Pose startPose; +// private Pose previousPose; +// +// /** +// * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously +// * used Road Runner localization system to the new Pedro Pathing localization system. +// * +// * @param hardwareMap the HardwareMap +// */ +// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { +// List lastTrackingEncPositions = new ArrayList<>(); +// List lastTrackingEncVels = new ArrayList<>(); +// +// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); +// +// startPose = new Pose(); +// previousPose = new Pose(); +// } +// +// /** +// * This returns the current pose estimate as a Pose. +// * +// * @return returns the current pose estimate +// */ +// @Override +// public Pose getPose() { +// Pose2d pose = localizer.getPoseEstimate(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// /** +// * This returns the current velocity estimate as a Pose. +// * +// * @return returns the current velocity estimate +// */ +// @Override +// public Pose getVelocity() { +// Pose2d pose = localizer.getPoseVelocity(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// /** +// * This returns the current velocity estimate as a Vector. +// * +// * @return returns the current velocity estimate +// */ +// @Override +// public Vector getVelocityVector() { +// Pose2d pose = localizer.getPoseVelocity(); +// Vector returnVector = new Vector(); +// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); +// return returnVector; +// } +// +// /** +// * This sets the start pose. Any movement of the robot is treated as a displacement from the +// * start pose, so moving the start pose will move the current pose estimate the same amount. +// * +// * @param setStart the new start pose +// */ +// @Override +// public void setStartPose(Pose setStart) { +// Pose oldStart = startPose; +// startPose = setStart; +// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); +// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); +// } +// +// /** +// * This sets the current pose estimate. This has no effect on the start pose. +// * +// * @param setPose the new current pose estimate +// */ +// @Override +// public void setPose(Pose setPose) { +// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); +// } +// +// /** +// * This updates the total heading and previous pose estimate. Everything else is handled by the +// * Road Runner localizer on its own, but updating this tells you how far the robot has really +// * turned. +// */ +// @Override +// public void update() { +// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); +// previousPose = getPose(); +// } +// +// /** +// * This returns how far the robot has actually turned. +// * +// * @return returns the total angle turned, in degrees. +// */ +// @Override +// public double getTotalHeading() { +// return totalHeading; +// } +// +// /** +// * This returns the forward multiplier of the Road Runner localizer, which converts from ticks +// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything +// * multiplied together should be. If you do use that, then do be aware that the value returned is +// * the product of the Road Runner ticks to inches and the x multiplier. +// * +// * @return returns the forward multiplier +// */ +// @Override +// public double getForwardMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; +// } +// +// /** +// * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks +// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything +// * multiplied together should be. If you do use that, then do be aware that the value returned is +// * the product of the Road Runner ticks to inches and the y multiplier. +// * +// * @return returns the lateral multiplier +// */ +// @Override +// public double getLateralMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; +// } +// +// /** +// * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. +// * There really isn't a point in tuning the turning for the Road Runner localizer. This will +// * actually just return the average of the two other multipliers. +// * +// * @return returns the turning multiplier +// */ +// @Override +// public double getTurningMultiplier() { +// return (getForwardMultiplier() + getLateralMultiplier()) / 2; +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java new file mode 100644 index 0000000..d743b49 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java @@ -0,0 +1,132 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.util.NanoClock; +//import com.qualcomm.robotcore.hardware.DcMotorEx; +//import com.qualcomm.robotcore.hardware.DcMotorSimple; +// +///** +// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a +// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected +// * velocity counts and allow reversing independently of the corresponding slot's motor direction. +// * +// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have +// * import statements, so I'm not crediting myself as an author for this. +// * +// * @author Road Runner dev team +// * @version 1.0, 5/9/2024 +// */ +//public class RoadRunnerEncoder { +// private final static int CPS_STEP = 0x10000; +// +// private static double inverseOverflow(double input, double estimate) { +// // convert to uint16 +// int real = (int) input & 0xffff; +// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits +// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window +// real += ((real % 20) / 4) * CPS_STEP; +// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by +// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; +// return real; +// } +// +// public enum Direction { +// FORWARD(1), +// REVERSE(-1); +// +// private int multiplier; +// +// Direction(int multiplier) { +// this.multiplier = multiplier; +// } +// +// public int getMultiplier() { +// return multiplier; +// } +// } +// +// private DcMotorEx motor; +// private NanoClock clock; +// +// private Direction direction; +// +// private int lastPosition; +// private int velocityEstimateIdx; +// private double[] velocityEstimates; +// private double lastUpdateTime; +// +// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { +// this.motor = motor; +// this.clock = clock; +// +// this.direction = Direction.FORWARD; +// +// this.lastPosition = 0; +// this.velocityEstimates = new double[3]; +// this.lastUpdateTime = clock.seconds(); +// } +// +// public RoadRunnerEncoder(DcMotorEx motor) { +// this(motor, NanoClock.system()); +// } +// +// public Direction getDirection() { +// return direction; +// } +// +// private int getMultiplier() { +// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); +// } +// +// /** +// * 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 +// */ +// public void setDirection(Direction direction) { +// this.direction = direction; +// } +// +// /** +// * Gets the position from the underlying motor and adjusts for the set direction. +// * Additionally, this method updates the velocity estimates used for compensated velocity +// * +// * @return encoder position +// */ +// public int getCurrentPosition() { +// int multiplier = getMultiplier(); +// int currentPosition = motor.getCurrentPosition() * multiplier; +// if (currentPosition != lastPosition) { +// double currentTime = clock.seconds(); +// double dt = currentTime - lastUpdateTime; +// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; +// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; +// lastPosition = currentPosition; +// lastUpdateTime = currentTime; +// } +// return currentPosition; +// } +// +// /** +// * Gets the velocity directly from the underlying motor and compensates for the direction +// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) +// * +// * @return raw velocity +// */ +// public double getRawVelocity() { +// int multiplier = getMultiplier(); +// return motor.getVelocity() * multiplier; +// } +// +// /** +// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity +// * that are lost in overflow due to velocity being transmitted as 16 bits. +// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. +// * +// * @return corrected velocity +// */ +// public double getCorrectedVelocity() { +// double median = velocityEstimates[0] > velocityEstimates[1] +// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) +// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); +// return inverseOverflow(getRawVelocity(), median); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java new file mode 100644 index 0000000..2ee75a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java @@ -0,0 +1,123 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import androidx.annotation.NonNull; +// +//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 java.util.Arrays; +//import java.util.List; +// +///* +// * Sample tracking wheel localizer implementation assuming the standard configuration: +// * +// * left on robot is y pos +// * +// * front of robot is x pos +// * +// * /--------------\ +// * | ____ | +// * | ---- | +// * | || || | +// * | || || | +// * | | +// * | | +// * \--------------/ +// * +// */ +// +///** +// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will +// * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an +// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to +// * Pedro Pathing to avoid having imports. +// * +// * @author Road Runner dev team +// * @author Anyi Lin - 10158 Scott's Bots +// * @version 1.0, 5/9/2024 +// */ +//@Config +//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { +// public static double TICKS_PER_REV = 8192; +// public static double WHEEL_RADIUS = 1.37795; // in +// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed +// +// public static double X_MULTIPLIER = 0.5008239963; +// public static double Y_MULTIPLIER = 0.5018874659; +// +// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; +// +// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; +// +// private List lastEncPositions, lastEncVels; +// +// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { +// super(Arrays.asList( +// new Pose2d(leftX, leftY, 0), // left +// new Pose2d(rightX, rightY, 0), // right +// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe +// )); +// +// lastEncPositions = lastTrackingEncPositions; +// lastEncVels = lastTrackingEncVels; +// +// // TODO: redo the configs here +// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); +// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); +// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); +// +// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) +// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); +// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); +// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); +// } +// +// public void resetHeading(double heading) { +// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); +// } +// +// public static double encoderTicksToInches(double ticks) { +// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; +// } +// +// @NonNull +// @Override +// public List getWheelPositions() { +// int leftPos = leftEncoder.getCurrentPosition(); +// int rightPos = rightEncoder.getCurrentPosition(); +// int frontPos = strafeEncoder.getCurrentPosition(); +// +// lastEncPositions.clear(); +// lastEncPositions.add(leftPos); +// lastEncPositions.add(rightPos); +// lastEncPositions.add(frontPos); +// +// return Arrays.asList( +// encoderTicksToInches(leftPos) * X_MULTIPLIER, +// encoderTicksToInches(rightPos) * X_MULTIPLIER, +// encoderTicksToInches(frontPos) * Y_MULTIPLIER +// ); +// } +// +// @NonNull +// @Override +// public List getWheelVelocities() { +// int leftVel = (int) leftEncoder.getCorrectedVelocity(); +// int rightVel = (int) rightEncoder.getCorrectedVelocity(); +// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); +// +// lastEncVels.clear(); +// lastEncVels.add(leftVel); +// lastEncVels.add(rightVel); +// lastEncVels.add(frontVel); +// +// return Arrays.asList( +// encoderTicksToInches(leftVel) * X_MULTIPLIER, +// encoderTicksToInches(rightVel) * X_MULTIPLIER, +// encoderTicksToInches(frontVel) * Y_MULTIPLIER +// ); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java new file mode 100644 index 0000000..83c172e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -0,0 +1,326 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the three wheel odometry set up with the IMU to have more accurate heading + * readings. The diagram below, which is modified from Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ + * + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/9/2024 + */ +@Config +public class ThreeWheelIMULocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder leftEncoder; + private Encoder rightEncoder; + private Encoder strafeEncoder; + private Pose leftEncoderPose; + private Pose rightEncoderPose; + private Pose strafeEncoderPose; + + public final IMU imu; + private double previousIMUOrientation; + private double deltaRadians; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5; + + public static boolean useIMU = true; + + /** + * This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public ThreeWheelIMULocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) { + hardwareMap = map; + imu = hardwareMap.get(IMU.class, "imu"); + + // TODO: replace this with your IMU's orientation + imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP))); + + // TODO: replace these with your encoder positions + leftEncoderPose = new Pose(-3, 5.7, 0); + rightEncoderPose = new Pose(-3, -5.7, 0); + strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90)); + + + // TODO: replace these with your encoder ports + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rf")); + + // TODO: reverse any encoders necessary + leftEncoder.setDirection(Encoder.REVERSE); + rightEncoder.setDirection(Encoder.FORWARD); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + totalHeading = 0; + + resetEncoders(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders. + */ + public void updateEncoders() { + leftEncoder.update(); + rightEncoder.update(); + strafeEncoder.update(); + + double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); + previousIMUOrientation = currentIMUOrientation; + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + leftEncoder.reset(); + rightEncoder.reset(); + strafeEncoder.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); + // theta/turning + if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) { + returnMatrix.set(2, 0, deltaRadians); + } else { + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + } + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; + } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } + + /** + * This is returns the IMU. + * + * @return returns the IMU + */ + @Override + public IMU getIMU() { + return imu; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java new file mode 100644 index 0000000..960a366 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -0,0 +1,292 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the three wheel odometry set up. The diagram below, which is modified from + * Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +@Config +public class ThreeWheelLocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder leftEncoder; + private Encoder rightEncoder; + private Encoder strafeEncoder; + private Pose leftEncoderPose; + private Pose rightEncoderPose; + private Pose strafeEncoderPose; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5; + + /** + * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public ThreeWheelLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new ThreeWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0); + strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + // TODO: replace these with your encoder ports + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + leftEncoder.setDirection(Encoder.REVERSE); + rightEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + totalHeading = 0; + + resetEncoders(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders. + */ + public void updateEncoders() { + leftEncoder.update(); + rightEncoder.update(); + strafeEncoder.update(); + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + leftEncoder.reset(); + rightEncoder.reset(); + strafeEncoder.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); + // theta/turning + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; + } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java new file mode 100644 index 0000000..72cc63b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -0,0 +1,309 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from + * Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * +* forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +@Config +public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work + private HardwareMap hardwareMap; + private IMU imu; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder forwardEncoder; + private Encoder strafeEncoder; + private Pose forwardEncoderPose; + private Pose strafeEncoderPose; + private double previousIMUOrientation; + private double deltaRadians; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + + /** + * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public TwoWheelLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + imu = hardwareMap.get(IMU.class, "imu"); + // TODO: replace this with your IMU's orientation + imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT))); + + // TODO: replace these with your encoder ports + forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + forwardEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + + previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + deltaRadians = 0; + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders and the IMU readings. Then, the robot's global change in + * position is calculated using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders as well as the IMU. + */ + public void updateEncoders() { + forwardEncoder.update(); + strafeEncoder.update(); + + double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); + previousIMUOrientation = currentIMUOrientation; + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + forwardEncoder.reset(); + strafeEncoder.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders and IMU. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); + // theta/turning + returnMatrix.set(2,0, deltaRadians); + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return 1; + } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } + + /** + * This is returns the IMU. + * + * @return returns the IMU + */ + @Override + public IMU getIMU() { + return imu; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java new file mode 100644 index 0000000..87849b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -0,0 +1,321 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + + +/** + * This is the TwoWheelPinpointIMULocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry with the Pinpoint IMU set up. The diagram below, which is modified from + * Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * +* forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Havish Sripada - 12808 RevAmped Robotics + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 11/28/2024 + */ +@Config +public class TwoWheelPinpointIMULocalizer extends Localizer { + private HardwareMap hardwareMap; + private GoBildaPinpointDriver pinpoint; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder forwardEncoder; + private Encoder strafeEncoder; + private Pose forwardEncoderPose; + private Pose strafeEncoderPose; + private double previousHeading; + private double deltaRadians; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + + /** + * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public TwoWheelPinpointIMULocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + pinpoint.resetPosAndIMU(); + + // TODO: replace these with your encoder ports + forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + forwardEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + previousHeading = startPose.getHeading(); + deltaRadians = 0; + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return new Pose(startPose.getX()+displacementPose.getX(), startPose.getY()+displacementPose.getY(),displacementPose.getHeading()); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3, 3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders and the IMU readings. Then, the robot's global change in + * position is calculated using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3, 3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders as well as the IMU. + */ + public void updateEncoders() { + forwardEncoder.update(); + strafeEncoder.update(); + + pinpoint.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); + double currentHeading = startPose.getHeading() + MathFunctions.normalizeAngle(pinpoint.getHeading()); + deltaRadians = MathFunctions.getTurnDirection(previousHeading, currentHeading) * MathFunctions.getSmallestAngleDifference(currentHeading, previousHeading); + previousHeading = currentHeading; + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + forwardEncoder.reset(); + strafeEncoder.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders and IMU. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3, 1); + // x/forward movement + returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); + //y/strafe movement + returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); + // theta/turning + returnMatrix.set(2, 0, deltaRadians); + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return 1; + } + + /** + * This resets the IMU. + */ + + @Override + public void resetIMU() throws InterruptedException { + pinpoint.recalibrateIMU(); + + try { + Thread.sleep(300); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + + /** + * This resets the pinpoint. + */ + private void resetPinpoint() throws InterruptedException{ + pinpoint.resetPosAndIMU(); + + try { + Thread.sleep(300); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java new file mode 100644 index 0000000..d876746 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") +public class ForwardTuner extends OpMode { + private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; + + private Telemetry telemetryA; + + public static double DISTANCE = 48; + + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } + + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + poseUpdater.update(); + + telemetryA.addData("distance moved", poseUpdater.getPose().getX()); + telemetryA.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getForwardMultiplier())); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java new file mode 100644 index 0000000..dfa39dc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning") +public class LateralTuner extends OpMode { + private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; + + private Telemetry telemetryA; + + public static double DISTANCE = 48; + + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } + + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + poseUpdater.update(); + + telemetryA.addData("distance moved", poseUpdater.getPose().getY()); + telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier())); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java new file mode 100644 index 0000000..8da3c3e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; + +import java.util.Arrays; +import java.util.List; + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot + * on FTC Dashboard (192/168/43/1:8080/dash). You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test") +public class LocalizationTest extends OpMode { + private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; + private Telemetry telemetryA; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + /** + * This initializes the PoseUpdater, the mecanum drive motors, and the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the FTC + * Dashboard telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + poseUpdater.update(); + dashboardPoseTracker.update(); + + double y = -gamepad1.left_stick_y; // Remember, this is reversed! + double x = gamepad1.left_stick_x; // this is strafing + double rx = gamepad1.right_stick_x; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double leftFrontPower = (y + x + rx) / denominator; + double leftRearPower = (y - x + rx) / denominator; + double rightFrontPower = (y - x - rx) / denominator; + double rightRearPower = (y + x - rx) / denominator; + + leftFront.setPower(leftFrontPower); + leftRear.setPower(leftRearPower); + rightFront.setPower(rightFrontPower); + rightRear.setPower(rightRearPower); + + telemetryA.addData("x", poseUpdater.getPose().getX()); + telemetryA.addData("y", poseUpdater.getPose().getY()); + telemetryA.addData("heading", poseUpdater.getPose().getHeading()); + telemetryA.addData("total heading", poseUpdater.getTotalHeading()); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java similarity index 78% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SensorGoBildaPinpointExample.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java index 1c6a981..9fbac95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java @@ -20,15 +20,17 @@ * SOFTWARE. */ -package org.firstinspires.ftc.teamcode.roadrunner; +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver; -import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; + import java.util.Locale; /* @@ -57,11 +59,13 @@ -Ethan Doak */ -@TeleOp(name = "goBILDA® PinPoint Odometry Example", group = "Linear OpMode") -//@Disabled +//TODO: If tuning comment out the @Disabled +@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode") +@Disabled + public class SensorGoBildaPinpointExample extends LinearOpMode { - GoBildaPinpointDriverRR odo; // Declare OpMode member for the Odometry Computer + GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer double oldTime = 0; @@ -72,7 +76,7 @@ public void runOpMode() { // Initialize the hardware variables. Note that the strings used here must correspond // to the names assigned during the robot configuration step on the DS or RC devices. - odo = hardwareMap.get(GoBildaPinpointDriverRR.class, "pinpoint"); + odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); /* Set the odometry pod positions relative to the point that the odometry computer tracks around. @@ -117,7 +121,7 @@ increase when you move the robot forward. And the Y (strafe) pod should increase telemetry.addData("X offset", odo.getXOffset()); telemetry.addData("Y offset", odo.getYOffset()); telemetry.addData("Device Version Number:", odo.getDeviceVersion()); - telemetry.addData("Device SCalar", odo.getYawScalar()); + telemetry.addData("Device Scalar", odo.getYawScalar()); telemetry.update(); // Wait for the game to start (driver presses START) @@ -129,50 +133,52 @@ increase when you move the robot forward. And the Y (strafe) pod should increase while (opModeIsActive()) { /* - Request a bulk update from the Pinpoint odometry computer. This checks almost all outputs + Request an update from the Pinpoint odometry computer. This checks almost all outputs from the device in a single I2C read. */ odo.update(); + /* + Optionally, you can update only the heading of the device. This takes less time to read, but will not + pull any other data. Only the heading (which you can pull with getHeading() or in getPosition(). + */ + //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); + - if (gamepad1.a) { + if (gamepad1.a){ odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU } - if (gamepad1.b) { + if (gamepad1.b){ odo.recalibrateIMU(); //recalibrates the IMU without resetting position } /* This code prints the loop frequency of the REV Control Hub. This frequency is effected - by I2C reads/writes. So it's good to keep an eye on. This code calculates the amount + by I²C reads/writes. So it's good to keep an eye on. This code calculates the amount of time each cycle takes and finds the frequency (number of updates per second) from that cycle time. */ double newTime = getRuntime(); - double loopTime = newTime - oldTime; - double frequency = 1 / loopTime; + double loopTime = newTime-oldTime; + double frequency = 1/loopTime; oldTime = newTime; /* - gets the current Position (x & y in inches, and heading in radians) of the robot, and prints it. + gets the current Position (x & y in mm, and heading in degrees) of the robot, and prints it. */ - Pose2d pos = odo.getPositionRR(); - String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.position.x, pos.position.y, pos.heading.toDouble()); + Pose2D pos = odo.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); - /* - gets the current Velocity (x & y in inches/sec and heading in radians/sec) and prints it. + gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it. */ - PoseVelocity2d vel = odo.getVelocityRR(); - String velocity = String.format(Locale.US, "{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.linearVel.x, vel.linearVel.y, vel.angVel); + Pose2D vel = odo.getVelocity(); + String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES)); telemetry.addData("Velocity", velocity); - telemetry.addData("X Encoder:", odo.getEncoderX()); //gets the raw data from the X encoder - telemetry.addData("Y Encoder:", odo.getEncoderY()); //gets the raw data from the Y encoder - telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint /* Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see @@ -185,8 +191,10 @@ gets the current Velocity (x & y in inches/sec and heading in radians/sec) and p */ telemetry.addData("Status", odo.getDeviceStatus()); + telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint + telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate telemetry.update(); + } - } -} + }} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java new file mode 100644 index 0000000..1693990 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * You can adjust the target angle on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning") +public class TurnTuner extends OpMode { + private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; + + private Telemetry telemetryA; + + public static double ANGLE = 2 * Math.PI; + + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } + + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + poseUpdater.update(); + + telemetryA.addData("total angle", poseUpdater.getTotalHeading()); + telemetryA.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryA.addData("multiplier", ANGLE / (poseUpdater.getTotalHeading() / poseUpdater.getLocalizer().getTurningMultiplier())); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java new file mode 100644 index 0000000..f5bb12a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java @@ -0,0 +1,348 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + + +import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; + +import java.util.ArrayList; +import java.util.Arrays; + +/** + * This is the BezierCurve class. This class handles the creation of Bezier curves, which are used + * as the basis of the path for the Path class. Bezier curves are parametric curves defined by a set + * of control points. So, they take in one input, t, that ranges from [0, 1] and that returns a point + * on the curve. Essentially, Bezier curves are a way of defining a parametric line easily. You can + * read more on Bezier curves here: https://en.wikipedia.org/wiki/Bézier_curve + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class BezierCurve { + // This contains the coefficients for the curve points + private ArrayList pointCoefficients = new ArrayList<>(); + + // This contains the control points for the Bezier curve + private ArrayList controlPoints = new ArrayList<>(); + + private Vector endTangent = new Vector(); + + private final int APPROXIMATION_STEPS = FollowerConstants.APPROXIMATION_STEPS; + + private final int DASHBOARD_DRAWING_APPROXIMATION_STEPS = 100; + + private double[][] dashboardDrawingPoints; + + private double UNIT_TO_TIME; + private double length; + + /** + * This creates an empty BezierCurve. + * IMPORTANT NOTE: Only use this for the constructors of classes extending this. If you try to + * run the robot on a Path containing an empty BezierCurve, then it will explode. + */ + public BezierCurve() { + } + + /** + * This creates a new BezierCurve with an ArrayList of control points and generates the curve. + * IMPORTANT NOTE: The order of the control points is important. That's the order the code will + * process them in, with the 0 index being the start point and the final index being the end point + * + * @param controlPoints This is the ArrayList of control points that define the BezierCurve. + */ + public BezierCurve(ArrayList controlPoints) { + if (controlPoints.size()<3) { + try { + throw new Exception("Too few control points"); + } catch (Exception e) { + e.printStackTrace(); + } + } + this.controlPoints = controlPoints; + initialize(); + } + + /** + * This creates a new Bezier curve with some specified control points and generates the curve. + * IMPORTANT NOTE: The order of the control points is important. That's the order the code will + * process them in, with the 0 index being the start point and the final index being the end point. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + */ + public BezierCurve(Point... controlPoints) { + for (Point controlPoint : controlPoints) { + this.controlPoints.add(controlPoint); + } + if (this.controlPoints.size()<3) { + try { + throw new Exception("Too few control points"); + } catch (Exception e) { + e.printStackTrace(); + } + } + initialize(); + } + + /** + * This handles most of the initialization of the BezierCurve that is called from the constructor. + */ + public void initialize() { + generateBezierCurve(); + length = approximateLength(); + UNIT_TO_TIME = 1/length; + endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); + endTangent = MathFunctions.normalizeVector(endTangent); + initializeDashboardDrawingPoints(); + } + + /** + * This creates the Array that holds the Points to draw on the Dashboard. + */ + public void initializeDashboardDrawingPoints() { + dashboardDrawingPoints = new double[2][DASHBOARD_DRAWING_APPROXIMATION_STEPS + 1]; + for (int i = 0; i <= DASHBOARD_DRAWING_APPROXIMATION_STEPS; i++) { + Point currentPoint = getPoint(i/(double) (DASHBOARD_DRAWING_APPROXIMATION_STEPS)); + dashboardDrawingPoints[0][i] = currentPoint.getX(); + dashboardDrawingPoints[1][i] = currentPoint.getY(); + } + } + + /** + * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC + * Dashboard. + * + * @return returns the 2D Array to draw on FTC Dashboard + */ + public double[][] getDashboardDrawingPoints() { + return dashboardDrawingPoints; + } + + /** + * This generates the Bezier curve. It assumes that the ArrayList of control points has been set. + * Well, this actually generates the coefficients for each control point on the Bezier curve. + * These coefficients can then be used to calculate a position, velocity, or accleration on the + * Bezier curve on the fly without much computational expense. + * + * See https://en.wikipedia.org/wiki/Bézier_curve for the explicit formula for Bezier curves + */ + public void generateBezierCurve() { + int n = controlPoints.size()-1; + for (int i = 0; i <= n; i++) { + pointCoefficients.add(new BezierCurveCoefficients(n, i)); + } + } + + /** + * This returns the unit tangent Vector at the end of the BezierCurve. + * + * @return returns the end tangent Vector. + */ + public Vector getEndTangent() { + return MathFunctions.copyVector(endTangent); + } + + /** + * This approximates the length of the BezierCurve in APPROXIMATION_STEPS number of steps. It's + * like a Riemann's sum, but for a parametric function's arc length. + * + * @return returns the approximated length of the BezierCurve. + */ + public double approximateLength() { + Point previousPoint = getPoint(0); + Point currentPoint; + double approxLength = 0; + for (int i = 1; i <= APPROXIMATION_STEPS; i++) { + currentPoint = getPoint(i/(double)APPROXIMATION_STEPS); + approxLength += previousPoint.distanceFrom(currentPoint); + previousPoint = currentPoint; + } + return approxLength; + } + + /** + * This returns the point on the Bezier curve that is specified by the parametric t value. A + * Bezier curve is a parametric function that returns points along it with t ranging from [0, 1], + * with 0 being the beginning of the curve and 1 being at the end. The Follower will follow + * BezierCurves from 0 to 1, in terms of t. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the point requested. + */ + public Point getPoint(double t) { + t = MathFunctions.clamp(t, 0, 1); + double xCoordinate = 0; + double yCoordinate = 0; + + // calculates the x coordinate of the point requested + for (int i = 0; i < controlPoints.size(); i++) { + xCoordinate += pointCoefficients.get(i).getValue(t) * controlPoints.get(i).getX(); + } + + // calculates the y coordinate of the point requested + for (int i = 0; i < controlPoints.size(); i++) { + yCoordinate += pointCoefficients.get(i).getValue(t) * controlPoints.get(i).getY(); + } + return new Point(xCoordinate, yCoordinate, Point.CARTESIAN); + } + + /** + * This returns the curvature of the Bezier curve at a specified t-value. + * + * @param t the parametric t input. + * @return returns the curvature. + */ + public double getCurvature(double t) { + t = MathFunctions.clamp(t, 0, 1); + Vector derivative = getDerivative(t); + Vector secondDerivative = getSecondDerivative(t); + + if (derivative.getMagnitude() == 0) return 0; + return (MathFunctions.crossProduct(derivative, secondDerivative))/Math.pow(derivative.getMagnitude(),3); + } + + /** + * This returns the derivative on the BezierCurve that is specified by the parametric t value. + * This is returned as a Vector, and this Vector is the tangent to the BezierCurve. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the derivative requested. + */ + public Vector getDerivative(double t) { + t = MathFunctions.clamp(t, 0, 1); + double xCoordinate = 0; + double yCoordinate = 0; + Vector returnVector = new Vector(); + + // calculates the x coordinate of the point requested + for (int i = 0; i < controlPoints.size()-1; i++) { + xCoordinate += pointCoefficients.get(i).getDerivativeValue(t) * (MathFunctions.subtractPoints(controlPoints.get(i+1), controlPoints.get(i)).getX()); + } + + // calculates the y coordinate of the point requested + for (int i = 0; i < controlPoints.size()-1; i++) {; + yCoordinate += pointCoefficients.get(i).getDerivativeValue(t) * (MathFunctions.subtractPoints(controlPoints.get(i+1), controlPoints.get(i)).getY()); + } + + returnVector.setOrthogonalComponents(xCoordinate, yCoordinate); + + return returnVector; + } + + /** + * This returns the second derivative on the BezierCurve that is specified by the parametric t value. + * This is returned as a Vector, and this Vector is the acceleration on the BezierCurve. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the second derivative requested. + */ + public Vector getSecondDerivative(double t) { + t = MathFunctions.clamp(t, 0, 1); + double xCoordinate = 0; + double yCoordinate = 0; + Vector returnVector = new Vector(); + + // calculates the x coordinate of the point requested + for (int i = 0; i < controlPoints.size()-2; i++) { + xCoordinate += pointCoefficients.get(i).getSecondDerivativeValue(t) * (MathFunctions.addPoints(MathFunctions.subtractPoints(controlPoints.get(i+2), new Point(2*controlPoints.get(i+1).getX(), 2*controlPoints.get(i+1).getY(), Point.CARTESIAN)), controlPoints.get(i)).getX()); + } + + // calculates the y coordinate of the point requested + for (int i = 0; i < controlPoints.size()-2; i++) { + yCoordinate += pointCoefficients.get(i).getSecondDerivativeValue(t) * (MathFunctions.addPoints(MathFunctions.subtractPoints(controlPoints.get(i+2), new Point(2*controlPoints.get(i+1).getX(), 2*controlPoints.get(i+1).getY(), Point.CARTESIAN)), controlPoints.get(i)).getY()); + } + + returnVector.setOrthogonalComponents(xCoordinate, yCoordinate); + + return returnVector; + } + + /** + * Because, for whatever reason, the second derivative returned by the getSecondDerivative(double t) + * method doesn't return the correct heading of the second derivative, this gets an approximate + * second derivative essentially using the limit method. I use this for its heading only. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the approximated second derivative. + */ + public Vector getApproxSecondDerivative(double t) { + double current = getDerivative(t).getTheta(); + double deltaCurrent = getDerivative(t + 0.0001).getTheta(); + + return new Vector(1, deltaCurrent - current); + } + + /** + * Returns the ArrayList of control points for this BezierCurve. + * + * @return This returns the control points. + */ + public ArrayList getControlPoints() { + return controlPoints; + } + + /** + * Returns the first control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getFirstControlPoint() { + return controlPoints.get(0); + } + + /** + * Returns the second control point, or the one after the start, for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondControlPoint() { + return controlPoints.get(1); + } + + /** + * Returns the second to last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondToLastControlPoint() { + return controlPoints.get(controlPoints.size()-2); + } + + /** + * Returns the last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getLastControlPoint() { + return controlPoints.get(controlPoints.size()-1); + } + + /** + * Returns the approximate length of this BezierCurve. + * + * @return This returns the length. + */ + public double length() { + return length; + } + + /** + * Returns the conversion factor of one unit of distance into t-value. Since parametric functions + * are defined by t, which can mean time, I use "time" in some method names for conciseness. + * + * @return returns the conversion factor. + */ + public double UNIT_TO_TIME() { + return UNIT_TO_TIME; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + public String pathType() { + return "curve"; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java new file mode 100644 index 0000000..f8fc51d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +/** + * This is the BezierCurveCoefficients class. This class handles holding the coefficients for each + * control point for the BezierCurve class to allow for faster on the fly calculations of points, + * derivatives, and second derivatives on Bezier curves. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/8/2024 + */ +public class BezierCurveCoefficients { + private double coefficient; + private double derivativeCoefficient; + private double secondDerivativeCoefficient; + + private int n; + private int i; + + /** + * This creates the coefficients within the summation equations for calculating positions, + * derivatives, and second derivatives on Bezier curves. + * + * @param n this is the degree of the Bezier curve function. + * @param i this is the i within the summation equation, so basically which place it is in the + * summation. + */ + public BezierCurveCoefficients(int n, int i) { + this.n = n; + this.i = i; + coefficient = MathFunctions.nCr(n, i); + derivativeCoefficient = MathFunctions.nCr(n - 1, i); + secondDerivativeCoefficient = MathFunctions.nCr(n - 2, i); + } + + /** + * This returns the coefficient for the summation to calculate a position on BezierCurves. + * + * @param t this is the t value within the parametric equation for the Bezier curve. + * @return this returns the coefficient. + */ + public double getValue(double t) { + return coefficient * Math.pow(1 - t, n - i) * Math.pow(t, i); + } + + /** + * This returns the coefficient for the summation to calculate a derivative on BezierCurves. + * + * @param t this is the t value within the parametric equation for the Bezier curve. + * @return this returns the coefficient. + */ + public double getDerivativeValue(double t) { + return n * derivativeCoefficient * Math.pow(t, i) * Math.pow(1 - t, n - i - 1); + } + + /** + * This returns the coefficient for the summation to calculate a second derivative on BezierCurves. + * + * @param t this is the t value within the parametric equation for the Bezier curve. + * @return this returns the coefficient. + */ + public double getSecondDerivativeValue(double t) { + return n * (n - 1) * secondDerivativeCoefficient * Math.pow(t, i) * Math.pow(1 - t, n - i - 2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java new file mode 100644 index 0000000..04c7979 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java @@ -0,0 +1,209 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the BezierLine class. This class handles the creation of BezierLines, which is what I + * call Bezier curves with only two control points. The parent BezierCurve class cannot handle Bezier + * curves with less than three control points, so this class handles lines. Additionally, it makes + * the calculations done on the fly a little less computationally expensive and more streamlined. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/9/2024 + */ +public class BezierLine extends BezierCurve { + + private Point startPoint; + private Point endPoint; + + private Vector endTangent; + + private double UNIT_TO_TIME; + private double length; + + /** + * This creates a new BezierLine with specified start and end Points. + * This is just a line but it extends the BezierCurve class so things work. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + */ + public BezierLine(Point startPoint, Point endPoint) { + super(); + this.startPoint = startPoint; + this.endPoint = endPoint; + length = approximateLength(); + UNIT_TO_TIME = 1 / length; + endTangent = MathFunctions.normalizeVector(getDerivative(1)); + super.initializeDashboardDrawingPoints(); + } + + /** + * This returns the unit tangent Vector at the end of the BezierLine. + * + * @return returns the tangent Vector. + */ + @Override + public Vector getEndTangent() { + return MathFunctions.copyVector(endTangent); + } + + /** + * This gets the length of the BezierLine. + * + * @return returns the length of the BezierLine. + */ + @Override + public double approximateLength() { + return Math.sqrt(Math.pow(startPoint.getX() - endPoint.getX(), 2) + Math.pow(startPoint.getY() - endPoint.getY(), 2)); + } + + /** + * This returns the Point on the Bezier line that is specified by the parametric t value. + * + * @param t this is the t value of the parametric line. t is clamped to be between 0 and 1 inclusive. + * @return this returns the Point requested. + */ + @Override + public Point getPoint(double t) { + t = MathFunctions.clamp(t, 0, 1); + return new Point((endPoint.getX() - startPoint.getX()) * t + startPoint.getX(), (endPoint.getY() - startPoint.getY()) * t + startPoint.getY(), Point.CARTESIAN); + } + + /** + * This returns the curvature of the BezierLine, which is zero. + * + * @param t the parametric t value. + * @return returns the curvature. + */ + @Override + public double getCurvature(double t) { + return 0.0; + } + + /** + * This returns the derivative on the BezierLine as a Vector, which is a constant slope. + * The t value doesn't really do anything, but it's there so I can override methods. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the derivative requested. + */ + @Override + public Vector getDerivative(double t) { + Vector returnVector = new Vector(); + + returnVector.setOrthogonalComponents(endPoint.getX() - startPoint.getX(), endPoint.getY() - startPoint.getY()); + + return returnVector; + } + + /** + * This returns the second derivative on the Bezier line, which is a zero Vector. + * Once again, the t is only there for the override. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the second derivative requested. + */ + @Override + public Vector getSecondDerivative(double t) { + return new Vector(); + } + + /** + * This returns the zero Vector, but it's here so I can override the method in the BezierCurve + * class. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the approximated second derivative, which is the zero Vector. + */ + @Override + public Vector getApproxSecondDerivative(double t) { + return new Vector(); + } + + /** + * Returns the ArrayList of control points for this BezierLine. + * + * @return This returns the control points. + */ + @Override + public ArrayList getControlPoints() { + ArrayList returnList = new ArrayList<>(); + returnList.add(startPoint); + returnList.add(endPoint); + return returnList; + } + + /** + * Returns the first control point for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getFirstControlPoint() { + return startPoint; + } + + /** + * Returns the second control point, or the one after the start, for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getSecondControlPoint() { + return endPoint; + } + + /** + * Returns the second to last control point for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getSecondToLastControlPoint() { + return startPoint; + } + + /** + * Returns the last control point for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getLastControlPoint() { + return endPoint; + } + + /** + * Returns the length of this BezierLine. + * + * @return This returns the length. + */ + @Override + public double length() { + return length; + } + + /** + * Returns the conversion factor of one unit of distance into t value. + * + * @return returns the conversion factor. + */ + @Override + public double UNIT_TO_TIME() { + return UNIT_TO_TIME; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + @Override + public String pathType() { + return "line"; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java new file mode 100644 index 0000000..cd2382e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java @@ -0,0 +1,209 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the BezierPoint class. This class handles the creation of BezierPoints, which is what I + * call Bezier curves with only one control point. The parent BezierCurve class cannot handle Bezier + * curves with less than three control points, so this class handles points. Additionally, it makes + * the calculations done on the fly a little less computationally expensive and more streamlined. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/9/2024 + */ +public class BezierPoint extends BezierCurve { + + private Point point; + + private Vector endTangent = new Vector(); + + private double UNIT_TO_TIME; + private double length; + + /** + * This creates a new BezierPoint with a specified Point. + * This is just a point but it extends the BezierCurve class so things work. + * + * @param point the specified point. + */ + public BezierPoint(Point point) { + super(); + this.point = point; + length = approximateLength(); + super.initializeDashboardDrawingPoints(); + } + + /** + * This supposedly returns the unit tangent Vector at the end of the path, but since there is + * no end tangent of a point, this returns a zero Vector instead. Holding BezierPoints in the + * Follower doesn't use the drive Vector, so the end tangent Vector is not needed or truly used. + * + * @return returns the zero Vector. + */ + @Override + public Vector getEndTangent() { + return MathFunctions.copyVector(endTangent); + } + + /** + * This gets the length of the BezierPoint. Since points don't have length, this returns zero. + * + * @return returns the length of the BezierPoint. + */ + @Override + public double approximateLength() { + return 0.0; + } + + /** + * This returns the point on the BezierPoint that is specified by the parametric t value. Since + * this is a Point, this just returns the one control point's position. + * + * @param t this is the t value of the parametric line. t is clamped to be between 0 and 1 inclusive. + * @return this returns the Point requested. + */ + @Override + public Point getPoint(double t) { + return new Point(point.getX(), point.getY(), Point.CARTESIAN); + } + + /** + * This returns the curvature of the BezierPoint, which is zero since this is a Point. + * + * @param t the parametric t value. + * @return returns the curvature, which is zero. + */ + @Override + public double getCurvature(double t) { + return 0.0; + } + + /** + * This returns the derivative on the BezierPoint, which is the zero Vector since this is a Point. + * The t value doesn't really do anything, but it's there so I can override methods. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the derivative requested, which is the zero Vector. + */ + @Override + public Vector getDerivative(double t) { + return MathFunctions.copyVector(endTangent); + } + + /** + * This returns the second derivative on the Bezier line, which is the zero Vector since this + * is a Point. + * Once again, the t is only there for the override. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the second derivative requested, which is the zero Vector. + */ + @Override + public Vector getSecondDerivative(double t) { + return new Vector(); + } + + /** + * This returns the zero Vector, but it's here so I can override the method in the BezierCurve + * class. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the approximated second derivative, which is the zero Vector. + */ + @Override + public Vector getApproxSecondDerivative(double t) { + return new Vector(); + } + + /** + * Returns the ArrayList of control points for this BezierPoint + * + * @return This returns the control point. + */ + @Override + public ArrayList getControlPoints() { + ArrayList returnList = new ArrayList<>(); + returnList.add(point); + return returnList; + } + + /** + * Returns the first, and only, control point for this BezierPoint + * + * @return This returns the Point. + */ + @Override + public Point getFirstControlPoint() { + return point; + } + + /** + * Returns the second control point, or the one after the start, for this BezierPoint. This + * returns the one control point of the BezierPoint, and there are several redundant methods + * that return the same control point, but it's here so I can override methods. + * + * @return This returns the Point. + */ + @Override + public Point getSecondControlPoint() { + return point; + } + + /** + * Returns the second to last control point for this BezierPoint. This + * returns the one control point of the BezierPoint, and there are several redundant methods + * that return the same control point, but it's here so I can override methods. + * + * @return This returns the Point. + */ + @Override + public Point getSecondToLastControlPoint() { + return point; + } + + /** + * Returns the last control point for this BezierPoint. This + * returns the one control point of the BezierPoint, and there are several redundant methods + * that return the same control point, but it's here so I can override methods. + * + * @return This returns the Point. + */ + @Override + public Point getLastControlPoint() { + return point; + } + + /** + * Returns the length of this BezierPoint, which is zero since Points don't have length. + * + * @return This returns the length. + */ + @Override + public double length() { + return length; + } + + /** + * Returns the conversion factor of one unit of distance into t value. There is no length or + * conversion factor to speak of for Points. + * + * @return returns the conversion factor. + */ + @Override + public double UNIT_TO_TIME() { + return 0; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + @Override + public String pathType() { + return "point"; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java new file mode 100644 index 0000000..bd4e393 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -0,0 +1,323 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; + +/** + * This is the MathFunctions class. This contains many useful math related methods that I use in + * other classes to simplify code elsewhere. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/9/2024 + */ +public class MathFunctions { + + /** + * This is a simple implementation of the choose function in math. It's equivalent to the number + * of ways you can choose r items from n total items, if only which items got picked and not the + * order of picking the items mattered. + * + * @param n this is how many you want to choose from. + * @param r this is how many you want to choose. + * @return returns the result of the "n choose r" function. + */ + public static double nCr(int n, int r) { + double num = 1; + double denom = 1; + + // this multiplies up the numerator of the nCr function + for (int i = n; i > n - r; i--) { + num *= i; + } + + // this multiplies up the denominator of the nCr function + for (int i = 1; i <= r; i++) { + denom *= i; + } + + return num / denom; + } + + /** + * This returns the sign (positive/negative) of a number. + * + * @param get the number. + * @return returns the sign of the number. + */ + public static double getSign(double get) { + if (get == 0) return 0; + if (get > 0) return 1; + return -1; + } + + /** + * This clamps down a value to between the lower and upper bounds inclusive. + * + * @param num the number to be clamped. + * @param lower the lower bound. + * @param upper the upper bound. + * @return returns the clamped number. + */ + public static double clamp(double num, double lower, double upper) { + if (num < lower) return lower; + if (num > upper) return upper; + return num; + } + + /** + * This normalizes an angle to be between 0 and 2 pi radians, inclusive. + *

+ * IMPORTANT NOTE: This method operates in radians. + * + * @param angleRadians the angle to be normalized. + * @return returns the normalized angle. + */ + public static double normalizeAngle(double angleRadians) { + double angle = angleRadians; + while (angle < 0) angle += 2 * Math.PI; + while (angle > 2 * Math.PI) angle -= 2 * Math.PI; + return angle; + } + + /** + * This returns the smallest angle between two angles. This operates in radians. + * + * @param one one of the angles. + * @param two the other one. + * @return returns the smallest angle. + */ + public static double getSmallestAngleDifference(double one, double two) { + return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one)); + } + + /** + * This gets the direction to turn between a start heading and an end heading. Positive is left + * and negative is right. This operates in radians. + * + * @return returns the turn direction. + */ + public static double getTurnDirection(double startHeading, double endHeading) { + if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) { + return 1; // counter clock wise + } + return -1; // clock wise + } + + /** + * This returns the distance between a Pose and a Point, + * + * @param pose this is the Pose. + * @param point this is the Point. + * @return returns the distance between the two. + */ + public static double distance(Pose pose, Point point) { + return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2)); + } + + /** + * This returns the distance between a Pose and another Pose. + * + * @param one this is the first Pose. + * @param two this is the second Pose. + * @return returns the distance between the two. + */ + public static double distance(Pose one, Pose two) { + return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2)); + } + + /** + * This returns a Point that is the sum of the two input Point. + * + * @param one the first Point + * @param two the second Point + * @return returns the sum of the two Points. + */ + public static Point addPoints(Point one, Point two) { + return new Point(one.getX() + two.getX(), one.getY() + two.getY(), Point.CARTESIAN); + } + + /** + * This returns a Pose that is the sum of the two input Pose. + * + * @param one the first Pose + * @param two the second Pose + * @return returns the sum of the two Pose. + */ + public static Pose addPoses(Pose one, Pose two) { + return new Pose(one.getX() + two.getX(), one.getY() + two.getY(), one.getHeading() + two.getHeading()); + } + + /** + * This subtracts the second Point from the first Point and returns the result as a Point. + * Do note that order matters here. + * + * @param one the first Point. + * @param two the second Point. + * @return returns the difference of the two Points. + */ + public static Point subtractPoints(Point one, Point two) { + return new Point(one.getX() - two.getX(), one.getY() - two.getY(), Point.CARTESIAN); + } + + /** + * This subtracts the second Pose from the first Pose and returns the result as a Pose. + * Do note that order matters here. + * + * @param one the first Pose. + * @param two the second Pose. + * @return returns the difference of the two Pose. + */ + public static Pose subtractPoses(Pose one, Pose two) { + return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); + } + + /** + * This rotates the given pose by the given theta, + * + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. + * @param rotateHeading whether to adjust the Pose heading too. + * @return the rotated Pose. + */ + public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { + double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); + double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); + double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); + + return new Pose(x, y, heading); + } + + /** + * This multiplies a Point by a scalar and returns the result as a Point + * + * @param point the Point being multiplied. + * @param scalar the scalar multiplying into the Point. + * @return returns the scaled Point. + */ + public static Point scalarMultiplyPoint(Point point, double scalar) { + return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN); + } + + /** + * Copies a Point, but with a different reference location in the memory. So basically a deep + * copy. + * + * @param point the Point to be deep copied. + * @return returns the copied Point. + */ + public static Point copyPoint(Point point) { + return new Point(point.getX(), point.getY(), Point.CARTESIAN); + } + + /** + * Copies a Vector, but with a different reference location in the memory. So basically a deep + * copy. + * + * @param vector Vector to be deep copied. + * @return returns the copied Vector. + */ + public static Vector copyVector(Vector vector) { + return new Vector(vector.getMagnitude(), vector.getTheta()); + } + + /** + * This multiplies a Vector by a scalar and returns the result as a Vector. + * + * @param vector the Vector being multiplied. + * @param scalar the scalar multiplying into the Vector. + * @return returns the scaled Vector. + */ + public static Vector scalarMultiplyVector(Vector vector, double scalar) { + return new Vector(vector.getMagnitude() * scalar, vector.getTheta()); + } + + /** + * This normalizes a Vector to be of magnitude 1, unless the Vector is the zero Vector. + * In that case, it just returns back the zero Vector but with a different memory location. + * + * @param vector the Vector being normalized. + * @return returns the normalized (or zero) Vector. + */ + public static Vector normalizeVector(Vector vector) { + if (vector.getMagnitude() == 0) { + return new Vector(0.0, vector.getTheta()); + } else { + return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta()); + } + } + + /** + * This returns a Vector that is the sum of the two input Vectors. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the sum of the Vectors. + */ + public static Vector addVectors(Vector one, Vector two) { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent()); + return returnVector; + } + + /** + * This subtracts the second Vector from the first Vector and returns the result as a Vector. + * Do note that order matters here. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the second Vector subtracted from the first Vector. + */ + public static Vector subtractVectors(Vector one, Vector two) { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent()); + return returnVector; + } + + /** + * This computes the dot product of the two Vectors. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the dot product of the two Vectors. + */ + public static double dotProduct(Vector one, Vector two) { + return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent(); + } + + /** + * This computes the first Vector crossed with the second Vector, so a cross product. + * Do note that order matters here. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the cross product of the two Vectors. + */ + public static double crossProduct(Vector one, Vector two) { + return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent(); + } + + /** + * This returns whether a specified value is within a second specified value by plus/minus a + * specified accuracy amount. + * + * @param one first number specified. + * @param two Second number specified. + * @param accuracy the level of accuracy specified. + * @return returns if the two numbers are within the specified accuracy of each other. + */ + public static boolean roughlyEquals(double one, double two, double accuracy) { + return (one < two + accuracy && one > two - accuracy); + } + + /** + * This returns whether a specified number is within a second specified number by plus/minus 0.0001. + * + * @param one first number specified. + * @param two second number specified. + * @return returns if a specified number is within plus/minus 0.0001 of the second specified number. + */ + public static boolean roughlyEquals(double one, double two) { + return roughlyEquals(one, two, 0.0001); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java new file mode 100644 index 0000000..1539a49 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -0,0 +1,486 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; + +import java.util.ArrayList; + +/** + * This is the Path class. This class handles containing information on the actual path the Follower + * will follow, not just the shape of the path that the BezierCurve class handles. This contains + * information on the stop criteria for a Path, the heading interpolation, and deceleration. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/10/2024 + */ +public class Path { + private BezierCurve curve; + + private double startHeading; + private double endHeading; + private double closestPointCurvature; + private double closestPointTValue; + private double linearInterpolationEndTime; + + private Vector closestPointTangentVector; + private Vector closestPointNormalVector; + + private boolean isTangentHeadingInterpolation = true; + private boolean followTangentReversed; + + // A multiplier for the zero power acceleration to change the speed the robot decelerates at + // the end of paths. + // Increasing this will cause the robot to try to decelerate faster, at the risk of overshoots + // or localization slippage. + // Decreasing this will cause the deceleration at the end of the Path to be slower, making the + // robot slower but reducing risk of end-of-path overshoots or localization slippage. + // This can be set individually for each Path, but this is the default. + private double zeroPowerAccelerationMultiplier = FollowerConstants.zeroPowerAccelerationMultiplier; + + // When the robot is at the end of its current Path or PathChain and the velocity goes + // this value, then end the Path. This is in inches/second. + // This can be custom set for each Path. + private double pathEndVelocityConstraint = FollowerConstants.pathEndVelocityConstraint; + + // When the robot is at the end of its current Path or PathChain and the translational error + // goes below this value, then end the Path. This is in inches. + // This can be custom set for each Path. + private double pathEndTranslationalConstraint = FollowerConstants.pathEndTranslationalConstraint; + + // When the robot is at the end of its current Path or PathChain and the heading error goes + // below this value, then end the Path. This is in radians. + // This can be custom set for each Path. + private double pathEndHeadingConstraint = FollowerConstants.pathEndHeadingConstraint; + + // When the t-value of the closest point to the robot on the Path is greater than this value, + // then the Path is considered at its end. + // This can be custom set for each Path. + private double pathEndTValueConstraint = FollowerConstants.pathEndTValueConstraint; + + // When the Path is considered at its end parametrically, then the Follower has this many + // milliseconds to further correct by default. + // This can be custom set for each Path. + private double pathEndTimeoutConstraint = FollowerConstants.pathEndTimeoutConstraint; + + /** + * Creates a new Path from a BezierCurve. The default heading interpolation is tangential. + * + * @param curve the BezierCurve. + */ + public Path(BezierCurve curve) { + this.curve = curve; + } + + /** + * This sets the heading interpolation to linear with a specified start heading and end heading + * for the Path. This will interpolate across the entire length of the Path, so there may be + * some issues with end heading accuracy and precision if this is used. If a precise end heading + * is necessary, then use the setLinearHeadingInterpolation(double startHeading, + * double endHeading, double endTime) method. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + */ + public void setLinearHeadingInterpolation(double startHeading, double endHeading) { + linearInterpolationEndTime = 1; + isTangentHeadingInterpolation = false; + this.startHeading = startHeading; + this.endHeading = endHeading; + } + + /** + * This sets the heading interpolation to linear with a specified start heading and end heading + * for the Path. This will interpolate from the start of the Path to the specified end time. + * This ensures high accuracy and precision than interpolating across the entire Path. However, + * interpolating too quickly can cause undesired oscillations and inaccuracies of its own, so + * generally interpolating to something like 0.8 of your Path should work best. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + * @param endTime The end time on the Path that the linear heading interpolation will finish. + * This value ranges from [0, 1] since Bezier curves are parametric functions. + */ + public void setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { + linearInterpolationEndTime = MathFunctions.clamp(endTime, 0.000000001, 1); + isTangentHeadingInterpolation = false; + this.startHeading = startHeading; + this.endHeading = endHeading; + } + + /** + * This sets the heading interpolation to maintain a constant heading. + * + * @param setHeading the constant heading for the Path. + */ + public void setConstantHeadingInterpolation(double setHeading) { + linearInterpolationEndTime = 1; + isTangentHeadingInterpolation = false; + startHeading = setHeading; + endHeading = setHeading; + } + + /** + * This gets the closest Point from a specified pose to the BezierCurve with a binary search + * that is limited to some specified step limit. + * + * @param pose the pose. + * @param searchStepLimit the binary search step limit. + * @return returns the closest Point. + */ + public Pose getClosestPoint(Pose pose, int searchStepLimit) { + double lower = 0; + double upper = 1; + Point returnPoint; + + // we don't need to calculate the midpoint, so we start off at the 1/4 and 3/4 point + for (int i = 0; i < searchStepLimit; i++) { + if (MathFunctions.distance(pose, getPoint(lower + 0.25 * (upper-lower))) > MathFunctions.distance(pose, getPoint(lower + 0.75 * (upper-lower)))) { + lower += (upper-lower)/2.0; + } else { + upper -= (upper-lower)/2.0; + } + } + + closestPointTValue = lower + 0.5 * (upper-lower); + + returnPoint = getPoint(closestPointTValue); + + closestPointTangentVector = curve.getDerivative(closestPointTValue); + + closestPointNormalVector = curve.getApproxSecondDerivative(closestPointTValue); + + closestPointCurvature = curve.getCurvature(closestPointTValue); + + return new Pose(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); + } + + /** + * This sets whether to follow the tangent heading facing away from (reverse) or towards the + * tangent. This will also set your heading interpolation to tangential. + * + * @param set sets tangential heading reversed or not. + */ + public void setReversed(boolean set) { + isTangentHeadingInterpolation = true; + followTangentReversed = set; + } + + /** + * This sets the heading interpolation to tangential. + */ + public void setTangentHeadingInterpolation() { + isTangentHeadingInterpolation = true; + followTangentReversed = false; + } + + /** + * This returns the unit tangent Vector at the end of the BezierCurve. + * + * @return returns the end tangent Vector. + */ + public Vector getEndTangent() { + return curve.getEndTangent(); + } + + /** + * This returns the point on the Bezier curve that is specified by the parametric t value. A + * Bezier curve is a parametric function that returns points along it with t ranging from [0, 1], + * with 0 being the beginning of the curve and 1 being at the end. The Follower will follow + * BezierCurves from 0 to 1, in terms of t. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the point requested. + */ + public Point getPoint(double t) { + return curve.getPoint(t); + } + + /** + * This returns the t-value of the closest Point on the BezierCurve. + * + * @return returns the closest Point t-value. + */ + public double getClosestPointTValue() { + return closestPointTValue; + } + + /** + * This returns the approximated length of the BezierCurve. + * + * @return returns the length of the BezierCurve. + */ + public double length() { + return curve.length(); + } + + /** + * This returns the curvature of the BezierCurve at a specified t-value. + * + * @param t the specified t-value. + * @return returns the curvature of the BezierCurve at the specified t-value. + */ + public double getCurvature(double t) { + return curve.getCurvature(t); + } + + /** + * This returns the curvature of the BezierCurve at the closest Point. + * + * @return returns the curvature of the BezierCurve at the closest Point. + */ + public double getClosestPointCurvature() { + return closestPointCurvature; + } + + /** + * This returns the normal Vector at the closest Point. + * + * @return returns the normal Vector at the closest Point. + */ + public Vector getClosestPointNormalVector() { + return MathFunctions.copyVector(closestPointNormalVector); + } + + /** + * This returns the tangent Vector at the closest Point. + * + * @return returns the tangent Vector at the closest Point. + */ + public Vector getClosestPointTangentVector() { + return MathFunctions.copyVector(closestPointTangentVector); + } + + /** + * This returns the heading goal at the closest Point. + * + * @return returns the heading goal at the closest Point. + */ + public double getClosestPointHeadingGoal() { + if (isTangentHeadingInterpolation) { + if (followTangentReversed) return MathFunctions.normalizeAngle(closestPointTangentVector.getTheta() + Math.PI); + return closestPointTangentVector.getTheta(); + } else { + return getHeadingGoal(closestPointTValue); + } + } + + /** + * This gets the heading goal at a specified t-value. + * + * @param t the specified t-value. + * @return returns the heading goal at the specified t-value. + */ + public double getHeadingGoal(double t) { + if (isTangentHeadingInterpolation) { + if (followTangentReversed) return MathFunctions.normalizeAngle(curve.getDerivative(t).getTheta() + Math.PI); + return curve.getDerivative(t).getTheta(); + } else { + if (t > linearInterpolationEndTime) { + return MathFunctions.normalizeAngle(endHeading); + } + return MathFunctions.normalizeAngle(startHeading + MathFunctions.getTurnDirection(startHeading, endHeading) * MathFunctions.getSmallestAngleDifference(endHeading, startHeading) * (t / linearInterpolationEndTime)); + } + } + + /** + * This returns if the robot is at the end of the Path, according to the parametric t-value. + * + * @return returns if at end. + */ + public boolean isAtParametricEnd() { + if (closestPointTValue >= pathEndTValueConstraint) return true; + return false; + } + + /** + * This returns if the robot is at the beginning of the Path, according to the parametric t-value. + * + * @return returns if at start. + */ + public boolean isAtParametricStart() { + if (closestPointTValue <= 1- pathEndTValueConstraint) return true; + return false; + } + + /** + * Returns the ArrayList of control points for this BezierCurve. + * + * @return This returns the control points. + */ + public ArrayList getControlPoints() { + return curve.getControlPoints(); + } + + /** + * Returns the first control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getFirstControlPoint() { + return curve.getFirstControlPoint(); + } + + /** + * Returns the second control point, or the one after the start, for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondControlPoint() { + return curve.getSecondControlPoint(); + } + + /** + * Returns the second to last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondToLastControlPoint() { + return curve.getSecondToLastControlPoint(); + } + + /** + * Returns the last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getLastControlPoint() { + return curve.getLastControlPoint(); + } + + /** + * This sets the path's deceleration factor in terms of the natural deceleration of the robot + * when power is cut from the drivetrain. + * + * @param set This sets the multiplier. + */ + public void setZeroPowerAccelerationMultiplier(double set) { + zeroPowerAccelerationMultiplier = set; + } + + /** + * This sets the velocity stop criteria. When velocity is below this amount, then this is met. + * + * @param set This sets the velocity end constraint. + */ + public void setPathEndVelocityConstraint(double set) { + pathEndVelocityConstraint = set; + } + + /** + * This sets the translational stop criteria. When the translational error, or how far off the + * end point the robot is, goes below this, then the translational end criteria is met. + * + * @param set This sets the translational end constraint. + */ + public void setPathEndTranslationalConstraint(double set) { + pathEndTranslationalConstraint = set; + } + + /** + * This sets the heading stop criteria. When the heading error is less than this amount, then + * the heading end criteria is met. + * + * @param set This sets the heading end constraint. + */ + public void setPathEndHeadingConstraint(double set) { + pathEndHeadingConstraint = set; + } + + /** + * This sets the parametric end criteria. When the t-value of the closest Point on the Path is + * greater than this amount, then the parametric end criteria is met. + * + * @param set This sets the t-value end constraint. + */ + public void setPathEndTValueConstraint(double set) { + pathEndTValueConstraint = set; + } + + /** + * This sets the Path end timeout. If the Path is at its end parametrically, then the Follower + * has this many milliseconds to correct before the Path gets ended anyways. + * + * @param set This sets the Path end timeout. + */ + public void setPathEndTimeoutConstraint(double set) { + pathEndTimeoutConstraint = set; + } + + /** + * This gets the deceleration multiplier. + * + * @return This returns the deceleration multiplier. + */ + public double getZeroPowerAccelerationMultiplier() { + return zeroPowerAccelerationMultiplier; + } + + /** + * This gets the velocity stop criteria. + * + * @return This returns the velocity stop criteria. + */ + public double getPathEndVelocityConstraint() { + return pathEndVelocityConstraint; + } + + /** + * This gets the translational stop criteria. + * + * @return This returns the translational stop criteria. + */ + public double getPathEndTranslationalConstraint() { + return pathEndTranslationalConstraint; + } + + /** + * This gets the heading stop criteria. + * + * @return This returns the heading stop criteria. + */ + public double getPathEndHeadingConstraint() { + return pathEndHeadingConstraint; + } + + /** + * This gets the parametric end criteria. + * + * @return This returns the parametric end criteria. + */ + public double getPathEndTValueConstraint() { + return pathEndTValueConstraint; + } + + /** + * This gets the Path end correction time. + * + * @return This returns the Path end correction time. + */ + public double getPathEndTimeoutConstraint() { + return pathEndTimeoutConstraint; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + public String pathType() { + return curve.pathType(); + } + + /** + * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC + * Dashboard. + * + * @return returns the 2D Array to draw on FTC Dashboard + */ + public double[][] getDashboardDrawingPoints() { + return curve.getDashboardDrawingPoints(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java new file mode 100644 index 0000000..f044ce7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -0,0 +1,249 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the PathBuilder class. This class makes it easier to create PathChains, so you don't have + * to individually create Path instances to create a PathChain. A PathBuilder can be accessed + * through running the pathBuilder() method on an instance of the Follower class, or just creating + * an instance of PathBuilder regularly. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class PathBuilder { + private ArrayList paths = new ArrayList<>(); + + private ArrayList callbacks = new ArrayList<>(); + + /** + * This is an empty constructor for the PathBuilder class so it can get started. + * The PathBuilder allows for easier construction of PathChains. + * The proper usage is using an instance of the Follower class: + * Follower follower = new Follower(hardwareMap); + * Then calling "follower.pathBuilder.[INSERT PATH BUILDING METHODS].build(); + * Of course, you can split up the method calls onto separate lines for readability. + */ + public PathBuilder() { + } + + /** + * This adds a Path to the PathBuilder. + * + * @param path The Path being added. + * @return This returns itself with the updated data. + */ + public PathBuilder addPath(Path path) { + this.paths.add(path); + return this; + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param curve The curve is turned into a Path and added. + * @return This returns itself with the updated data. + */ + public PathBuilder addPath(BezierCurve curve) { + this.paths.add(new Path(curve)); + return this; + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(Point... controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(ArrayList controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierLine to the PathBuilder. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierLine(Point startPoint, Point endPoint) { + return addPath(new BezierLine(startPoint, endPoint)); + } + + /** + * This sets a linear heading interpolation on the last Path added to the PathBuilder. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + * @return This returns itself with the updated data. + */ + public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading); + return this; + } + + /** + * This sets a linear heading interpolation on the last Path added to the PathBuilder. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + * @param endTime The end time on the Path that the linear heading interpolation will end. + * This value goes from [0, 1] since Bezier curves are parametric functions. + * @return This returns itself with the updated data. + */ + public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); + return this; + } + + /** + * This sets a constant heading interpolation on the last Path added to the PathBuilder. + * + * @param setHeading The constant heading specified. + * @return This returns itself with the updated data. + */ + public PathBuilder setConstantHeadingInterpolation(double setHeading) { + this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading); + return this; + } + + /** + * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. + * + * @param set This sets the heading to reversed tangent following if this parameter is true. + * Conversely, this sets a tangent following if this parameter is false. + * @return This returns itself with the updated data. + */ + public PathBuilder setReversed(boolean set) { + this.paths.get(paths.size() - 1).setReversed(set); + return this; + } + + /** + * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. + * There really shouldn't be a reason to use this since the default heading interpolation is + * tangential but it's here. + */ + public PathBuilder setTangentHeadingInterpolation() { + this.paths.get(paths.size() - 1).setTangentHeadingInterpolation(); + return this; + } + + /** + * This sets the deceleration multiplier on the last Path added to the PathBuilder. + * + * @param set This sets the multiplier for the goal for the deceleration of the robot. + * @return This returns itself with the updated data. + */ + public PathBuilder setZeroPowerAccelerationMultiplier(double set) { + this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set); + return this; + } + + /** + * This sets the path end velocity constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end velocity constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndVelocityConstraint(double set) { + this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set); + return this; + } + + /** + * This sets the path end translational constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end translational constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndTranslationalConstraint(double set) { + this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set); + return this; + } + + /** + * This sets the path end heading constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end heading constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndHeadingConstraint(double set) { + this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set); + return this; + } + + /** + * This sets the path end t-value (parametric time) constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end t-value (parametric time) constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndTValueConstraint(double set) { + this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set); + return this; + } + + /** + * This sets the path end timeout constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end timeout constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndTimeoutConstraint(double set) { + this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set); + return this; + } + + /** + * This adds a temporal callback on the last Path added to the PathBuilder. + * This callback is set to run at a specified number of milliseconds after the start of the path. + * + * @param time This sets the number of milliseconds of wait between the start of the Path and + * the calling of the callback. + * @param runnable This sets the code for the callback to run. Use lambda statements for this. + * @return This returns itself with the updated data. + */ + public PathBuilder addTemporalCallback(double time, Runnable runnable) { + this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1)); + return this; + } + + /** + * This adds a parametric callback on the last Path added to the PathBuilder. + * This callback is set to run at a certain point on the Path. + * + * @param t This sets the t-value (parametric time) on the Path for when to run the callback. + * @param runnable This sets the code for the callback to run. Use lambda statements for this. + * @return This returns itself with the updated data. + */ + public PathBuilder addParametricCallback(double t, Runnable runnable) { + this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1)); + return this; + } + + /** + * This builds all the Path and callback information together into a PathChain. + * + * @return This returns a PathChain made of all the specified paths and callbacks. + */ + public PathChain build() { + PathChain returnChain = new PathChain(paths); + returnChain.setCallbacks(callbacks); + return returnChain; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java new file mode 100644 index 0000000..c02e1d5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import org.firstinspires.ftc.teamcode.pedroPathing.util.SingleRunAction; + +/** + * This is the PathCallback class. This class handles callbacks of Runnables in PathChains. + * Basically, this allows you to run non-blocking code in the middle of PathChains. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class PathCallback extends SingleRunAction { + + private double startCondition; + + private int type; + private int index; + + public static final int TIME = 0; + public static final int PARAMETRIC = 1; + + /** + * This creates a new PathCallback with a specified start condition (either time or parametric), + * a Runnable of code to run (preferably a lambda statement), a type (using the class constants), + * and an index for which Path within a PathChain the callback is to run on. + * + * @param startCondition This defines when the callback is to be run, either as a wait time in + * milliseconds or a t-value (parametric time) point. + * @param runnable This contains the code to run when the callback is called. + * @param type This defines the type of callback using the class constants. + * @param index This defines which Path within the PathChain the callback is to run on. + */ + public PathCallback(double startCondition, Runnable runnable, int type, int index) { + super(runnable); + this.startCondition = startCondition; + this.type = type; + if (this.type != TIME || this.type != PARAMETRIC) { + this.type = PARAMETRIC; + } + if (this.type == TIME && this.startCondition < 0) { + this.startCondition = 0.0; + } + if (this.type == PARAMETRIC) { + this.startCondition = MathFunctions.clamp(this.startCondition, 0, 1); + } + this.index = index; + } + + /** + * This returns the type of callback this is (time or parametric). + * + * @return This returns the type of callback. + */ + public int getType() { + return type; + } + + /** + * This returns the start condition for this callback. This will be the wait time in milliseconds + * if this is a time callback or a t-value if this is a parametric callback. + * + * @return This returns the start condition. + */ + public double getStartCondition() { + return startCondition; + } + + /** + * This returns the index of which Path the callback is to run on within the PathChain. + * + * @return This returns the Path index of this callback. + */ + public int getIndex() { + return index; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java new file mode 100644 index 0000000..f350f5e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the PathChain class. This class handles chaining together multiple Paths into a larger + * collection of Paths that can be run continuously. Additionally, this allows for the addition of + * PathCallbacks to specific Paths in the PathChain, allowing for non-blocking code to be run in + * the middle of a PathChain. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class PathChain { + private ArrayList pathChain = new ArrayList<>(); + + private ArrayList callbacks = new ArrayList<>(); + + /** + * This creates a new PathChain from some specified Paths. + * + * IMPORTANT NOTE: Order matters here. The order in which the Paths are input is the order in + * which they will be run. + * + * @param paths the specified Paths. + */ + public PathChain(Path... paths) { + for (Path path : paths) { + pathChain.add(path); + } + } + + /** + * This creates a new PathChain from an ArrayList of Paths. + * + * IMPORTANT NOTE: Order matters here. The order in which the Paths are input is the order in + * which they will be run. + * + * @param paths the ArrayList of Paths. + */ + public PathChain(ArrayList paths) { + pathChain = paths; + } + + /** + * This returns the Path on the PathChain at a specified index. + * + * @param index the index. + * @return returns the Path at the index. + */ + public Path getPath(int index) { + return pathChain.get(index); + } + + /** + * This returns the size of the PathChain. + * + * @return returns the size of the PathChain. + */ + public int size() { + return pathChain.size(); + } + + /** + * This sets the PathCallbacks of the PathChain with some specified PathCallbacks. + * + * @param callbacks the specified PathCallbacks. + */ + public void setCallbacks(PathCallback... callbacks) { + for (PathCallback callback : callbacks) { + this.callbacks.add(callback); + } + } + + /** + * This sets the PathCallbacks of the PathChain with an ArrayList of PathCallbacks. + * + * @param callbacks the ArrayList of PathCallbacks. + */ + public void setCallbacks(ArrayList callbacks) { + this.callbacks = callbacks; + } + + /** + * This returns the PathCallbacks of this PathChain in an ArrayList. + * + * @return returns the PathCallbacks. + */ + public ArrayList getCallbacks() { + return callbacks; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java new file mode 100644 index 0000000..95a5f05 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -0,0 +1,203 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import androidx.annotation.NonNull; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; + +/** + * This is the Point class. This class handles storing information about the location of points in + * 2D space in both Cartesian, or rectangular, and polar coordinates. Additionally, this contains + * the method to find the distance between two Points. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class Point { + + // IMPORTANT NOTE: theta is defined in radians. + // These are the values of the coordinate defined by this Point, in both polar and + // Cartesian systems. + private double r; + private double theta; + private double x; + private double y; + + // these are used for ease of changing/setting identification + public static final int POLAR = 0; + public static final int CARTESIAN = 1; + + + /** + * This creates a new Point with coordinate inputs and a specified coordinate system. + * + * @param rOrX Depending on the coordinate system specified, this is either the r or x value. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. + * @param thetaOrY Depending on the coordinate system specified, this is either the theta or + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. + * @param identifier this specifies what coordinate system the coordinate inputs are in. + */ + public Point(double rOrX, double thetaOrY, int identifier) { + setCoordinates(rOrX, thetaOrY, identifier); + } + + /** + * This creates a new Point from a Pose. + * + * @param pose the Pose. + */ + public Point(Pose pose) { + setCoordinates(pose.getX(), pose.getY(), CARTESIAN); + } + + /** + * This creates a new Point from a X and Y value. + * + * @param setX the X value. + * @param setY the Y value. + */ + public Point(double setX, double setY) { + setCoordinates(setX, setY, CARTESIAN); + } + + /** + * This sets the coordinates of the Point using the specified coordinate system. + * + * @param rOrX Depending on the coordinate system specified, this is either the r or x value. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. + * @param thetaOrY Depending on the coordinate system specified, this is either the theta or + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. + * @param identifier this specifies what coordinate system to use when setting values. + */ + public void setCoordinates(double rOrX, double thetaOrY, int identifier) { + double[] setOtherCoordinates; + switch (identifier) { // this detects which coordinate system to use + // there is no POLAR case since that's covered by the default + case CARTESIAN: + x = rOrX; + y = thetaOrY; + setOtherCoordinates = cartesianToPolar(x, y); + r = setOtherCoordinates[0]; + theta = setOtherCoordinates[1]; + break; + default: + if (rOrX < 0) { + r = -rOrX; + theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI); + } else { + r = rOrX; + theta = MathFunctions.normalizeAngle(thetaOrY); + } + setOtherCoordinates = polarToCartesian(r, theta); + x = setOtherCoordinates[0]; + y = setOtherCoordinates[1]; + break; + } + } + + /** + * Calculates the distance between this Point and some other specified Point. + * + * @param otherPoint the other specified Point. + * @return returns the distance between the two Points. + */ + public double distanceFrom(Point otherPoint) { + return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2)); + } + + /** + * This takes in an r and theta value and converts them to Cartesian coordinates. + * + * @param r this is the r value of the Point being converted. + * @param theta this is the theta value of the Point being converted. + * @return this returns the x and y values, in that order, in an Array of doubles. + */ + public static double[] polarToCartesian(double r, double theta) { + return new double[]{r * Math.cos(theta), r * Math.sin(theta)}; + } + + /** + * This takes in an x and y value and converts them to polar coordinates. + * + * @param x this is the x value of the Point being converted. + * @param y this is the y value of the Point being converted. + * @return this returns the r and theta values, in that order, in an Array of doubles. + */ + public static double[] cartesianToPolar(double x, double y) { + if (x == 0) { + if (y > 0) { + return new double[]{Math.abs(y), Math.PI / 2}; + } else { + return new double[]{Math.abs(y), (3 * Math.PI) / 2}; + } + } + double r = Math.sqrt(x * x + y * y); + if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)}; + if (y > 0) { + return new double[]{r, Math.atan(y / x)}; + } else { + return new double[]{r, (2 * Math.PI) + Math.atan(y / x)}; + } + } + + /** + * Returns the r value of this Point. This is a polar coordinate value. + * + * @return returns the r value. + */ + public double getR() { + return r; + } + + /** + * Returns the theta value of this Point. This is a polar coordinate value. + * + * @return returns the theta value. + */ + public double getTheta() { + return theta; + } + + /** + * Returns the x value of this Point. This is a Cartesian coordinate value. + * + * @return returns the x value. + */ + public double getX() { + return x; + } + + /** + * Returns the y value of this Point. This is a Cartesian coordinate value. + * + * @return returns the y value. + */ + public double getY() { + return y; + } + + /** + * This creates a new Point with the same information as this Point, just pointing to a different + * memory location. In other words, a deep copy. + * + * @return returns a copy of this Point. + */ + public Point copy() { + return new Point(getX(), getY(), CARTESIAN); + } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ")"; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java new file mode 100644 index 0000000..f1a093b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java @@ -0,0 +1,141 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +/** + * This is the Point class. This class handles storing information about vectors, which are + * basically Points but using polar coordinates as the default. The main reason this class exists + * is because some vector math needs to be done in the Follower, and dot products and cross + * products of Points just don't seem right. Also, there are a few more methods in here that make + * using Vectors a little easier than using a Point in polar coordinates. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class Vector { + + // IMPORTANT NOTE: theta is defined in radians. + // These are the values of the coordinate defined by this Point, in both polar and + // Cartesian systems. + private double magnitude; + private double theta; + private double xComponent; + private double yComponent; + + /** + * This creates a new Vector with zero magnitude and direction. + */ + public Vector() { + setComponents(0, 0); + } + + /** + * This creates a new Vector with a specified magnitude and direction. + * + * @param magnitude magnitude of the Vector. + * @param theta the direction of the Vector in radians. + */ + public Vector(double magnitude, double theta) { + setComponents(magnitude, theta); + } + + /** + * This sets the components of the Vector in regular vector coordinates. + * + * @param magnitude sets the magnitude of this Vector. + * @param theta sets the theta value of this Vector. + */ + public void setComponents(double magnitude, double theta) { + double[] orthogonalComponents; + if (magnitude<0) { + this.magnitude = -magnitude; + this.theta = MathFunctions.normalizeAngle(theta+Math.PI); + } else { + this.magnitude = magnitude; + this.theta = MathFunctions.normalizeAngle(theta); + } + orthogonalComponents = Point.polarToCartesian(magnitude, theta); + xComponent = orthogonalComponents[0]; + yComponent = orthogonalComponents[1]; + } + + /** + * This sets only the magnitude of the Vector. + * + * @param magnitude sets the magnitude of this Vector. + */ + public void setMagnitude(double magnitude) { + setComponents(magnitude, theta); + } + + /** + * This sets only the angle, theta, of the Vector. + * + * @param theta sets the angle, or theta value, of this Vector. + */ + public void setTheta(double theta) { + setComponents(magnitude, theta); + } + + /** + * This rotates the Vector by an angle, theta. + * + * @param theta2 the angle to be added. + */ + public void rotateVector(double theta2) { + setTheta(theta+theta2); + } + + /** + * This sets the orthogonal components of the Vector. These orthogonal components are assumed + * to be in the direction of the x-axis and y-axis. In other words, this is setting the + * coordinates of the Vector using x and y coordinates instead of a direction and magnitude. + * + * @param xComponent sets the x component of this Vector. + * @param yComponent sets the y component of this Vector. + */ + public void setOrthogonalComponents(double xComponent, double yComponent) { + double[] polarComponents; + this.xComponent = xComponent; + this.yComponent = yComponent; + polarComponents = Point.cartesianToPolar(xComponent, yComponent); + magnitude = polarComponents[0]; + theta = polarComponents[1]; + } + + /** + * Returns the magnitude of this Vector. + * + * @return returns the magnitude. + */ + public double getMagnitude() { + return magnitude; + } + + /** + * Returns the theta value, or angle, of this Vector. + * + * @return returns the theta value. + */ + public double getTheta() { + return theta; + } + + /** + * Returns the x component of this Vector. + * + * @return returns the x component. + */ + public double getXComponent() { + return xComponent; + } + + /** + * Returns the y component of this Vector. + * + * @return returns the y component. + */ + public double getYComponent() { + return yComponent; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java new file mode 100644 index 0000000..196b378 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous (name = "Circle", group = "Autonomous Pathing Tuning") +public class Circle extends OpMode { + private Telemetry telemetryA; + + public static double RADIUS = 10; + + private Follower follower; + + private PathChain circle; + + /** + * This initializes the Follower and creates the PathChain for the "circle". Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + circle = follower.pathBuilder() + .addPath(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(RADIUS,0, Point.CARTESIAN), new Point(RADIUS, RADIUS, Point.CARTESIAN))) + .addPath(new BezierCurve(new Point(RADIUS, RADIUS, Point.CARTESIAN), new Point(RADIUS,2*RADIUS, Point.CARTESIAN), new Point(0,2*RADIUS, Point.CARTESIAN))) + .addPath(new BezierCurve(new Point(0,2*RADIUS, Point.CARTESIAN), new Point(-RADIUS,2*RADIUS, Point.CARTESIAN), new Point(-RADIUS, RADIUS, Point.CARTESIAN))) + .addPath(new BezierCurve(new Point(-RADIUS, RADIUS, Point.CARTESIAN), new Point(-RADIUS,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))) + .build(); + + follower.followPath(circle); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will run in a roughly circular shape of radius " + RADIUS + + ", starting on the right-most edge. So, make sure you have enough " + + "space to the left, front, and back to run the OpMode."); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java new file mode 100644 index 0000000..6ff548f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java @@ -0,0 +1,85 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the CurvedBackAndForth autonomous OpMode. It runs the robot in a specified distance + * forward and to the left. On reaching the end of the forward Path, the robot runs the backward + * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety + * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the + * centripetal Vector. Remember to test your tunings on StraightBackAndForth as well, since tunings + * that work well for curves might have issues going in straight lines. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous (name = "Curved Back And Forth", group = "Autonomous Pathing Tuning") +public class CurvedBackAndForth extends OpMode { + private Telemetry telemetryA; + + public static double DISTANCE = 20; + + private boolean forward = true; + + private Follower follower; + + private Path forwards; + private Path backwards; + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN))); + backwards = new Path(new BezierCurve(new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + + backwards.setReversed(true); + + follower.followPath(forwards); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will run the robot in a curve going " + DISTANCE + " inches" + + " to the left and the same number of inches forward. The robot will go" + + "forward and backward continuously along the path. Make sure you have" + + "enough room."); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryA.addData("going forward", forward); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java new file mode 100644 index 0000000..e3f25b4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -0,0 +1,217 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; + +/** + * This is the FollowerConstants class. It holds many constants and parameters for various parts of + * the Follower. This is here to allow for easier tuning of Pedro Pathing, as well as concentrate + * everything tunable for the Paths themselves in one place. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +@Config +public class FollowerConstants { + + // This section is for configuring your motors + public static String leftFrontMotorName = "leftFront"; + public static String leftRearMotorName = "leftRear"; + public static String rightFrontMotorName = "rightFront"; + public static String rightRearMotorName = "rightRear"; + + public static DcMotorSimple.Direction leftFrontMotorDirection = DcMotorSimple.Direction.REVERSE; + public static DcMotorSimple.Direction rightFrontMotorDirection = DcMotorSimple.Direction.REVERSE; + public static DcMotorSimple.Direction leftRearMotorDirection = DcMotorSimple.Direction.FORWARD; + public static DcMotorSimple.Direction rightRearMotorDirection = DcMotorSimple.Direction.FORWARD; + + // This section is for setting the actual drive vector for the front left wheel, if the robot + // is facing a heading of 0 radians with the wheel centered at (0,0) + private static double xMovement = 81.34056; + private static double yMovement = 65.43028; + private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); + public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1])); + + + // Translational PIDF coefficients (don't use integral) + public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients( + 0.1, + 0, + 0, + 0); + + // Translational Integral + public static CustomPIDFCoefficients translationalIntegral = new CustomPIDFCoefficients( + 0, + 0, + 0, + 0); + + // Feed forward constant added on to the translational PIDF + public static double translationalPIDFFeedForward = 0.015; + + + // Heading error PIDF coefficients + public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( + 1, + 0, + 0, + 0); + + // Feed forward constant added on to the heading PIDF + public static double headingPIDFFeedForward = 0.01; + + + // Drive PIDF coefficients + public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients( + 0.025, + 0, + 0.00001, + 0.6, + 0); + + // Feed forward constant added on to the drive PIDF + public static double drivePIDFFeedForward = 0.01; + + // Kalman filter parameters for the drive error Kalman filter + public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( + 6, + 1); + + + // Mass of robot in kilograms + public static double mass = 10.65942; + + // Centripetal force to power scaling + public static double centripetalScaling = 0.0005; + + + // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) + // if not negative, then the robot thinks that its going to go faster under 0 power + public static double forwardZeroPowerAcceleration = -34.62719; + + // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) + // if not negative, then the robot thinks that its going to go faster under 0 power + public static double lateralZeroPowerAcceleration = -78.15554; + + // A multiplier for the zero power acceleration to change the speed the robot decelerates at + // the end of paths. + // Increasing this will cause the robot to try to decelerate faster, at the risk of overshoots + // or localization slippage. + // Decreasing this will cause the deceleration at the end of the Path to be slower, making the + // robot slower but reducing risk of end-of-path overshoots or localization slippage. + // This can be set individually for each Path, but this is the default. + public static double zeroPowerAccelerationMultiplier = 4; + + + // When the robot is at the end of its current Path or PathChain and the velocity goes below + // this value, then end the Path. This is in inches/second. + // This can be custom set for each Path. + public static double pathEndVelocityConstraint = 0.1; + + // When the robot is at the end of its current Path or PathChain and the translational error + // goes below this value, then end the Path. This is in inches. + // This can be custom set for each Path. + public static double pathEndTranslationalConstraint = 0.1; + + // When the robot is at the end of its current Path or PathChain and the heading error goes + // below this value, then end the Path. This is in radians. + // This can be custom set for each Path. + public static double pathEndHeadingConstraint = 0.007; + + // When the t-value of the closest point to the robot on the Path is greater than this value, + // then the Path is considered at its end. + // This can be custom set for each Path. + public static double pathEndTValueConstraint = 0.995; + + // When the Path is considered at its end parametrically, then the Follower has this many + // milliseconds to further correct by default. + // This can be custom set for each Path. + public static double pathEndTimeoutConstraint = 500; + + // This is how many steps the BezierCurve class uses to approximate the length of a BezierCurve. + public static int APPROXIMATION_STEPS = 1000; + + // This is scales the translational error correction power when the Follower is holding a Point. + public static double holdPointTranslationalScaling = 0.45; + + // This is scales the heading error correction power when the Follower is holding a Point. + public static double holdPointHeadingScaling = 0.35; + + // This is the number of times the velocity is recorded for averaging when approximating a first + // and second derivative for on the fly centripetal correction. The velocity is calculated using + // half of this number of samples, and the acceleration uses all of this number of samples. + public static int AVERAGED_VELOCITY_SAMPLE_NUMBER = 8; + + // This is the number of steps the binary search for closest point uses. More steps is more + // accuracy, and this increases at an exponential rate. However, more steps also does take more + // time. + public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10; + + + // These activate / deactivate the secondary PIDs. These take over at errors under a set limit for + // the translational, heading, and drive PIDs. + public static boolean useSecondaryTranslationalPID = false; + public static boolean useSecondaryHeadingPID = false; + public static boolean useSecondaryDrivePID = false; + + + // the limit at which the translational PIDF switches between the main and secondary translational PIDFs, + // if the secondary PID is active + public static double translationalPIDFSwitch = 3; + + // Secondary translational PIDF coefficients (don't use integral) + public static CustomPIDFCoefficients secondaryTranslationalPIDFCoefficients = new CustomPIDFCoefficients( + 0.3, + 0, + 0.01, + 0); + + // Secondary translational Integral value + public static CustomPIDFCoefficients secondaryTranslationalIntegral = new CustomPIDFCoefficients( + 0, + 0, + 0, + 0); + + // Feed forward constant added on to the small translational PIDF + public static double secondaryTranslationalPIDFFeedForward = 0.015; + + + // the limit at which the heading PIDF switches between the main and secondary heading PIDFs + public static double headingPIDFSwitch = Math.PI / 20; + + // Secondary heading error PIDF coefficients + public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 5, + 0, + 0.08, + 0); + + // Feed forward constant added on to the secondary heading PIDF + public static double secondaryHeadingPIDFFeedForward = 0.01; + + + // the limit at which the heading PIDF switches between the main and secondary drive PIDFs + public static double drivePIDFSwitch = 20; + + // Secondary drive PIDF coefficients + public static CustomFilteredPIDFCoefficients secondaryDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( + 0.02, + 0, + 0.000005, + 0.6, + 0); + + // Feed forward constant added on to the secondary drive PIDF + public static double secondaryDrivePIDFFeedForward = 0.01; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java new file mode 100644 index 0000000..cb64063 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -0,0 +1,166 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the ForwardVelocityTuner autonomous tuning OpMode. This runs the robot forwards at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * You can adjust the distance the robot will travel on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +public class ForwardVelocityTuner extends OpMode { + private ArrayList velocities = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private Telemetry telemetryA; + + private boolean end; + + /** + * This initializes the drive motors as well as the cache of velocities and the FTC Dashboard + * telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + + } + + /** + * This starts the OpMode by setting the drive motors to run forward at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(1); + rightFront.setPower(1); + rightRear.setPower(1); + end = false; + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run forward enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + requestOpModeStop(); + } + + poseUpdater.update(); + if (!end) { + if (Math.abs(poseUpdater.getPose().getX()) > DISTANCE) { + end = true; + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + } else { + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, 0))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + leftFront.setPower(0); + leftRear.setPower(0); + rightFront.setPower(0); + rightRear.setPower(0); + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + double average = 0; + for (Double velocity : velocities) { + average += velocity; + } + average /= (double) velocities.size(); + + telemetryA.addData("forward velocity:", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java new file mode 100644 index 0000000..e774504 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -0,0 +1,165 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the ForwardZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +public class ForwardZeroPowerAccelerationTuner extends OpMode { + private ArrayList accelerations = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double VELOCITY = 30; + + private double previousVelocity; + + private long previousTimeNano; + + private Telemetry telemetryA; + + private boolean stopping; + private boolean end; + + /** + * This initializes the drive motors as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetryA.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryA.addLine("Make sure you have enough room."); + telemetryA.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + } + + /** + * This starts the OpMode by setting the drive motors to run forward at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(1); + rightFront.setPower(1); + rightRear.setPower(1); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + requestOpModeStop(); + } + + poseUpdater.update(); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading()); + if (!end) { + if (!stopping) { + if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { + previousVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + previousTimeNano = System.nanoTime(); + stopping = true; + for (DcMotorEx motor : motors) { + motor.setPower(0); + } + } + } else { + double currentVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < FollowerConstants.pathEndVelocityConstraint) { + end = true; + } + } + } else { + double average = 0; + for (Double acceleration : accelerations) { + average += acceleration; + } + average /= (double) accelerations.size(); + + telemetryA.addData("forward zero power acceleration (deceleration):", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java new file mode 100644 index 0000000..db4e903 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -0,0 +1,165 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the LateralZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +public class LateralZeroPowerAccelerationTuner extends OpMode { + private ArrayList accelerations = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double VELOCITY = 30; + + private double previousVelocity; + + private long previousTimeNano; + + private Telemetry telemetryA; + + private boolean stopping; + private boolean end; + + /** + * This initializes the drive motors as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); + telemetryA.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryA.addLine("Make sure you have enough room."); + telemetryA.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + } + + /** + * This starts the OpMode by setting the drive motors to run forward at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(-1); + rightFront.setPower(-1); + rightRear.setPower(1); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + requestOpModeStop(); + } + + poseUpdater.update(); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); + if (!end) { + if (!stopping) { + if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { + previousVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + previousTimeNano = System.nanoTime(); + stopping = true; + for (DcMotorEx motor : motors) { + motor.setPower(0); + } + } + } else { + double currentVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < FollowerConstants.pathEndVelocityConstraint) { + end = true; + } + } + } else { + double average = 0; + for (Double acceleration : accelerations) { + average += acceleration; + } + average /= (double) accelerations.size(); + + telemetryA.addData("lateral zero power acceleration (deceleration):", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java new file mode 100644 index 0000000..8d51c08 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -0,0 +1,164 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the StrafeVelocityTuner autonomous tuning OpMode. This runs the robot right at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * You can adjust the distance the robot will travel on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +public class StrafeVelocityTuner extends OpMode { + private ArrayList velocities = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private Telemetry telemetryA; + + private boolean end; + + /** + * This initializes the drive motors as well as the cache of velocities and the FTC Dashboard + * telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); + telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + } + + /** + * This starts the OpMode by setting the drive motors to run right at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(-1); + rightFront.setPower(-1); + rightRear.setPower(1); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run sideways enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + requestOpModeStop(); + } + + poseUpdater.update(); + if (!end) { + if (Math.abs(poseUpdater.getPose().getY()) > DISTANCE) { + end = true; + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + } else { + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + leftFront.setPower(0); + leftRear.setPower(0); + rightFront.setPower(0); + rightRear.setPower(0); + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + double average = 0; + for (Double velocity : velocities) { + average += velocity; + } + average /= (double) velocities.size(); + + telemetryA.addData("strafe velocity:", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java new file mode 100644 index 0000000..6224a92 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the StraightBackAndForth autonomous OpMode. It runs the robot in a specified distance + * straight forward. On reaching the end of the forward Path, the robot runs the backward Path the + * same distance back to the start. Rinse and repeat! This is good for testing a variety of Vectors, + * like the drive Vector, the translational Vector, and the heading Vector. Remember to test your + * tunings on CurvedBackAndForth as well, since tunings that work well for straight lines might + * have issues going in curves. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous (name = "Straight Back And Forth", group = "Autonomous Pathing Tuning") +public class StraightBackAndForth extends OpMode { + private Telemetry telemetryA; + + public static double DISTANCE = 40; + + private boolean forward = true; + + private Follower follower; + + private Path forwards; + private Path backwards; + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + forwards = new Path(new BezierLine(new Point(0,0, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + backwards.setConstantHeadingInterpolation(0); + + follower.followPath(forwards); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will run the robot in a straight line going " + DISTANCE + + " inches forward. The robot will go forward and backward continuously" + + " along the path. Make sure you have enough room."); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryA.addData("going forward", forward); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java new file mode 100644 index 0000000..0b02174 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import kotlin.jvm.JvmField; + +/** + * This is the CustomFilteredPIDFCoefficients class. This class handles holding coefficients for filtered PIDF + * controllers. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/15/2024 + */ +public class CustomFilteredPIDFCoefficients { + @JvmField public double P; + @JvmField public double I; + @JvmField public double D; + @JvmField public double T; + @JvmField public double F; + + public FeedForwardConstant feedForwardConstantEquation; + + private boolean usingEquation; + + /** + * This creates a new CustomFilteredPIDFCoefficients with constant coefficients. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param t the time constant for the filter + * @param f the coefficient for the feedforward factor. + */ + public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, double f) { + P = p; + I = i; + D = d; + T = t; + F = f; + } + + /** + * This creates a new CustomFilteredPIDFCoefficients with constant PID coefficients and a variable + * feedforward equation using a FeedForwardConstant. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param t the time constant for the filter + * @param f the equation for the feedforward factor. + */ + public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, FeedForwardConstant f) { + usingEquation = true; + P = p; + I = i; + D = d; + T = t; + feedForwardConstantEquation = f; + } + + /** + * This returns the coefficient for the feedforward factor. + * + * @param input this is inputted into the feedforward equation, if applicable. If there's no + * equation, then any input can be used. + * @return This returns the coefficient for the feedforward factor. + */ + public double getCoefficient(double input) { + if (!usingEquation) return F; + return feedForwardConstantEquation.getConstant(input); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java new file mode 100644 index 0000000..65a783a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import kotlin.jvm.JvmField; + +/** + * This is the CustomPIDFCoefficients class. This class handles holding coefficients for PIDF + * controllers. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class CustomPIDFCoefficients { + @JvmField public double P; + @JvmField public double I; + @JvmField public double D; + @JvmField public double F; + + public FeedForwardConstant feedForwardConstantEquation; + + private boolean usingEquation; + + /** + * This creates a new CustomPIDFCoefficients with constant coefficients. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param f the coefficient for the feedforward factor. + */ + public CustomPIDFCoefficients(double p, double i, double d, double f) { + P = p; + I = i; + D = d; + F = f; + } + + /** + * This creates a new CustomPIDFCoefficients with constant PID coefficients and a variable + * feedforward equation using a FeedForwardConstant. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param f the equation for the feedforward factor. + */ + public CustomPIDFCoefficients(double p, double i, double d, FeedForwardConstant f) { + usingEquation = true; + P = p; + I = i; + D = d; + feedForwardConstantEquation = f; + } + + /** + * This returns the coefficient for the feedforward factor. + * + * @param input this is inputted into the feedforward equation, if applicable. If there's no + * equation, then any input can be used. + * @return This returns the coefficient for the feedforward factor. + */ + public double getCoefficient(double input) { + if (!usingEquation) return F; + return feedForwardConstantEquation.getConstant(input); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java new file mode 100644 index 0000000..3c54156 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +import java.util.ArrayList; + +/** + * This is the DashboardPoseTracker class. This tracks the pose history of the robot through a + * PoseUpdater, adding to the pose history at specified increments of time and storing the history + * for a specified length of time. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/13/2024 + */ +public class DashboardPoseTracker { + private double[] xPositions; + private double[] yPositions; + private PoseUpdater poseUpdater; + private long lastUpdateTime; + private final int TRACKING_LENGTH = 1500; + private final long UPDATE_TIME = 50; + private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; + + /** + * This creates a new DashboardPoseTracker from a PoseUpdater. + * + * @param poseUpdater the PoseUpdater + */ + public DashboardPoseTracker(PoseUpdater poseUpdater) { + this.poseUpdater = poseUpdater; + xPositions = new double[TRACKING_SIZE]; + yPositions = new double[TRACKING_SIZE]; + + for (int i = 0; i < TRACKING_SIZE; i++) { + xPositions[i] = poseUpdater.getPose().getX(); + yPositions[i] = poseUpdater.getPose().getY(); + } + + lastUpdateTime = System.currentTimeMillis() - UPDATE_TIME; + } + + /** + * This updates the DashboardPoseTracker. When the specified update time has passed from the last + * pose history log, another pose can be logged. The least recent log is also removed. + */ + public void update() { + if (System.currentTimeMillis() - lastUpdateTime > UPDATE_TIME) { + lastUpdateTime = System.currentTimeMillis(); + for (int i = TRACKING_SIZE - 1; i > 0; i--) { + xPositions[i] = xPositions[i - 1]; + yPositions[i] = yPositions[i - 1]; + } + xPositions[0] = poseUpdater.getPose().getX(); + yPositions[0] = poseUpdater.getPose().getY(); + } + } + + /** + * This returns the x positions of the pose history as an Array of doubles. + * + * @return returns the x positions of the pose history + */ + public double[] getXPositionsArray() { + return xPositions; + } + + /** + * This returns the y positions of the pose history as an Array of doubles. + * + * @return returns the y positions of the pose history + */ + public double[] getYPositionsArray() { + return yPositions; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java new file mode 100644 index 0000000..9af9662 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -0,0 +1,155 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot. + * + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/22/2024 + */ +public class Drawing { + public static final double ROBOT_RADIUS = 9; + + private static TelemetryPacket packet; + + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * + * @param follower + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), "#3F51B5"); + Point closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5"); + } + drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); + drawRobot(follower.getPose(), "#4CAF50"); + + sendPacket(); + } + + /** + * This adds instructions to the current packet to draw a robot at a specified Pose with a specified + * color. If no packet exists, then a new one is created. + * + * @param pose the Pose to draw the robot at + * @param color the color to draw the robot with + */ + public static void drawRobot(Pose pose, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy()); + } + + /** + * This adds instructions to the current packet to draw a Path with a specified color. If no + * packet exists, then a new one is created. + * + * @param path the Path to draw + * @param color the color to draw the Path with + */ + public static void drawPath(Path path, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + Drawing.drawPath(packet.fieldOverlay(), path.getDashboardDrawingPoints()); + } + + /** + * This adds instructions to the current packet to draw all the Paths in a PathChain with a + * specified color. If no packet exists, then a new one is created. + * + * @param pathChain the PathChain to draw + * @param color the color to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, String color) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), color); + } + } + + /** + * This adds instructions to the current packet to draw the pose history of the robot. If no + * packet exists, then a new one is created. + * + * @param poseTracker the DashboardPoseTracker to get the pose history from + * @param color the color to draw the pose history with + */ + public static void drawPoseHistory(DashboardPoseTracker poseTracker, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray()); + } + + /** + * This tries to send the current packet to FTC Dashboard. + * + * @return returns if the operation was successful. + */ + public static boolean sendPacket() { + if (packet != null) { + FtcDashboard.getInstance().sendTelemetryPacket(packet); + packet = null; + return true; + } + return false; + } + + /** + * This draws a robot on the Dashboard at a specified Point. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param t the Point to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Point t) { + c.setStrokeWidth(1); + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + + Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta()); + Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); + Vector p2 = MathFunctions.addVectors(p1, halfv); + c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); + } + + /** + * This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the + * actual robot, since the Pose contains the direction the robot is facing as well as its position. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param t the Pose to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Pose t) { + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + Vector v = t.getHeadingVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; + double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); + c.strokeLine(x1, y1, x2, y2); + } + + /** + * This draws a Path on the Dashboard from a specified Array of Points. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param points the Points to draw + */ + public static void drawPath(Canvas c, double[][] points) { + c.strokePolyline(points[0], points[1]); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java new file mode 100644 index 0000000..0085e7c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the FeedForwardConstant interface. This interface holds a feedforward equation for PIDFs. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public interface FeedForwardConstant { + + /** + * This returns the coefficient for the feedforward factor. + * + * @param input this is inputted into the feedforward equation, if applicable. If there's no + * equation, then any input can be used. + * @return This returns the coefficient for the feedforward factor. + */ + double getConstant(double input); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java new file mode 100644 index 0000000..07747fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java @@ -0,0 +1,245 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the FilteredPIDFController class. This class handles the running of filtered filtered PIDFs. This + * behaves very similarly to a regular filtered PIDF controller, but the derivative portion is filtered with + * a low pass filter to reduce high frequency noise that could affect results. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/15/2024 + */ +public class FilteredPIDFController { + private CustomFilteredPIDFCoefficients coefficients; + + private double previousError; + private double error; + private double position; + private double targetPosition; + private double errorIntegral; + private double errorDerivative; + private double previousDerivative; + private double filteredDerivative; + private double feedForwardInput; + + private long previousUpdateTimeNano; + private long deltaTimeNano; + + /** + * This creates a new filtered PIDFController from a CustomPIDFCoefficients. + * + * @param set the coefficients to use. + */ + public FilteredPIDFController(CustomFilteredPIDFCoefficients set) { + setCoefficients(set); + reset(); + } + + /** + * This takes the current error and runs the filtered PIDF on it. + * + * @return this returns the value of the filtered PIDF from the current error. + */ + public double runPIDF() { + return error * P() + filteredDerivative * D() + errorIntegral * I() + F(); + } + + /** + * This can be used to update the filtered PIDF's current position when inputting a current position and + * a target position to calculate error. This will update the error from the current position to + * the target position specified. + * + * @param update This is the current position. + */ + public void updatePosition(double update) { + position = update; + previousError = error; + error = targetPosition - position; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + previousDerivative = filteredDerivative; + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative; + } + + /** + * As opposed to updating position against a target position, this just sets the error to some + * specified value. + * + * @param error The error specified. + */ + public void updateError(double error) { + previousError = this.error; + this.error = error; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + previousDerivative = errorDerivative; + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative; + } + + /** + * This can be used to update the feedforward equation's input, if applicable. + * + * @param input the input into the feedforward equation. + */ + public void updateFeedForwardInput(double input) { + feedForwardInput = input; + } + + /** + * This resets all the filtered PIDF's error and position values, as well as the time stamps. + */ + public void reset() { + previousError = 0; + error = 0; + position = 0; + targetPosition = 0; + errorIntegral = 0; + errorDerivative = 0; + previousDerivative = 0; + filteredDerivative = 0; + previousUpdateTimeNano = System.nanoTime(); + } + + /** + * This is used to set the target position if the filtered PIDF is being run with current position and + * target position inputs rather than error inputs. + * + * @param set this sets the target position. + */ + public void setTargetPosition(double set) { + targetPosition = set; + } + + /** + * This returns the target position of the filtered PIDF. + * + * @return this returns the target position. + */ + public double getTargetPosition() { + return targetPosition; + } + + /** + * This is used to set the coefficients of the filtered PIDF. + * + * @param set the coefficients that the filtered PIDF will use. + */ + public void setCoefficients(CustomFilteredPIDFCoefficients set) { + coefficients = set; + } + + /** + * This returns the filtered PIDF's current coefficients. + * + * @return this returns the current coefficients. + */ + public CustomFilteredPIDFCoefficients getCoefficients() { + return coefficients; + } + + /** + * This sets the proportional (P) coefficient of the filtered PIDF only. + * + * @param set this sets the P coefficient. + */ + public void setP(double set) { + coefficients.P = set; + } + + /** + * This returns the proportional (P) coefficient of the filtered PIDF. + * + * @return this returns the P coefficient. + */ + public double P() { + return coefficients.P; + } + + /** + * This sets the integral (I) coefficient of the filtered PIDF only. + * + * @param set this sets the I coefficient. + */ + public void setI(double set) { + coefficients.I = set; + } + + /** + * This returns the integral (I) coefficient of the filtered PIDF. + * + * @return this returns the I coefficient. + */ + public double I() { + return coefficients.I; + } + + /** + * This sets the derivative (D) coefficient of the filtered PIDF only. + * + * @param set this sets the D coefficient. + */ + public void setD(double set) { + coefficients.D = set; + } + + /** + * This returns the derivative (D) coefficient of the filtered PIDF. + * + * @return this returns the D coefficient. + */ + public double D() { + return coefficients.D; + } + + /** + * This sets the time constant (T) of the filtered PIDF only. + * + * @param set this sets the time constant. + */ + public void setT(double set) { + coefficients.T = set; + } + + /** + * This returns the time constant (T) of the filtered PIDF. + * + * @return this returns the time constant. + */ + public double T() { + return coefficients.T; + } + + /** + * This sets the feedforward (F) constant of the filtered PIDF only. + * + * @param set this sets the F constant. + */ + public void setF(double set) { + coefficients.F = set; + } + + /** + * This returns the feedforward (F) constant of the filtered PIDF. + * + * @return this returns the F constant. + */ + public double F() { + return coefficients.getCoefficient(feedForwardInput); + } + + /** + * This returns the current error of the filtered PIDF. + * + * @return this returns the error. + */ + public double getError() { + return error; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java new file mode 100644 index 0000000..41aa630 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/** + * This is the KalmanFilter class. This creates a Kalman filter that is used to smooth out data. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/17/2024 + */ +public class KalmanFilter { + private KalmanFilterParameters parameters; + private double state; + private double variance; + private double kalmanGain; + private double previousState; + private double previousVariance; + + public KalmanFilter(KalmanFilterParameters parameters) { + this.parameters = parameters; + reset(); + } + + public KalmanFilter(KalmanFilterParameters parameters, double startState, double startVariance, double startGain) { + this.parameters = parameters; + reset(startState, startVariance, startGain); + } + + public void reset(double startState, double startVariance, double startGain) { + state = startState; + previousState = startState; + variance = startVariance; + previousVariance = startVariance; + kalmanGain = startGain; + } + + public void reset() { + reset(0, 1, 1); + } + + public void update(double updateData, double updateProjection) { + state = previousState + updateData; + variance = previousVariance + parameters.modelCovariance; + kalmanGain = variance / (variance + parameters.dataCovariance); + state += kalmanGain * (updateProjection - state); + variance *= (1.0 - kalmanGain); + previousState = state; + previousVariance = variance; + } + + public double getState() { + return state; + } + + public void debug(Telemetry telemetry) { + telemetry.addData("state", state); + telemetry.addData("variance", variance); + telemetry.addData("Kalman gain", kalmanGain); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java new file mode 100644 index 0000000..e31260b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import kotlin.jvm.JvmField; + +/** + * This is the KalmanFilterParameters class. This class handles holding parameters Kalman filters. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/17/2024 + */ +public class KalmanFilterParameters { + @JvmField public double modelCovariance; + @JvmField public double dataCovariance; + + /** + * This creates a new KalmanFilterParameters with a specified model and data covariance. + * + * @param modelCovariance the covariance of the model. + * @param dataCovariance the covariance of the data. + */ + public KalmanFilterParameters(double modelCovariance, double dataCovariance) { + this.modelCovariance = modelCovariance; + this.dataCovariance = dataCovariance; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java new file mode 100644 index 0000000..6501093 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the NanoTimer class. It is an elapsed time clock with nanosecond precision, or at least + * as precise as the System.nanoTime() is. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class NanoTimer { + private long startTime; + + /** + * This creates a new NanoTimer with the start time set to its creation time. + */ + public NanoTimer() { + resetTimer(); + } + + /** + * This resets the NanoTimer's start time to the current time using System.nanoTime(). + */ + public void resetTimer() { + startTime = System.nanoTime(); + } + + /** + * This returns the elapsed time in nanoseconds since the start time of the NanoTimer. + * + * @return this returns the elapsed time in nanoseconds. + */ + public long getElapsedTime() { + return System.nanoTime() - startTime; + } + + /** + * This returns the elapsed time in seconds since the start time of the NanoTimer. + * + * @return this returns the elapsed time in seconds. + */ + public double getElapsedTimeSeconds() { + return (getElapsedTime() / Math.pow(10.0,9)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java new file mode 100644 index 0000000..0b59050 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java @@ -0,0 +1,223 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the PIDFController class. This class handles the running of PIDFs. PIDF stands for + * proportional, integral, derivative, and feedforward. PIDFs take the error of a system as an input. + * Coefficients multiply into the error, the integral of the error, the derivative of the error, and + * a feedforward value. Then, these values are added up and returned. In this way, error in the + * system is corrected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class PIDFController { + private CustomPIDFCoefficients coefficients; + + private double previousError; + private double error; + private double position; + private double targetPosition; + private double errorIntegral; + private double errorDerivative; + private double feedForwardInput; + + private long previousUpdateTimeNano; + private long deltaTimeNano; + + /** + * This creates a new PIDFController from a CustomPIDFCoefficients. + * + * @param set the coefficients to use. + */ + public PIDFController(CustomPIDFCoefficients set) { + setCoefficients(set); + reset(); + } + + /** + * This takes the current error and runs the PIDF on it. + * + * @return this returns the value of the PIDF from the current error. + */ + public double runPIDF() { + return error * P() + errorDerivative * D() + errorIntegral * I() + F(); + } + + /** + * This can be used to update the PIDF's current position when inputting a current position and + * a target position to calculate error. This will update the error from the current position to + * the target position specified. + * + * @param update This is the current position. + */ + public void updatePosition(double update) { + position = update; + previousError = error; + error = targetPosition - position; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + } + + /** + * As opposed to updating position against a target position, this just sets the error to some + * specified value. + * + * @param error The error specified. + */ + public void updateError(double error) { + previousError = this.error; + this.error = error; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + } + + /** + * This can be used to update the feedforward equation's input, if applicable. + * + * @param input the input into the feedforward equation. + */ + public void updateFeedForwardInput(double input) { + feedForwardInput = input; + } + + /** + * This resets all the PIDF's error and position values, as well as the time stamps. + */ + public void reset() { + previousError = 0; + error = 0; + position = 0; + targetPosition = 0; + errorIntegral = 0; + errorDerivative = 0; + previousUpdateTimeNano = System.nanoTime(); + } + + /** + * This is used to set the target position if the PIDF is being run with current position and + * target position inputs rather than error inputs. + * + * @param set this sets the target position. + */ + public void setTargetPosition(double set) { + targetPosition = set; + } + + /** + * This returns the target position of the PIDF. + * + * @return this returns the target position. + */ + public double getTargetPosition() { + return targetPosition; + } + + /** + * This is used to set the coefficients of the PIDF. + * + * @param set the coefficients that the PIDF will use. + */ + public void setCoefficients(CustomPIDFCoefficients set) { + coefficients = set; + } + + /** + * This returns the PIDF's current coefficients. + * + * @return this returns the current coefficients. + */ + public CustomPIDFCoefficients getCoefficients() { + return coefficients; + } + + /** + * This sets the proportional (P) coefficient of the PIDF only. + * + * @param set this sets the P coefficient. + */ + public void setP(double set) { + coefficients.P = set; + } + + /** + * This returns the proportional (P) coefficient of the PIDF. + * + * @return this returns the P coefficient. + */ + public double P() { + return coefficients.P; + } + + /** + * This sets the integral (I) coefficient of the PIDF only. + * + * @param set this sets the I coefficient. + */ + public void setI(double set) { + coefficients.I = set; + } + + /** + * This returns the integral (I) coefficient of the PIDF. + * + * @return this returns the I coefficient. + */ + public double I() { + return coefficients.I; + } + + /** + * This sets the derivative (D) coefficient of the PIDF only. + * + * @param set this sets the D coefficient. + */ + public void setD(double set) { + coefficients.D = set; + } + + /** + * This returns the derivative (D) coefficient of the PIDF. + * + * @return this returns the D coefficient. + */ + public double D() { + return coefficients.D; + } + + /** + * This sets the feedforward (F) constant of the PIDF only. + * + * @param set this sets the F constant. + */ + public void setF(double set) { + coefficients.F = set; + } + + /** + * This returns the feedforward (F) constant of the PIDF. + * + * @return this returns the F constant. + */ + public double F() { + return coefficients.getCoefficient(feedForwardInput); + } + + /** + * This returns the current error of the PIDF. + * + * @return this returns the error. + */ + public double getError() { + return error; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java new file mode 100644 index 0000000..48a5702 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the SingleRunAction class. It handles running Runnables once, until a reset is called. + * It also forms the basis of the PathCallback class. Basically, if you want to run a certain action + * once despite looping through a section of code multiple times, then this is for you. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/6/2024 + */ +public class SingleRunAction { + private boolean hasBeenRun; + + private Runnable runnable; + + /** + * This creates a new SingleRunAction with a Runnable containing the code to be run for this action. + * The name is a slight bit misleading, as this can actually be run multiple times. However, the + * run() method only runs once before the reset() method needs to be called to allow the + * SingleRunAction to run again. + * + * @param runnable This is a Runnable containing the code to be run. Preferably, use lambda statements. + */ + public SingleRunAction(Runnable runnable) { + this.runnable = runnable; + } + + /** + * This returns if the SingleRunAction has been run yet. Running reset() will reset this. + * + * @return This returns if it has been run. + */ + public boolean hasBeenRun() { + return hasBeenRun; + } + + /** + * This runs the Runnable of the SingleRunAction. It will only run once before requiring a reset. + * + * @return This returns if the Runnable was successfully run. If not, then a reset is needed to run again. + */ + public boolean run() { + if (!hasBeenRun) { + hasBeenRun = true; + runnable.run(); + return true; + } + return false; + } + + /** + * This resets the SingleRunAction and makes it able to run again. The SingleRunAction is set + * to "has not been run", allowing for multiple uses of the Runnable. + */ + public void reset() { + hasBeenRun = false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java new file mode 100644 index 0000000..a606043 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the Timer class. It is an elapsed time clock with millisecond precision, or at least as + * precise as the System.currentTimeMillis() is. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class Timer { + private long startTime; + + /** + * This creates a new Timer with the start time set to its creation time. + */ + public Timer() { + resetTimer(); + } + + /** + * This resets the Timer's start time to the current time using System.currentTimeMillis(). + */ + public void resetTimer() { + startTime = System.currentTimeMillis(); + } + + /** + * This returns the elapsed time in milliseconds since the start time of the Timer. + * + * @return this returns the elapsed time in milliseconds. + */ + public long getElapsedTime() { + return System.currentTimeMillis() - startTime; + } + + /** + * This returns the elapsed time in seconds since the start time of the Timer. + * + * @return this returns the elapsed time in seconds. + */ + public double getElapsedTimeSeconds() { + return (getElapsedTime() / 1000.0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java deleted file mode 100644 index 2c9c15a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.teamcode.roadrunner; - -import com.acmerobotics.dashboard.canvas.Canvas; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.Vector2d; - -public final class Drawing { - private Drawing() { - } - - - public static void drawRobot(Canvas c, Pose2d t) { - final double ROBOT_RADIUS = 9; - - c.setStrokeWidth(1); - c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); - - Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); - Vector2d p1 = t.position.plus(halfv); - Vector2d p2 = p1.plus(halfv); - c.strokeLine(p1.x, p1.y, p2.x, p2.y); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java deleted file mode 100644 index ee7a9f9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode.roadrunner; - -import com.acmerobotics.roadrunner.Time; -import com.acmerobotics.roadrunner.Twist2dDual; - -public interface Localizer { - Twist2dDual

- * Released under the BSD 3-Clause Clear License by j5155 from 12087 Capital City Dynamics - * Portions of this code made and released under the MIT License by Gobilda (Base 10 Assets, LLC) - * Unless otherwise noted, comments are from Gobilda - */ -@Config -public class PinpointDrive extends MecanumDrive { - public static class Params { - /* - Set this to the name that your Pinpoint is configured as in your hardware config. - */ - public String pinpointDeviceName = "pinpoint"; - /* - Set the odometry pod positions relative to the point that the odometry computer tracks around. - The X pod offset refers to how far sideways from the tracking point the - X (forward) odometry pod is. Left of the center is a positive number, - right of the center is a negative number. The Y pod offset refers to how far forwards from - the tracking point the Y (strafe) odometry pod is: forward of the center is a positive number, - backwards is a negative number. - */ - //These are tuned for 3110-0002-0001 Product Insight #1 - // RR localizer note: These units are inches, presets are converted from mm (which is why they are inexact) - public double xOffset = -3.3071; - public double yOffset = -6.6142; - - /* - Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either - the goBILDA_SWINGARM_POD or the goBILDA_4_BAR_POD. - If you're using another kind of odometry pod, input the number of ticks per millimeter for that pod. - - RR LOCALIZER NOTE: this is ticks per MILLIMETER, NOT inches per tick. - This value should be more than one; the value for the Gobilda 4 Bar Pod is approximately 20. - To get this value from inPerTick, first convert the value to millimeters (multiply by 25.4) - and then take its inverse (one over the value) - */ - public double encoderResolution = GoBildaPinpointDriverRR.goBILDA_4_BAR_POD; - - /* - Set the direction that each of the two odometry pods count. The X (forward) pod should - increase when you move the robot forward. And the Y (strafe) pod should increase when - you move the robot to the left. - */ - public GoBildaPinpointDriver.EncoderDirection xDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED; - public GoBildaPinpointDriver.EncoderDirection yDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED; - - /* - Use the pinpoint IMU for tuning - If true, overrides any IMU setting in MecanumDrive and uses exclusively Pinpoint for tuning - You can also use the pinpoint directly in MecanumDrive if this doesn't work for some reason; - replace "imu" with "pinpoint" or whatever your pinpoint is called in config. - Note: Pinpoint IMU is always used for base localization - */ - public boolean usePinpointIMUForTuning = true; - } - - public static Params PARAMS = new Params(); - public GoBildaPinpointDriverRR pinpoint; - private Pose2d lastPinpointPose = pose; - - public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) { - super(hardwareMap, pose); - FlightRecorder.write("PINPOINT_PARAMS", PARAMS); - pinpoint = hardwareMap.get(GoBildaPinpointDriverRR.class, PARAMS.pinpointDeviceName); - - if (PARAMS.usePinpointIMUForTuning) { - lazyImu = new LazyImu(hardwareMap, PARAMS.pinpointDeviceName, new RevHubOrientationOnRobot(zyxOrientation(0, 0, 0))); - } - - // RR localizer note: don't love this conversion (change driver?) - pinpoint.setOffsets(DistanceUnit.MM.fromInches(PARAMS.xOffset), DistanceUnit.MM.fromInches(PARAMS.yOffset)); - - - pinpoint.setEncoderResolution(PARAMS.encoderResolution); - - pinpoint.setEncoderDirections(PARAMS.xDirection, PARAMS.yDirection); - - /* - Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary - The IMU will automatically calibrate when first powered on, but recalibrating before running - the robot is a good idea to ensure that the calibration is "good". - resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. - This is recommended before you run your autonomous, as a bad initial calibration can cause - an incorrect starting value for x, y, and heading. - */ - //pinpoint.recalibrateIMU(); - pinpoint.resetPosAndIMU(); - // wait for pinpoint to finish calibrating - try { - Thread.sleep(300); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - - pinpoint.setPosition(pose); - } - - @Override - public PoseVelocity2d updatePoseEstimate() { - if (lastPinpointPose != pose) { - // RR localizer note: - // Something else is modifying our pose (likely for relocalization), - // so we override the sensor's pose with the new pose. - // This could potentially cause up to 1 loop worth of drift. - // I don't like this solution at all, but it preserves compatibility. - // The only alternative is to add getter and setters, but that breaks compat. - // Potential alternate solution: timestamp the pose set and backtrack it based on speed? - pinpoint.setPosition(pose); - } - pinpoint.update(); - pose = pinpoint.getPositionRR(); - lastPinpointPose = pose; - - // RR standard - poseHistory.add(pose); - while (poseHistory.size() > 100) { - poseHistory.removeFirst(); - } - - FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose)); - FlightRecorder.write("PINPOINT_RAW_POSE", new FTCPoseMessage(pinpoint.getPosition())); - FlightRecorder.write("PINPOINT_STATUS", pinpoint.getDeviceStatus()); - - return pinpoint.getVelocityRR(); - } - - - // for debug logging - public static final class FTCPoseMessage { - public long timestamp; - public double x; - public double y; - public double heading; - - public FTCPoseMessage(Pose2D pose) { - this.timestamp = System.nanoTime(); - this.x = pose.getX(DistanceUnit.INCH); - this.y = pose.getY(DistanceUnit.INCH); - this.heading = pose.getHeading(AngleUnit.RADIANS); - } - } - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java deleted file mode 100644 index 8999440..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java +++ /dev/null @@ -1,148 +0,0 @@ -package org.firstinspires.ftc.teamcode.roadrunner; - - -import static com.acmerobotics.roadrunner.ftc.OTOSKt.OTOSPoseToRRPose; -import static com.acmerobotics.roadrunner.ftc.OTOSKt.RRPoseToOTOSPose; - -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.DownsampledWriter; -import com.acmerobotics.roadrunner.ftc.FlightRecorder; -import com.acmerobotics.roadrunner.ftc.SparkFunOTOSCorrected; -import com.qualcomm.hardware.sparkfun.SparkFunOTOS; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage; - -/** - * Experimental extension of MecanumDrive that uses the SparkFun OTOS sensor for localization. - *

- * Released under the BSD 3-Clause Clear License by j5155 from 12087 Capital City Dynamics - * Portions of this code made and released under the MIT License by SparkFun - * Unless otherwise noted, comments are from SparkFun - */ -public class SparkFunOTOSDrive extends MecanumDrive { - public static class Params { - // Assuming you've mounted your sensor to a robot and it's not centered, - // you can specify the offset for the sensor relative to the center of the - // robot. The units default to inches and degrees, but if you want to use - // different units, specify them before setting the offset! Note that as of - // firmware version 1.0, these values will be lost after a power cycle, so - // you will need to set them each time you power up the sensor. For example, if - // the sensor is mounted 5 inches to the left (negative X) and 10 inches - // forward (positive Y) of the center of the robot, and mounted 90 degrees - // clockwise (negative rotation) from the robot's orientation, the offset - // would be {-5, 10, -90}. These can be any value, even the angle can be - // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees). - - // RR localizer note: These units are inches and radians. - public SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, Math.toRadians(0)); - - // Here we can set the linear and angular scalars, which can compensate for - // scaling issues with the sensor measurements. Note that as of firmware - // version 1.0, these values will be lost after a power cycle, so you will - // need to set them each time you power up the sensor. They can be any value - // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to - // first set both scalars to 1.0, then calibrate the angular scalar, then - // the linear scalar. To calibrate the angular scalar, spin the robot by - // multiple rotations (eg. 10) to get a precise error, then set the scalar - // to the inverse of the error. Remember that the angle wraps from -180 to - // 180 degrees, so for example, if after 10 rotations counterclockwise - // (positive rotation), the sensor reports -15 degrees, the required scalar - // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the - // robot a known distance and measure the error; do this multiple times at - // multiple speeds to get an average, then set the linear scalar to the - // inverse of the error. For example, if you move the robot 100 inches and - // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971 - public double linearScalar = 1.0; - public double angularScalar = 1.0; - } - - public static SparkFunOTOSDrive.Params PARAMS = new SparkFunOTOSDrive.Params(); - public SparkFunOTOSCorrected otos; - private Pose2d lastOtosPose = pose; - - private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); - - public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) { - super(hardwareMap, pose); - FlightRecorder.write("OTOS_PARAMS", PARAMS); - otos = hardwareMap.get(SparkFunOTOSCorrected.class, "sensor_otos"); - // RR localizer note: - // don't change the units, it will stop Dashboard field view from working properly - // and might cause various other issues - otos.setLinearUnit(DistanceUnit.INCH); - otos.setAngularUnit(AngleUnit.RADIANS); - - otos.setOffset(PARAMS.offset); - System.out.println("OTOS calibration beginning!"); - System.out.println(otos.setLinearScalar(PARAMS.linearScalar)); - System.out.println(otos.setAngularScalar(PARAMS.angularScalar)); - - otos.setPosition(RRPoseToOTOSPose(pose)); - // The IMU on the OTOS includes a gyroscope and accelerometer, which could - // have an offset. Note that as of firmware version 1.0, the calibration - // will be lost after a power cycle; the OTOS performs a quick calibration - // when it powers up, but it is recommended to perform a more thorough - // calibration at the start of all your programs. Note that the sensor must - // be completely stationary and flat during calibration! When calling - // calibrateImu(), you can specify the number of samples to take and whether - // to wait until the calibration is complete. If no parameters are provided, - // it will take 255 samples and wait until done; each sample takes about - // 2.4ms, so about 612ms total - - // RR localizer note: It is technically possible to change the number of samples to slightly reduce init times, - // however, I found that it caused pretty severe heading drift. - // Also, if you're careful to always wait more than 612ms in init, you could technically disable waitUntilDone; - // this would allow your OpMode code to run while the calibration occurs. - // However, that may cause other issues. - // In the future I hope to do that by default and just add a check in updatePoseEstimate for it - System.out.println(otos.calibrateImu(255, true)); - System.out.println("OTOS calibration complete!"); - } - - @Override - public PoseVelocity2d updatePoseEstimate() { - if (lastOtosPose != pose) { - // RR localizer note: - // Something else is modifying our pose (likely for relocalization), - // so we override otos pose with the new pose. - // This could potentially cause up to 1 loop worth of drift. - // I don't like this solution at all, but it preserves compatibility. - // The only alternative is to add getter and setters, but that breaks compat. - // Potential alternate solution: timestamp the pose set and backtrack it based on speed? - otos.setPosition(RRPoseToOTOSPose(pose)); - } - // RR localizer note: - // The values are passed by reference, so we create variables first, - // then pass them into the function, then read from them. - - // Reading acceleration worsens loop times by 1ms, - // but not reading it would need a custom driver and would break compatibility. - // The same is true for speed: we could calculate speed ourselves from pose and time, - // but it would be hard, less accurate, and would only save 1ms of loop time. - SparkFunOTOS.Pose2D otosPose = new SparkFunOTOS.Pose2D(); - SparkFunOTOS.Pose2D otosVel = new SparkFunOTOS.Pose2D(); - SparkFunOTOS.Pose2D otosAcc = new SparkFunOTOS.Pose2D(); - otos.getPosVelAcc(otosPose, otosVel, otosAcc); - pose = OTOSPoseToRRPose(otosPose); - lastOtosPose = pose; - - // RR standard - poseHistory.add(pose); - while (poseHistory.size() > 100) { - poseHistory.removeFirst(); - } - - estimatedPoseWriter.write(new PoseMessage(pose)); - - // RR localizer note: - // OTOS velocity units happen to be identical to Roadrunners, so we don't need any conversion! - return new PoseVelocity2d(new Vector2d(otosVel.x, otosVel.y), otosVel.h); - } - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java deleted file mode 100644 index 135ee22..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java +++ /dev/null @@ -1,497 +0,0 @@ -package org.firstinspires.ftc.teamcode.roadrunner; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.canvas.Canvas; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.AccelConstraint; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Actions; -import com.acmerobotics.roadrunner.AngularVelConstraint; -import com.acmerobotics.roadrunner.Arclength; -import com.acmerobotics.roadrunner.DualNum; -import com.acmerobotics.roadrunner.MinVelConstraint; -import com.acmerobotics.roadrunner.MotorFeedforward; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.Pose2dDual; -import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.acmerobotics.roadrunner.PoseVelocity2dDual; -import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.ProfileParams; -import com.acmerobotics.roadrunner.RamseteController; -import com.acmerobotics.roadrunner.TankKinematics; -import com.acmerobotics.roadrunner.Time; -import com.acmerobotics.roadrunner.TimeTrajectory; -import com.acmerobotics.roadrunner.TimeTurn; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TrajectoryBuilderParams; -import com.acmerobotics.roadrunner.TurnConstraints; -import com.acmerobotics.roadrunner.Twist2dDual; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.Vector2dDual; -import com.acmerobotics.roadrunner.VelConstraint; -import com.acmerobotics.roadrunner.ftc.DownsampledWriter; -import com.acmerobotics.roadrunner.ftc.Encoder; -import com.acmerobotics.roadrunner.ftc.FlightRecorder; -import com.acmerobotics.roadrunner.ftc.LazyImu; -import com.acmerobotics.roadrunner.ftc.LynxFirmware; -import com.acmerobotics.roadrunner.ftc.OverflowEncoder; -import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; -import com.acmerobotics.roadrunner.ftc.RawEncoder; -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.VoltageSensor; - -import org.firstinspires.ftc.teamcode.roadrunner.messages.DriveCommandMessage; -import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage; -import org.firstinspires.ftc.teamcode.roadrunner.messages.TankCommandMessage; -import org.firstinspires.ftc.teamcode.roadrunner.messages.TankLocalizerInputsMessage; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collections; -import java.util.LinkedList; -import java.util.List; - -@Config -public final class TankDrive { - public static class Params { - // IMU orientation - // TODO: fill in these values based on - // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting - public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = - RevHubOrientationOnRobot.LogoFacingDirection.UP; - public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; - - // drive model parameters - public double inPerTick = 0; - public double trackWidthTicks = 0; - - // feedforward parameters (in tick units) - public double kS = 0; - public double kV = 0; - public double kA = 0; - - // path profile parameters (in inches) - public double maxWheelVel = 50; - public double minProfileAccel = -30; - public double maxProfileAccel = 50; - - // turn profile parameters (in radians) - public double maxAngVel = Math.PI; // shared with path - public double maxAngAccel = Math.PI; - - // path controller gains - public double ramseteZeta = 0.7; // in the range (0, 1) - public double ramseteBBar = 2.0; // positive - - // turn controller gains - public double turnGain = 0.0; - public double turnVelGain = 0.0; - } - - public static Params PARAMS = new Params(); - - public final TankKinematics kinematics = new TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks); - - public final TurnConstraints defaultTurnConstraints = new TurnConstraints( - PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); - public final VelConstraint defaultVelConstraint = - new MinVelConstraint(Arrays.asList( - kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), - new AngularVelConstraint(PARAMS.maxAngVel) - )); - public final AccelConstraint defaultAccelConstraint = - new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); - - public final List leftMotors, rightMotors; - - public final LazyImu lazyImu; - - public final VoltageSensor voltageSensor; - - public final Localizer localizer; - public Pose2d pose; - - private final LinkedList poseHistory = new LinkedList<>(); - - private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); - private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); - private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); - - private final DownsampledWriter tankCommandWriter = new DownsampledWriter("TANK_COMMAND", 50_000_000); - - public class DriveLocalizer implements Localizer { - public final List leftEncs, rightEncs; - - private double lastLeftPos, lastRightPos; - private boolean initialized; - - public DriveLocalizer() { - { - List leftEncs = new ArrayList<>(); - for (DcMotorEx m : leftMotors) { - Encoder e = new OverflowEncoder(new RawEncoder(m)); - leftEncs.add(e); - } - this.leftEncs = Collections.unmodifiableList(leftEncs); - } - - { - List rightEncs = new ArrayList<>(); - for (DcMotorEx m : rightMotors) { - Encoder e = new OverflowEncoder(new RawEncoder(m)); - rightEncs.add(e); - } - this.rightEncs = Collections.unmodifiableList(rightEncs); - } - - // TODO: reverse encoder directions if needed - // leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE); - } - - @Override - public Twist2dDual

- * Note: This vector is clamped to be at most 1 in magnitude. - * - * @return returns the drive vector. - */ - public Vector getDriveVector() { - if (!useDrive) return new Vector(); - if (followingPathChain && chainIndex < currentPathChain.size() - 1) { - return new Vector(driveVectorScaler.getMaxPowerScaling(), currentPath.getClosestPointTangentVector().getTheta()); - } - - driveError = getDriveVelocityError(); - - if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) { - secondaryDrivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); - return MathFunctions.copyVector(driveVector); - } - - drivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); - return MathFunctions.copyVector(driveVector); - } - - /** - * This returns the velocity the robot needs to be at to make it to the end of the Path - * at some specified deceleration (well technically just some negative acceleration). - * - * @return returns the projected velocity. - */ - public double getDriveVelocityError() { - double distanceToGoal; - if (!currentPath.isAtParametricEnd()) { - distanceToGoal = currentPath.length() * (1 - currentPath.getClosestPointTValue()); - } else { - Vector offset = new Vector(); - offset.setOrthogonalComponents(getPose().getX() - currentPath.getLastControlPoint().getX(), getPose().getY() - currentPath.getLastControlPoint().getY()); - distanceToGoal = MathFunctions.dotProduct(currentPath.getEndTangent(), offset); - } - - Vector distanceToGoalVector = MathFunctions.scalarMultiplyVector(MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector()), distanceToGoal); - Vector velocity = new Vector(MathFunctions.dotProduct(getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()); - - Vector forwardHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading()); - double forwardVelocity = MathFunctions.dotProduct(forwardHeadingVector, velocity); - double forwardDistanceToGoal = MathFunctions.dotProduct(forwardHeadingVector, distanceToGoalVector); - double forwardVelocityGoal = MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * forwardZeroPowerAcceleration * forwardDistanceToGoal)); - double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * forwardDistanceToGoal)); - - Vector lateralHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); - double lateralVelocity = MathFunctions.dotProduct(lateralHeadingVector, velocity); - double lateralDistanceToGoal = MathFunctions.dotProduct(lateralHeadingVector, distanceToGoalVector); - double lateralVelocityGoal = MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * lateralZeroPowerAcceleration * lateralDistanceToGoal)); - double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * lateralDistanceToGoal)); - - Vector forwardVelocityError = new Vector(forwardVelocityGoal - forwardVelocityZeroPowerDecay - forwardVelocity, forwardHeadingVector.getTheta()); - Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); - Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); - - previousRawDriveError = rawDriveError; - rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); - - double projection = 2 * driveErrors[1] - driveErrors[0]; - - driveKalmanFilter.update(rawDriveError - previousRawDriveError, projection); - - for (int i = 0; i < driveErrors.length - 1; i++) { - driveErrors[i] = driveErrors[i + 1]; - } - driveErrors[1] = driveKalmanFilter.getState(); - - return driveKalmanFilter.getState(); - } - - /** - * This returns a Vector in the direction of the robot that contains the heading correction - * as its magnitude. Positive heading correction turns the robot counter-clockwise, and negative - * heading correction values turn the robot clockwise. So basically, Pedro Pathing uses a right- - * handed coordinate system. - *

- * Note: This vector is clamped to be at most 1 in magnitude. - * - * @return returns the heading vector. - */ - public Vector getHeadingVector() { - if (!useHeading) return new Vector(); - headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); - if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) { - secondaryHeadingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); - return MathFunctions.copyVector(headingVector); - } - headingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); - return MathFunctions.copyVector(headingVector); - } - - /** - * This returns a combined Vector in the direction the robot must go to correct both translational - * error as well as centripetal force. - *

- * Note: This vector is clamped to be at most 1 in magnitude. - * - * @return returns the corrective vector. - */ - public Vector getCorrectiveVector() { - Vector centripetal = getCentripetalForceCorrection(); - Vector translational = getTranslationalCorrection(); - Vector corrective = MathFunctions.addVectors(centripetal, translational); - - if (corrective.getMagnitude() > driveVectorScaler.getMaxPowerScaling()) { - return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational))); - } - - correctiveVector = MathFunctions.copyVector(corrective); - - return corrective; - } - - /** - * This returns a Vector in the direction the robot must go to account for only translational - * error. - *

- * Note: This vector is clamped to be at most 1 in magnitude. - * - * @return returns the translational correction vector. - */ - public Vector getTranslationalCorrection() { - if (!useTranslational) return new Vector(); - Vector translationalVector = new Vector(); - double x = closestPose.getX() - poseUpdater.getPose().getX(); - double y = closestPose.getY() - poseUpdater.getPose().getY(); - translationalVector.setOrthogonalComponents(x, y); - - if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) { - translationalVector = MathFunctions.subtractVectors(translationalVector, new Vector(MathFunctions.dotProduct(translationalVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); - - secondaryTranslationalIntegralVector = MathFunctions.subtractVectors(secondaryTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(secondaryTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); - translationalIntegralVector = MathFunctions.subtractVectors(translationalIntegralVector, new Vector(MathFunctions.dotProduct(translationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); - } - - if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch && useSecondaryTranslationalPID) { - secondaryTranslationalIntegral.updateError(translationalVector.getMagnitude()); - secondaryTranslationalIntegralVector = MathFunctions.addVectors(secondaryTranslationalIntegralVector, new Vector(secondaryTranslationalIntegral.runPIDF() - previousSecondaryTranslationalIntegral, translationalVector.getTheta())); - previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.runPIDF(); - - secondaryTranslationalPIDF.updateError(translationalVector.getMagnitude()); - translationalVector.setMagnitude(secondaryTranslationalPIDF.runPIDF() + secondaryTranslationalPIDFFeedForward); - translationalVector = MathFunctions.addVectors(translationalVector, secondaryTranslationalIntegralVector); - } else { - translationalIntegral.updateError(translationalVector.getMagnitude()); - translationalIntegralVector = MathFunctions.addVectors(translationalIntegralVector, new Vector(translationalIntegral.runPIDF() - previousTranslationalIntegral, translationalVector.getTheta())); - previousTranslationalIntegral = translationalIntegral.runPIDF(); - - translationalPIDF.updateError(translationalVector.getMagnitude()); - translationalVector.setMagnitude(translationalPIDF.runPIDF() + translationalPIDFFeedForward); - translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector); - } - - translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, driveVectorScaler.getMaxPowerScaling())); - - this.translationalVector = MathFunctions.copyVector(translationalVector); - - return translationalVector; - } - - /** - * This returns the raw translational error, or how far off the closest point the robot is. - * - * @return This returns the raw translational error as a Vector. - */ - public Vector getTranslationalError() { - Vector error = new Vector(); - double x = closestPose.getX() - poseUpdater.getPose().getX(); - double y = closestPose.getY() - poseUpdater.getPose().getY(); - error.setOrthogonalComponents(x, y); - return error; - } - - /** - * This returns a Vector in the direction the robot must go to account for only centripetal - * force. - *

- * Note: This vector is clamped to be between [0, 1] in magnitude. - * - * @return returns the centripetal force correction vector. - */ - public Vector getCentripetalForceCorrection() { - if (!useCentripetal) return new Vector(); - double curvature; - if (!teleopDrive) { - curvature = currentPath.getClosestPointCurvature(); - } else { - double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); - double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); - curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); - } - if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); - return centripetalVector; - } - - /** - * This returns the closest pose to the robot on the Path the Follower is currently following. - * This closest pose is calculated through a binary search method with some specified number of - * steps to search. By default, 10 steps are used, which should be more than enough. - * - * @return returns the closest pose. - */ - public Pose getClosestPose() { - return closestPose; - } - - /** - * This returns whether the follower is at the parametric end of its current Path. - * The parametric end is determined by if the closest Point t-value is greater than some specified - * end t-value. - * If running a PathChain, this returns true only if at parametric end of last Path in the PathChain. - * - * @return returns whether the Follower is at the parametric end of its Path. - */ - public boolean atParametricEnd() { - if (followingPathChain) { - if (chainIndex == currentPathChain.size() - 1) return currentPath.isAtParametricEnd(); - return false; - } - return currentPath.isAtParametricEnd(); - } - - /** - * This returns the t value of the closest point on the current Path to the robot - * In the absence of a current Path, it returns 1.0. - * - * @return returns the current t value. - */ - public double getCurrentTValue() { - if (isBusy) return currentPath.getClosestPointTValue(); - return 1.0; - } - - /** - * This returns the current path number. For following Paths, this will return 0. For PathChains, - * this will return the current path number. For holding Points, this will also return 0. - * - * @return returns the current path number. - */ - public double getCurrentPathNumber() { - if (!followingPathChain) return 0; - return chainIndex; - } - - /** - * This returns a new PathBuilder object for easily building PathChains. - * - * @return returns a new PathBuilder object. - */ - public PathBuilder pathBuilder() { - return new PathBuilder(); - } - - /** - * This writes out information about the various motion Vectors to the Telemetry specified. - * - * @param telemetry this is an instance of Telemetry or the FTC Dashboard telemetry that this - * method will use to output the debug data. - */ - public void telemetryDebug(MultipleTelemetry telemetry) { - telemetry.addData("follower busy", isBusy()); - telemetry.addData("heading error", headingError); - telemetry.addData("heading vector magnitude", headingVector.getMagnitude()); - telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude()); - telemetry.addData("corrective vector heading", correctiveVector.getTheta()); - telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude()); - telemetry.addData("translational error direction", getTranslationalError().getTheta()); - telemetry.addData("translational vector magnitude", translationalVector.getMagnitude()); - telemetry.addData("translational vector heading", translationalVector.getMagnitude()); - telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude()); - telemetry.addData("centripetal vector heading", centripetalVector.getTheta()); - telemetry.addData("drive error", driveError); - telemetry.addData("drive vector magnitude", driveVector.getMagnitude()); - telemetry.addData("drive vector heading", driveVector.getTheta()); - telemetry.addData("x", getPose().getX()); - telemetry.addData("y", getPose().getY()); - telemetry.addData("heading", getPose().getHeading()); - telemetry.addData("total heading", poseUpdater.getTotalHeading()); - telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); - telemetry.addData("velocity heading", getVelocity().getTheta()); - driveKalmanFilter.debug(telemetry); - telemetry.update(); - if (drawOnDashboard) { - Drawing.drawDebug(this); - } - } - - /** - * This writes out information about the various motion Vectors to the Telemetry specified. - * - * @param telemetry this is an instance of Telemetry or the FTC Dashboard telemetry that this - * method will use to output the debug data. - */ - public void telemetryDebug(Telemetry telemetry) { - telemetryDebug(new MultipleTelemetry(telemetry)); - } - - /** - * This returns the total number of radians the robot has turned. - * - * @return the total heading. - */ - public double getTotalHeading() { - return poseUpdater.getTotalHeading(); - } - - /** - * This returns the current Path the Follower is following. This can be null. - * - * @return returns the current Path. - */ - public Path getCurrentPath() { - return currentPath; - } - - /** - * This returns the pose tracker for the robot to draw on the Dashboard. - * - * @return returns the pose tracker - */ - public DashboardPoseTracker getDashboardPoseTracker() { - return dashboardPoseTracker; - } - - /** - * This resets the IMU, if applicable. - */ - private void resetIMU() throws InterruptedException { - poseUpdater.resetIMU(); - } - - /** - * This puts the motors into brake mode. - */ - public void brakeMode() { - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java deleted file mode 100644 index 573c4cf..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java +++ /dev/null @@ -1,83 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -/** - * This is the Encoder class. This tracks the position of a motor of class DcMotorEx. The motor - * must have an encoder attached. It can also get changes in position. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -public class Encoder { - private DcMotorEx motor; - private double previousPosition; - private double currentPosition; - private double multiplier; - - public final static double FORWARD = 1, REVERSE = -1; - - /** - * This creates a new Encoder from a DcMotorEx. - * - * @param setMotor the motor this will be tracking - */ - public Encoder(DcMotorEx setMotor) { - motor = setMotor; - multiplier = FORWARD; - reset(); - } - - /** - * This sets the direction/multiplier of the Encoder. Setting 1 or -1 will make the Encoder track - * forward or in reverse, respectively. Any multiple of either one will scale the Encoder's output - * by that amount. - * - * @param setMultiplier the multiplier/direction to set - */ - public void setDirection(double setMultiplier) { - multiplier = setMultiplier; - } - - /** - * This resets the Encoder's position and the current and previous position in the code. - */ - public void reset() { - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - previousPosition = motor.getCurrentPosition(); - currentPosition = motor.getCurrentPosition(); - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - - /** - * This updates the Encoder's tracked current position and previous position. - */ - public void update() { - previousPosition = currentPosition; - currentPosition = motor.getCurrentPosition(); - } - - /** - * This returns the multiplier/direction of the Encoder. - * - * @return returns the multiplier - */ - public double getMultiplier() { - return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); - } - - /** - * This returns the change in position from the previous position to the current position. One - * important thing to note is that this encoder does not track velocity, only change in position. - * This is because I am using a pose exponential method of localization, which doesn't need the - * velocity of the encoders. Velocity of the robot is calculated in the localizer using an elapsed - * time timer there. - * - * @return returns the change in position of the Encoder - */ - public double getDeltaPosition() { - return getMultiplier() * (currentPosition - previousPosition); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java deleted file mode 100644 index 882ed56..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java +++ /dev/null @@ -1,520 +0,0 @@ - -/* MIT License - * Copyright (c) [2024] [Base 10 Assets, LLC] - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; - -import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; -import com.qualcomm.robotcore.hardware.I2cAddr; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; -import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; -import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; -import com.qualcomm.robotcore.util.TypeConversion; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.util.Arrays; - - -@I2cDeviceType -@DeviceProperties( - name = "goBILDA® Pinpoint Odometry Computer", - xmlTag = "goBILDAPinpoint", - description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" -) - -public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { - - private int deviceStatus = 0; - private int loopTime = 0; - private int xEncoderValue = 0; - private int yEncoderValue = 0; - private float xPosition = 0; - private float yPosition = 0; - private float hOrientation = 0; - private float xVelocity = 0; - private float yVelocity = 0; - private float hVelocity = 0; - - private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod - private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod - - //i2c address of the device - public static final byte DEFAULT_ADDRESS = 0x31; - - public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { - super(deviceClient, deviceClientIsOwned); - - this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); - super.registerArmingStateCallback(false); - } - - - @Override - public Manufacturer getManufacturer() { - return Manufacturer.Other; - } - - @Override - protected synchronized boolean doInitialize() { - ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); - return true; - } - - @Override - public String getDeviceName() { - return "goBILDA® Pinpoint Odometry Computer"; - } - - - //Register map of the i2c device - private enum Register { - DEVICE_ID (1), - DEVICE_VERSION (2), - DEVICE_STATUS (3), - DEVICE_CONTROL (4), - LOOP_TIME (5), - X_ENCODER_VALUE (6), - Y_ENCODER_VALUE (7), - X_POSITION (8), - Y_POSITION (9), - H_ORIENTATION (10), - X_VELOCITY (11), - Y_VELOCITY (12), - H_VELOCITY (13), - MM_PER_TICK (14), - X_POD_OFFSET (15), - Y_POD_OFFSET (16), - YAW_SCALAR (17), - BULK_READ (18); - - private final int bVal; - - Register(int bVal){ - this.bVal = bVal; - } - } - - //Device Status enum that captures the current fault condition of the device - public enum DeviceStatus{ - NOT_READY (0), - READY (1), - CALIBRATING (1 << 1), - FAULT_X_POD_NOT_DETECTED (1 << 2), - FAULT_Y_POD_NOT_DETECTED (1 << 3), - FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), - FAULT_IMU_RUNAWAY (1 << 4); - - private final int status; - - DeviceStatus(int status){ - this.status = status; - } - } - - //enum that captures the direction the encoders are set to - public enum EncoderDirection{ - FORWARD, - REVERSED; - } - - //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used - public enum GoBildaOdometryPods { - goBILDA_SWINGARM_POD, - goBILDA_4_BAR_POD; - } - //enum that captures a limited scope of read data. More options may be added in future update - public enum readData { - ONLY_UPDATE_HEADING, - } - - - /** Writes an int to the i2c device - @param reg the register to write the int to - @param i the integer to write to the register - */ - private void writeInt(final Register reg, int i){ - deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN)); - } - - /** - * Reads an int from a register of the i2c device - * @param reg the register to read from - * @return returns an int that contains the value stored in the read register - */ - private int readInt(Register reg){ - return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN); - } - - /** - * Converts a byte array to a float value - * @param byteArray byte array to transform - * @param byteOrder order of byte array to convert - * @return the float value stored by the byte array - */ - private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ - return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); - } - /** - * Reads a float from a register - * @param reg the register to read - * @return the float value stored in that register - */ - - private float readFloat(Register reg){ - return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); - } - - - /** - * Converts a float to a byte array - * @param value the float array to convert - * @return the byte array converted from the float - */ - private byte [] floatToByteArray (float value, ByteOrder byteOrder) { - return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); - } - - /** - * Writes a byte array to a register on the i2c device - * @param reg the register to write to - * @param bytes the byte array to write - */ - private void writeByteArray (Register reg, byte[] bytes){ - deviceClient.write(reg.bVal,bytes); - } - - /** - * Writes a float to a register on the i2c device - * @param reg the register to write to - * @param f the float to write - */ - private void writeFloat (Register reg, float f){ - byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array(); - deviceClient.write(reg.bVal,bytes); - } - - /** - * Looks up the DeviceStatus enum corresponding with an int value - * @param s int to lookup - * @return the Odometry Computer state - */ - private DeviceStatus lookupStatus (int s){ - if ((s & DeviceStatus.CALIBRATING.status) != 0){ - return DeviceStatus.CALIBRATING; - } - boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; - boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; - - if(!xPodDetected && !yPodDetected){ - return DeviceStatus.FAULT_NO_PODS_DETECTED; - } - if (!xPodDetected){ - return DeviceStatus.FAULT_X_POD_NOT_DETECTED; - } - if (!yPodDetected){ - return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; - } - if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ - return DeviceStatus.FAULT_IMU_RUNAWAY; - } - if ((s & DeviceStatus.READY.status) != 0){ - return DeviceStatus.READY; - } - else { - return DeviceStatus.NOT_READY; - } - } - - /** - * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. - */ - public void update(){ - byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); - deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); - loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); - xEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN); - yEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 12,16), ByteOrder.LITTLE_ENDIAN); - xPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN); - yPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN); - hOrientation = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN); - xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); - yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); - hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); - } - - /** - * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function - * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING - * is supported. - * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING - */ - public void update(readData data) { - if (data == readData.ONLY_UPDATE_HEADING) { - hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); - } - } - - /** - * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

- * The most common tracking position is the center of the robot.

- * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
- * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
- * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases - * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases - */ - public void setOffsets(double xOffset, double yOffset){ - writeFloat(Register.X_POD_OFFSET, (float) xOffset); - writeFloat(Register.Y_POD_OFFSET, (float) yOffset); - } - - /** - * Recalibrates the Odometry Computer's internal IMU.

- * Robot MUST be stationary

- * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. - */ - public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} - - /** - * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

- * Robot MUST be stationary

- * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. - */ - public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} - - /** - * Can reverse the direction of each encoder. - * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward - * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left - */ - public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ - if (xEncoder == EncoderDirection.FORWARD){ - writeInt(Register.DEVICE_CONTROL,1<<5); - } - if (xEncoder == EncoderDirection.REVERSED) { - writeInt(Register.DEVICE_CONTROL,1<<4); - } - - if (yEncoder == EncoderDirection.FORWARD){ - writeInt(Register.DEVICE_CONTROL,1<<3); - } - if (yEncoder == EncoderDirection.REVERSED){ - writeInt(Register.DEVICE_CONTROL,1<<2); - } - } - - /** - * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

- * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD - */ - public void setEncoderResolution(GoBildaOdometryPods pods){ - if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { - writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); - } - if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); - } - } - - /** - * Sets the encoder resolution in ticks per mm of the odometry pods.
- * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. - * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 - */ - public void setEncoderResolution(double ticks_per_mm){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Tuning this value should be unnecessary.
- * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

- * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

- * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. - * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

- * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com - * @param yawOffset A scalar for the robot's heading. - */ - public void setYawScalar(double yawOffset){ - writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Send a position that the Pinpoint should use to track your robot relative to. You can use this to - * update the estimated position of your robot with new external sensor data, or to run a robot - * in field coordinates.

- * This overrides the current position.

- * Using this feature to track your robot's position in field coordinates:
- * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
- * Say you're on the red alliance, your robot is against the wall and closer to the audience side, - * and the front of your robot is pointing towards the center of the field. - * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always - * keep track of how far away from the center of the field you are.

- * Using this feature to update your position with additional sensors:
- * Some robots have a secondary way to locate their robot on the field. This is commonly - * Apriltag localization in FTC, but it can also be something like a distance sensor. - * Often these external sensors are absolute (meaning they measure something about the field) - * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific - * position on the field to use them. In that case, spend most of your time relying on the Pinpoint - * to determine your location. Then when you pull a new position from your secondary sensor, - * send a setPosition command with the new position. The Pinpoint will then track your movement - * relative to that new, more accurate position. - * @param pos a Pose2D describing the robot's new position. - */ - public Pose2D setPosition(Pose2D pos){ - writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); - writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); - writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); - return pos; - } - - /** - * Checks the deviceID of the Odometry Computer. Should return 1. - * @return 1 if device is functional. - */ - public int getDeviceID(){return readInt(Register.DEVICE_ID);} - - /** - * @return the firmware version of the Odometry Computer - */ - public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } - - public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } - - /** - * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: - * @return one of the following states:
- * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
- * READY - The device is currently functioning as normal. GREEN LED
- * CALIBRATING - The device is currently recalibrating the gyro. RED LED
- * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
- * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
- * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
- */ - public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } - - /** - * Checks the Odometry Computer's most recent loop time.

- * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com - * @return loop time in microseconds (1/1,000,000 seconds) - */ - public int getLoopTime(){return loopTime; } - - /** - * Checks the Odometry Computer's most recent loop frequency.

- * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com - * @return Pinpoint Frequency in Hz (loops per second), - */ - public double getFrequency(){ - if (loopTime != 0){ - return 1000000.0/loopTime; - } - else { - return 0; - } - } - - /** - * @return the raw value of the X (forward) encoder in ticks - */ - public int getEncoderX(){return xEncoderValue; } - - /** - * @return the raw value of the Y (strafe) encoder in ticks - */ - public int getEncoderY(){return yEncoderValue; } - - /** - * @return the estimated X (forward) position of the robot in mm - */ - public double getPosX(){return xPosition; } - - /** - * @return the estimated Y (Strafe) position of the robot in mm - */ - public double getPosY(){return yPosition; } - - /** - * @return the estimated H (heading) position of the robot in Radians - */ - public double getHeading(){return hOrientation;} - - /** - * @return the estimated X (forward) velocity of the robot in mm/sec - */ - public double getVelX(){return xVelocity; } - - /** - * @return the estimated Y (strafe) velocity of the robot in mm/sec - */ - public double getVelY(){return yVelocity; } - - /** - * @return the estimated H (heading) velocity of the robot in radians/sec - */ - public double getHeadingVelocity(){return hVelocity; } - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the X (forward) pod - */ - public float getXOffset(){return readFloat(Register.X_POD_OFFSET);} - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the Y (strafe) pod - */ - public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);} - - /** - * @return a Pose2D containing the estimated position of the robot - */ - public Pose2D getPosition(){ - return new Pose2D(DistanceUnit.MM, - xPosition, - yPosition, - AngleUnit.RADIANS, - hOrientation); - } - - - - /** - * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second - */ - public Pose2D getVelocity(){ - return new Pose2D(DistanceUnit.MM, - xVelocity, - yVelocity, - AngleUnit.RADIANS, - hVelocity); - } - - - -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java deleted file mode 100644 index 64710ac..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ /dev/null @@ -1,105 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the Localizer class. It is an abstract superclass of all localizers used in Pedro Pathing, - * so it contains abstract methods that will have a concrete implementation in the subclasses. Any - * method that all localizers will need will be in this class. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -public abstract class Localizer { - - /** - * This returns the current pose estimate from the Localizer. - * - * @return returns the pose as a Pose object. - */ - public abstract Pose getPose(); - - /** - * This returns the current velocity estimate from the Localizer. - * - * @return returns the velocity as a Pose object. - */ - public abstract Pose getVelocity(); - - /** - * This returns the current velocity estimate from the Localizer as a Vector. - * - * @return returns the velocity as a Vector. - */ - public abstract Vector getVelocityVector(); - - /** - * This sets the start pose of the Localizer. Changing the start pose should move the robot as if - * all its previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - public abstract void setStartPose(Pose setStart); - - /** - * This sets the current pose estimate of the Localizer. Changing this should just change the - * robot's current pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - public abstract void setPose(Pose setPose); - - /** - * This calls an update to the Localizer, updating the current pose estimate and current velocity - * estimate. - */ - public abstract void update(); - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public abstract double getTotalHeading(); - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public abstract double getForwardMultiplier(); - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public abstract double getLateralMultiplier(); - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public abstract double getTurningMultiplier(); - - /** - * This resets the IMU of the localizer, if applicable. - */ - public abstract void resetIMU() throws InterruptedException; - - /** - * This is overridden to return the IMU, if there is one. - * - * @return returns the IMU if it exists - */ - public IMU getIMU() { - return null; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java deleted file mode 100644 index c93ab7e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java +++ /dev/null @@ -1,273 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import java.util.Arrays; - -/** - * This is the Matrix class. This defines matrices, primarily for use in the localizers. However, if - * matrices and matrix operations are necessary, this class as well as some operations in the - * MathFunctions class can absolutely be used there as well. It's similar to Mats in OpenCV if you've - * used them before, but with more limited functionality. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -public class Matrix { - private double[][] matrix; - - /** - * This creates a new Matrix of width and height 0. - */ - public Matrix() { - matrix = new double[0][0]; - } - - /** - * This creates a new Matrix with a specified width and height. - * - * @param rows the number of rows, or height - * @param columns the number of columns, or width - */ - public Matrix(int rows, int columns) { - matrix = new double[rows][columns]; - } - - /** - * This creates a new Matrix from a 2D matrix of doubles. Please only enter rectangular 2D - * Arrays of doubles or else things mess up. - * - * @param setMatrix the 2D Array of doubles - */ - public Matrix(double[][] setMatrix) { - setMatrix(setMatrix); - } - - /** - * This creates a new Matrix from another Matrix. - * - * @param setMatrix the Matrix input. - */ - public Matrix(Matrix setMatrix) { - setMatrix(setMatrix); - } - - /** - * This creates a copy of a 2D Array of doubles that references entirely new memory locations - * from the original 2D Array of doubles, so no issues with mutability. - * - * @param copyMatrix the 2D Array of doubles to copy - * @return returns a deep copy of the input Array - */ - public static double[][] deepCopy(double[][] copyMatrix) { - double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length]; - for (int i = 0; i < copyMatrix.length; i++) { - returnMatrix[i] = Arrays.copyOf(copyMatrix[i], copyMatrix[i].length); - } - return returnMatrix; - } - - /** - * This returns a deep copy of the 2D Array that this Matrix is based on. - * - * @return returns the 2D Array of doubles this Matrix is built on - */ - public double[][] getMatrix() { - return deepCopy(matrix); - } - - /** - * This returns a specified row of the Matrix in the form of an Array of doubles. - * - * @param row the index of the row to return - * @return returns the row of the Matrix specified - */ - public double[] get(int row) { - return Arrays.copyOf(matrix[row], matrix[row].length); - } - - /** - * This returns a specified element of the Matrix as a double. - * - * @param row the index of the row of the element - * @param column the index of the column of the element - * @return returns the element of the Matrix specified - */ - public double get(int row, int column) { - return get(row)[column]; - } - - /** - * This returns the number of rows of the Matrix, as determined by the length of the 2D Array. - * If the Matrix/2D Array is not rectangular, issues arise. - * - * @return returns the number of rows in the Matrix - */ - public int getRows() { - return matrix.length; - } - - /** - * This returns the number of columns of the Matrix, as determined by the length of the first Array - * in the 2D Array. If the Matrix/2D Array is not rectangular, issues arise. - * - * @return returns the number of columns in the Matrix - */ - public int getColumns() { - return matrix[0].length; - } - - /** - * This sets the 2D Array of this Matrix to a copy of the 2D Array of another Matrix. - * - * @param setMatrix the Matrix to copy from - * @return returns if the operation was successful - */ - public boolean setMatrix(Matrix setMatrix) { - return setMatrix(setMatrix.getMatrix()); - } - - /** - * This sets the 2D Array of this Matrix to a copy of a specified 2D Array. - * - * @param setMatrix the 2D Array to copy from - * @return returns if the operation was successful - */ - public boolean setMatrix(double[][] setMatrix) { - int columns = setMatrix[0].length; - for (int i = 0; i < setMatrix.length; i++) { - if (setMatrix[i].length != columns) { - return false; - } - } - matrix = deepCopy(setMatrix); - return true; - } - - /** - * This sets a row of the Matrix to a copy of a specified Array of doubles. - * - * @param row the row to be written over - * @param input the Array input - * @return returns if the operation was successful - */ - public boolean set(int row, double[] input) { - if (input.length != getColumns()) { - return false; - } - matrix[row] = Arrays.copyOf(input, input.length); - return true; - } - - /** - * This sets a specified element of the Matrix to an input value. - * - * @param row the index of the row of the specified element - * @param column the index of the column of the specified element - * @param input the input value - * @return returns if the operation was successful - */ - public boolean set(int row, int column, double input) { - matrix[row][column] = input; - return true; - } - - /** - * This adds a Matrix to this Matrix. - * - * @param input the Matrix to add to this. Nothing will change in this Matrix - * @return returns if the operation was successful - */ - public boolean add(Matrix input) { - if (input.getRows() == getRows() && input.getColumns() == getColumns()) { - for (int i = 0; i < getRows(); i++) { - for (int j = 0; j < getColumns(); j++) { - set(i, j, get(i,j) + input.get(i,j)); - } - } - return true; - } - return false; - } - - /** - * This subtracts a Matrix from this Matrix. - * - * @param input the Matrix to subtract from this. Nothing will change in this Matrix - * @return returns if the operation was successful - */ - public boolean subtract(Matrix input) { - if (input.getRows() == getRows() && input.getColumns() == getColumns()) { - for (int i = 0; i < getRows(); i++) { - for (int j = 0; j < getColumns(); j++) { - set(i, j, get(i,j) - input.get(i,j)); - } - } - return true; - } - return false; - } - - /** - * This multiplies this Matrix with a scalar. - * - * @param scalar the scalar number - * @return returns if the operation was successful - */ - public boolean scalarMultiply(double scalar) { - for (int i = 0; i < getRows(); i++) { - for (int j = 0; j < getColumns(); j++) { - set(i, j, scalar * get(i,j)); - } - } - return true; - } - - /** - * This multiplies the Matrix by -1, flipping the signs of all the elements within. - * - * @return returns if the operation was successful - */ - public boolean flipSigns() { - return scalarMultiply(-1); - } - - /** - * This multiplies a Matrix to this Matrix. - * - * @param input the Matrix to multiply to this. Nothing will change in this Matrix - * @return returns if the operation was successful - */ - public boolean multiply(Matrix input) { - if (getColumns() == input.getRows()) { - Matrix product = new Matrix(getRows(), input.getColumns()); - for (int i = 0; i < product.getRows(); i++) { - for (int j = 0; j < product.getColumns(); j++) { - double value = 0; - for (int k = 0; k < get(i).length; k++) { - value += get(i, k) * input.get(k, j); - } - product.set(i, j, value); - } - } - setMatrix(product); - return true; - } - return false; - } - - /** - * This multiplies a Matrix to another Matrix. This will not change any data in the two input - * Matrices. - * - * @param one the first Matrix to multiply. - * @param two the second Matrix to multiply - * @return returns if the operation was successful - */ - public static Matrix multiply(Matrix one, Matrix two) { - Matrix returnMatrix = new Matrix(one); - if (returnMatrix.multiply(two)) { - return returnMatrix; - } else { - return new Matrix(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java deleted file mode 100644 index d784c09..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ /dev/null @@ -1,219 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import androidx.annotation.NonNull; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the Pose class. It defines poses in 2D space, like the Pose2D class in Road Runner except - * in the Pedro Pathing code so I don't have to import the Road Runner library. A Pose consists of - * two coordinates defining a position and a third value for the heading, so basically just defining - * any position and orientation the robot can be at, unless your robot can fly for whatever reason. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -public class Pose { - private double x; - private double y; - private double heading; - - /** - * This creates a new Pose from a x, y, and heading inputs. - * - * @param setX the initial x value - * @param setY the initial y value - * @param setHeading the initial heading value - */ - public Pose(double setX, double setY, double setHeading) { - setX(setX); - setY(setY); - setHeading(setHeading); - } - - /** - * This creates a new Pose from x and y inputs. The heading is set to 0. - * - * @param setX the initial x value - * @param setY the initial y value - */ - public Pose(double setX, double setY) { - this(setX, setY, 0); - } - - /** - * This creates a new Pose with no inputs and 0 for all values. - */ - public Pose() { - this(0, 0, 0); - } - - /** - * This sets the x value. - * - * @param set the x value - */ - public void setX(double set) { - x = set; - } - - /** - * This sets the y value. - * - * @param set the y value - */ - public void setY(double set) { - y = set; - } - - /** - * This sets the heading value. - * - * @param set the heading value - */ - public void setHeading(double set) { - heading = MathFunctions.normalizeAngle(set); - } - - /** - * This returns the x value. - * - * @return returns the x value - */ - public double getX() { - return x; - } - - /** - * This returns the y value. - * - * @return returns the y value - */ - public double getY() { - return y; - } - - /** - * This returns the heading value. - * - * @return returns the heading value - */ - public double getHeading() { - return heading; - } - - /** - * This returns the Pose as a Vector. Naturally, the heading data in the Pose cannot be included - * in the Vector. - * - * @return returns the Pose as a Vector - */ - public Vector getVector() { - Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(x, y); - return returnVector; - } - - /** - * This returns a new Vector with magnitude 1 pointing in the direction of the heading. - * - * @return returns a unit Vector in the direction of the heading - */ - public Vector getHeadingVector() { - return new Vector(1, heading); - } - - /** - * This adds all the values of an input Pose to this Pose. The input Pose's data will not be - * changed. - * - * @param pose the input Pose - */ - public void add(Pose pose) { - setX(x + pose.getX()); - setY(y + pose.getY()); - setHeading(heading + pose.getHeading()); - } - - /** - * This subtracts all the values of an input Pose from this Pose. The input Pose's data will not - * be changed. - * - * @param pose the input Pose - */ - public void subtract(Pose pose) { - setX(x - pose.getX()); - setY(y - pose.getY()); - setHeading(heading - pose.getHeading()); - } - - /** - * This multiplies all the values of this Pose by a scalar. - * - * @param scalar the scalar - */ - public void scalarMultiply(double scalar) { - setX(x * scalar); - setY(y * scalar); - setHeading(heading * scalar); - } - - /** - * This divides all the values of this Pose by a scalar. - * - * @param scalar the scalar - */ - public void scalarDivide(double scalar) { - setX(x / scalar); - setY(y / scalar); - setHeading(heading / scalar); - } - - /** - * This flips the signs of all values in this Pose by multiplying them by -1. Heading values are - * still normalized to be between 0 and 2 * pi in value. - */ - public void flipSigns() { - setX(-x); - setY(-y); - setHeading(-heading); - } - - /** - * This returns if a Pose is within a specified accuracy of this Pose in terms of x position, - * y position, and heading. - * - * @param pose the input Pose to check - * @param accuracy the specified accuracy necessary to return true - * @return returns if the input Pose is within the specified accuracy of this Pose - */ - public boolean roughlyEquals(Pose pose, double accuracy) { - return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy); - } - - /** - * This checks if the input Pose is within 0.0001 in all values to this Pose. - * - * @param pose the input Pose - * @return returns if the input Pose is within 0.0001 of this Pose - */ - public boolean roughlyEquals(Pose pose) { - return roughlyEquals(pose, 0.0001); - } - - /** - * This creates a copy of this Pose that points to a new memory location. - * - * @return returns a deep copy of this Pose - */ - public Pose copy() { - return new Pose(getX(), getY(), getHeading()); - } - - @NonNull - @Override - public String toString() { - return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")"; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java deleted file mode 100644 index 15dddb3..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ /dev/null @@ -1,354 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.PinpointLocalizer; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the PoseUpdater class. This class handles getting pose data from the localizer and returning - * the information in a useful way to the Follower. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/4/2024 - */ -public class PoseUpdater { - private final HardwareMap hardwareMap; - - private final IMU imu; - - private final Localizer localizer; - - private Pose startingPose = new Pose(0, 0, 0); - - private Pose currentPose = startingPose; - - private Pose previousPose = startingPose; - - private Vector currentVelocity = new Vector(); - - private Vector previousVelocity = new Vector(); - - private Vector currentAcceleration = new Vector(); - - private double xOffset = 0; - private double yOffset = 0; - private double headingOffset = 0; - - private long previousPoseTime; - private long currentPoseTime; - - /** - * Creates a new PoseUpdater from a HardwareMap and a Localizer. - * - * @param hardwareMap the HardwareMap - * @param localizer the Localizer - */ - public PoseUpdater(HardwareMap hardwareMap, Localizer localizer) { - this.hardwareMap = hardwareMap; - - for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { - module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); - } - - this.localizer = localizer; - imu = localizer.getIMU(); - } - - /** - * Creates a new PoseUpdater from a HardwareMap. - * - * @param hardwareMap the HardwareMap - */ - public PoseUpdater(HardwareMap hardwareMap) { - // TODO: replace the second argument with your preferred localizer - this(hardwareMap, new PinpointLocalizer(hardwareMap)); - } - - /** - * This updates the robot's pose, as well as updating the previous pose, velocity, and - * acceleration. The cache for the current pose, velocity, and acceleration is cleared, and - * the time stamps are updated as well. - */ - public void update() { - previousVelocity = getVelocity(); - previousPose = applyOffset(getRawPose()); - currentPose = null; - currentVelocity = null; - currentAcceleration = null; - previousPoseTime = currentPoseTime; - currentPoseTime = System.nanoTime(); - localizer.update(); - } - - /** - * This sets the starting pose. Do not run this after moving at all. - * - * @param set the Pose to set the starting pose to. - */ - public void setStartingPose(Pose set) { - startingPose = set; - previousPose = startingPose; - previousPoseTime = System.nanoTime(); - currentPoseTime = System.nanoTime(); - localizer.setStartPose(set); - } - - /** - * This sets the current pose, using offsets. Think of using offsets as setting trim in an - * aircraft. This can be reset as well, so beware of using the resetOffset() method. - * - * @param set The pose to set the current pose to. - */ - public void setCurrentPoseWithOffset(Pose set) { - Pose currentPose = getRawPose(); - setXOffset(set.getX() - currentPose.getX()); - setYOffset(set.getY() - currentPose.getY()); - setHeadingOffset(MathFunctions.getTurnDirection(currentPose.getHeading(), set.getHeading()) * MathFunctions.getSmallestAngleDifference(currentPose.getHeading(), set.getHeading())); - } - - /** - * This sets the offset for only the x position. - * - * @param offset This sets the offset. - */ - public void setXOffset(double offset) { - xOffset = offset; - } - - /** - * This sets the offset for only the y position. - * - * @param offset This sets the offset. - */ - public void setYOffset(double offset) { - yOffset = offset; - } - - /** - * This sets the offset for only the heading. - * - * @param offset This sets the offset. - */ - public void setHeadingOffset(double offset) { - headingOffset = offset; - } - - /** - * This returns the x offset. - * - * @return returns the x offset. - */ - public double getXOffset() { - return xOffset; - } - - /** - * This returns the y offset. - * - * @return returns the y offset. - */ - public double getYOffset() { - return yOffset; - } - - /** - * This returns the heading offset. - * - * @return returns the heading offset. - */ - public double getHeadingOffset() { - return headingOffset; - } - - /** - * This applies the offset to a specified Pose. - * - * @param pose The pose to be offset. - * @return This returns a new Pose with the offset applied. - */ - public Pose applyOffset(Pose pose) { - return new Pose(pose.getX() + xOffset, pose.getY() + yOffset, pose.getHeading() + headingOffset); - } - - /** - * This resets all offsets set to the PoseUpdater. If you have reset your pose using the - * setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the - * PoseUpdater thinks your pose would be, not the pose you reset to. - */ - public void resetOffset() { - setXOffset(0); - setYOffset(0); - setHeadingOffset(0); - } - - /** - * This returns the current pose, with offsets applied. If this is called multiple times in - * a single update, the current pose is cached so that subsequent calls don't have to repeat - * localizer calls or calculations. - * - * @return returns the current pose. - */ - public Pose getPose() { - if (currentPose == null) { - currentPose = localizer.getPose(); - return applyOffset(currentPose); - } else { - return applyOffset(currentPose); - } - } - - /** - * This returns the current raw pose, without any offsets applied. If this is called multiple times in - * a single update, the current pose is cached so that subsequent calls don't have to repeat - * localizer calls or calculations. - * - * @return returns the raw pose. - */ - public Pose getRawPose() { - if (currentPose == null) { - currentPose = localizer.getPose(); - return currentPose; - } else { - return currentPose; - } - } - - /** - * This sets the current pose without using resettable offsets. - * - * @param set the pose to set the current pose to. - */ - public void setPose(Pose set) { - resetOffset(); - localizer.setPose(set); - } - - /** - * Returns the robot's pose from the previous update. - * - * @return returns the robot's previous pose. - */ - public Pose getPreviousPose() { - return previousPose; - } - - /** - * Returns the robot's change in pose from the previous update. - * - * @return returns the robot's delta pose. - */ - public Pose getDeltaPose() { - Pose returnPose = getPose(); - returnPose.subtract(previousPose); - return returnPose; - } - - /** - * This returns the velocity of the robot as a Vector. If this is called multiple times in - * a single update, the velocity Vector is cached so that subsequent calls don't have to repeat - * localizer calls or calculations. - * - * @return returns the velocity of the robot. - */ - public Vector getVelocity() { - if (currentVelocity == null) { -// currentVelocity = new Vector(); -// currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); -// currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); - currentVelocity = localizer.getVelocityVector(); - return MathFunctions.copyVector(currentVelocity); - } else { - return MathFunctions.copyVector(currentVelocity); - } - } - - /** - * This returns the angular velocity of the robot as a double. - * - * @return returns the angular velocity of the robot. - */ - public double getAngularVelocity() { - return MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(getPose().getHeading(), previousPose.getHeading()) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9)); - } - - /** - * This returns the acceleration of the robot as a Vector. If this is called multiple times in - * a single update, the acceleration Vector is cached so that subsequent calls don't have to - * repeat localizer calls or calculations. - * - * @return returns the acceleration of the robot. - */ - public Vector getAcceleration() { - if (currentAcceleration == null) { - currentAcceleration = MathFunctions.subtractVectors(getVelocity(), previousVelocity); - currentAcceleration.setMagnitude(currentAcceleration.getMagnitude() / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); - return MathFunctions.copyVector(currentAcceleration); - } else { - return MathFunctions.copyVector(currentAcceleration); - } - } - - /** - * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. - */ - public void resetHeadingToIMU() { - if (imu != null) { - localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); - } - } - - /** - * This resets the heading of the robot to the IMU's heading, using offsets instead of Road - * Runner's pose reset. This way, it's faster, but this can be wiped with the resetOffsets() - * method. - */ - public void resetHeadingToIMUWithOffsets() { - if (imu != null) { - setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); - } - } - - /** - * This returns the IMU heading normalized to be between [0, 2 PI] radians. - * - * @return returns the normalized IMU heading. - */ - public double getNormalizedIMUHeading() { - if (imu != null) { - return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - } - return 0; - } - - /** - * This returns the total number of radians the robot has turned. - * - * @return the total heading. - */ - public double getTotalHeading() { - return localizer.getTotalHeading(); - } - - /** - * This returns the Localizer. - * - * @return the Localizer - */ - public Localizer getLocalizer() { - return localizer; - } - - /** - * - */ - public void resetIMU() throws InterruptedException { - localizer.resetIMU(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt deleted file mode 100644 index 76d825c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt +++ /dev/null @@ -1,46 +0,0 @@ -package com.acmerobotics.roadrunner.ftc - -import com.qualcomm.hardware.sparkfun.SparkFunOTOS -import com.qualcomm.robotcore.hardware.I2cDeviceSynch -import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties -import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType -import java.util.* -@I2cDeviceType -@DeviceProperties( - name = "SparkFun OTOS Corrected", - xmlTag = "SparkFunOTOS2", - description = "SparkFun Qwiic Optical Tracking Odometry Sensor Corrected" -) -class SparkFunOTOSCorrected(deviceClient: I2cDeviceSynch) : SparkFunOTOS(deviceClient) { - /** - * Gets only the position and velocity measured by the - * OTOS in a single burst read - * @param pos Position measured by the OTOS - * @param vel Velocity measured by the OTOS - */ - fun getPosVel(pos: Pose2D, vel: Pose2D) { - // Read all pose registers - val rawData = deviceClient.read(REG_POS_XL.toInt(), 12) - - // Convert raw data to pose units - pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD)) - vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS)) - } - - // Modified version of poseToRegs to fix pose setting issue - // see https://discord.com/channels/225450307654647808/1246977443030368349/1271702497659977760 - override fun poseToRegs(rawData: ByteArray, pose: Pose2D, xyToRaw: Double, hToRaw: Double) { - // Convert pose units to raw data - val rawX = (_distanceUnit.toMeters(pose.x) * xyToRaw).toInt().toShort() - val rawY = (_distanceUnit.toMeters(pose.y) * xyToRaw).toInt().toShort() - val rawH = (_angularUnit.toRadians(pose.h) * hToRaw).toInt().toShort() - - // Store raw data in buffer - rawData[0] = (rawX.toInt() and 0xFF).toByte() - rawData[1] = ((rawX.toInt() shr 8) and 0xFF).toByte() - rawData[2] = (rawY.toInt() and 0xFF).toByte() - rawData[3] = ((rawY.toInt() shr 8) and 0xFF).toByte() - rawData[4] = (rawH.toInt() and 0xFF).toByte() - rawData[5] = ((rawH.toInt() shr 8) and 0xFF).toByte() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java deleted file mode 100644 index 5f3290a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ /dev/null @@ -1,272 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the drive encoder set up. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -@Config -public class DriveEncoderLocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder leftFront; - private Encoder rightFront; - private Encoder leftRear; - private Encoder rightRear; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 1; - public static double STRAFE_TICKS_TO_INCHES = 1; - public static double TURN_TICKS_TO_RADIANS = 1; - public static double ROBOT_WIDTH = 1; - public static double ROBOT_LENGTH = 1; - - /** - * This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public DriveEncoderLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; - - leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName)); - leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName)); - rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName)); - rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName)); - - // TODO: reverse any encoders necessary - leftFront.setDirection(Encoder.REVERSE); - leftRear.setDirection(Encoder.REVERSE); - rightFront.setDirection(Encoder.FORWARD); - rightRear.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders. Then, the robot's global change in position is calculated - * using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders. - */ - public void updateEncoders() { - leftFront.update(); - rightFront.update(); - leftRear.update(); - rightRear.update(); - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - leftFront.reset(); - rightFront.reset(); - leftRear.reset(); - rightRear.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition())); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition())); - // theta/turning - returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH))); - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return TURN_TICKS_TO_RADIANS; - } - - /** - * This does nothing since this localizer does not use the IMU. - */ - public void resetIMU() { - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java deleted file mode 100644 index 0f309a5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ /dev/null @@ -1,227 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import com.qualcomm.hardware.sparkfun.SparkFunOTOS; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the SparkFun OTOS. The diagram below, which is modified from - * Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || || | - * left (y positive) <--- | || || | - * | ____ | - * | ---- | - * \--------------/ - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/20/2024 - */ -public class OTOSLocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private SparkFunOTOS otos; - private SparkFunOTOS.Pose2D otosPose; - private SparkFunOTOS.Pose2D otosVel; - private SparkFunOTOS.Pose2D otosAcc; - private double previousHeading; - private double totalHeading; - - /** - * This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public OTOSLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public OTOSLocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; - - /* - TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the - 'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a - "SparkFunOTOS Corrected" in your robot config - */ - // TODO: replace this with your OTOS port - otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); - - otos.setLinearUnit(DistanceUnit.INCH); - otos.setAngularUnit(AngleUnit.RADIANS); - - // TODO: replace this with your OTOS offset from the center of the robot - // For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being - // positive y and forward being positive x. PI/2 radians is facing forward, and clockwise - // rotation is negative rotation. - otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2)); - - // TODO: replace these with your tuned multipliers - otos.setLinearScalar(1.0); - otos.setAngularScalar(1.0); - - otos.calibrateImu(); - otos.resetTracking(); - - setStartPose(setStartPose); - otosPose = new SparkFunOTOS.Pose2D(); - otosVel = new SparkFunOTOS.Pose2D(); - otosAcc = new SparkFunOTOS.Pose2D(); - totalHeading = 0; - previousHeading = startPose.getHeading(); - - resetOTOS(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.h); - - Vector vec = pose.getVector(); - vec.rotateVector(startPose.getHeading()); - - return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return new Pose(otosVel.x, otosVel.y, otosVel.h); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return getVelocity().getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - resetOTOS(); - Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose); - otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading())); - } - - /** - * This updates the total heading of the robot. The OTOS handles all other updates itself. - */ - @Override - public void update() { - otos.getPosVelAcc(otosPose,otosVel,otosAcc); - totalHeading += MathFunctions.getSmallestAngleDifference(otosPose.h, previousHeading); - previousHeading = otosPose.h; - } - - /** - * This resets the OTOS. - */ - public void resetOTOS() { - otos.resetTracking(); - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from OTOS - * ticks to inches. For the OTOS, this value is the same as the lateral multiplier. - * This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return otos.getLinearScalar(); - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier. - * This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return otos.getLinearScalar(); - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from OTOS ticks - * to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return otos.getAngularScalar(); - } - - /** - * This does nothing since this localizer does not use the IMU. - */ - public void resetIMU() { - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java deleted file mode 100644 index d3d3fda..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ /dev/null @@ -1,245 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; -import org.opencv.core.Mat; - -/** - * This is the Pinpoint class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading - * readings. The diagram below, which is modified from Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * /--------------\ - * | ____ | - * | ---- | - * | || | - * | || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) - * With the pinpoint your readings will be used in mm - * to use inches ensure to divide your mm value by 25.4 - * @author Logan Nash - * @author Havish Sripada 12808 - RevAmped Robotics - * @author Ethan Doak - Gobilda - * @version 1.0, 10/2/2024 - */ -public class PinpointLocalizer extends Localizer { - private HardwareMap hardwareMap; - private GoBildaPinpointDriver odo; - private double previousHeading; - private double totalHeading; - private Pose startPose; - private long deltaTimeNano; - private NanoTimer timer; - private Pose currentVelocity; - private Pose previousPinpointPose; - - /** - * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} - - /** - * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public PinpointLocalizer(HardwareMap map, Pose setStartPose){ - hardwareMap = map; - - odo = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint"); - - //The default units are inches, but you can swap the units if you wish. - //If you have already tuned the TwoWheelLocalizer, you can simply use the forwardEncoderPose's y value and strafeEncoderPose's x values. - setOffsets(-2.815, 0.125, DistanceUnit.INCH); //these are tuned for 3110-0002-0001 Product Insight #1 - - //TODO: Tune urself if needed -// odo.setYawScalar(1.0); - - odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - //odo.setEncoderResolution(13.26291192); - - odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.REVERSED, GoBildaPinpointDriver.EncoderDirection.FORWARD); - - resetPinpoint(); - - setStartPose(setStartPose); - totalHeading = 0; - timer = new NanoTimer(); - previousPinpointPose = new Pose(); - currentVelocity = new Pose(); - deltaTimeNano = 1; - previousHeading = setStartPose.getHeading(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(previousPinpointPose, startPose.getHeading(), false)); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Since nobody should be using this after the robot has begun moving, - * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - this.startPose = setStart; - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - Pose setNewPose = MathFunctions.subtractPoses(setPose, startPose); - odo.setPosition(new Pose2D(DistanceUnit.INCH, setNewPose.getX(), setNewPose.getY(), AngleUnit.RADIANS, setNewPose.getHeading())); - } - - /** - * This updates the total heading of the robot. The Pinpoint handles all other updates itself. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - odo.update(); - Pose2D pinpointPose = odo.getPosition(); - Pose currentPinpointPose = new Pose(pinpointPose.getX(DistanceUnit.INCH), pinpointPose.getY(DistanceUnit.INCH), pinpointPose.getHeading(AngleUnit.RADIANS)); - totalHeading += MathFunctions.getSmallestAngleDifference(currentPinpointPose.getHeading(), previousHeading); - previousHeading = currentPinpointPose.getHeading(); - Pose deltaPose = MathFunctions.subtractPoses(currentPinpointPose, previousPinpointPose); - currentVelocity = new Pose(deltaPose.getX() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getY() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getHeading() / (deltaTimeNano / Math.pow(10.0, 9))); - previousPinpointPose = currentPinpointPose; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - @Override - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the Y encoder value as none of the odometry tuners are required for this localizer - * @return returns the Y encoder value - */ - @Override - public double getForwardMultiplier() { - return odo.getEncoderY(); - } - - /** - * This returns the X encoder value as none of the odometry tuners are required for this localizer - * @return returns the X encoder value - */ - @Override - public double getLateralMultiplier() { - return odo.getEncoderX(); - } - - /** - * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. - * @return returns the yaw scalar - */ - @Override - public double getTurningMultiplier() { - return odo.getYawScalar(); - } - - /** - * This sets the offsets and converts inches to millimeters - * @param xOffset How far to the side from the center of the robot is the x-pod? Use positive values if it's to the left and negative if it's to the right. - * @param yOffset How far forward from the center of the robot is the y-pod? Use positive values if it's forward and negative if it's to the back. - * @param unit The units that the measurements are given in - */ - private void setOffsets(double xOffset, double yOffset, DistanceUnit unit) { - odo.setOffsets(unit.toMm(xOffset), unit.toMm(yOffset)); - } - - /** - * This resets the IMU. Does not change heading estimation. - */ - @Override - public void resetIMU() throws InterruptedException { - odo.recalibrateIMU(); - - try { - Thread.sleep(300); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } - - /** - * This resets the pinpoint. - */ - private void resetPinpoint() { - odo.resetPosAndIMU(); - - try { - Thread.sleep(300); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java deleted file mode 100644 index 15275f0..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java +++ /dev/null @@ -1,159 +0,0 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -// -//import java.util.ArrayList; -//import java.util.List; -// -///** -// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and -// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing -// * localizer system. -// * -// * @author Anyi Lin - 10158 Scott's Bots -// * @version 1.0, 5/9/2024 -// */ -//public class RRToPedroThreeWheelLocalizer extends Localizer { -// private RoadRunnerThreeWheelLocalizer localizer; -// private double totalHeading; -// private Pose startPose; -// private Pose previousPose; -// -// /** -// * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously -// * used Road Runner localization system to the new Pedro Pathing localization system. -// * -// * @param hardwareMap the HardwareMap -// */ -// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { -// List lastTrackingEncPositions = new ArrayList<>(); -// List lastTrackingEncVels = new ArrayList<>(); -// -// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); -// -// startPose = new Pose(); -// previousPose = new Pose(); -// } -// -// /** -// * This returns the current pose estimate as a Pose. -// * -// * @return returns the current pose estimate -// */ -// @Override -// public Pose getPose() { -// Pose2d pose = localizer.getPoseEstimate(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// /** -// * This returns the current velocity estimate as a Pose. -// * -// * @return returns the current velocity estimate -// */ -// @Override -// public Pose getVelocity() { -// Pose2d pose = localizer.getPoseVelocity(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// /** -// * This returns the current velocity estimate as a Vector. -// * -// * @return returns the current velocity estimate -// */ -// @Override -// public Vector getVelocityVector() { -// Pose2d pose = localizer.getPoseVelocity(); -// Vector returnVector = new Vector(); -// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); -// return returnVector; -// } -// -// /** -// * This sets the start pose. Any movement of the robot is treated as a displacement from the -// * start pose, so moving the start pose will move the current pose estimate the same amount. -// * -// * @param setStart the new start pose -// */ -// @Override -// public void setStartPose(Pose setStart) { -// Pose oldStart = startPose; -// startPose = setStart; -// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); -// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); -// } -// -// /** -// * This sets the current pose estimate. This has no effect on the start pose. -// * -// * @param setPose the new current pose estimate -// */ -// @Override -// public void setPose(Pose setPose) { -// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); -// } -// -// /** -// * This updates the total heading and previous pose estimate. Everything else is handled by the -// * Road Runner localizer on its own, but updating this tells you how far the robot has really -// * turned. -// */ -// @Override -// public void update() { -// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); -// previousPose = getPose(); -// } -// -// /** -// * This returns how far the robot has actually turned. -// * -// * @return returns the total angle turned, in degrees. -// */ -// @Override -// public double getTotalHeading() { -// return totalHeading; -// } -// -// /** -// * This returns the forward multiplier of the Road Runner localizer, which converts from ticks -// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything -// * multiplied together should be. If you do use that, then do be aware that the value returned is -// * the product of the Road Runner ticks to inches and the x multiplier. -// * -// * @return returns the forward multiplier -// */ -// @Override -// public double getForwardMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; -// } -// -// /** -// * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks -// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything -// * multiplied together should be. If you do use that, then do be aware that the value returned is -// * the product of the Road Runner ticks to inches and the y multiplier. -// * -// * @return returns the lateral multiplier -// */ -// @Override -// public double getLateralMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; -// } -// -// /** -// * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. -// * There really isn't a point in tuning the turning for the Road Runner localizer. This will -// * actually just return the average of the two other multipliers. -// * -// * @return returns the turning multiplier -// */ -// @Override -// public double getTurningMultiplier() { -// return (getForwardMultiplier() + getLateralMultiplier()) / 2; -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java deleted file mode 100644 index d743b49..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java +++ /dev/null @@ -1,132 +0,0 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import com.acmerobotics.roadrunner.util.NanoClock; -//import com.qualcomm.robotcore.hardware.DcMotorEx; -//import com.qualcomm.robotcore.hardware.DcMotorSimple; -// -///** -// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a -// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected -// * velocity counts and allow reversing independently of the corresponding slot's motor direction. -// * -// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have -// * import statements, so I'm not crediting myself as an author for this. -// * -// * @author Road Runner dev team -// * @version 1.0, 5/9/2024 -// */ -//public class RoadRunnerEncoder { -// private final static int CPS_STEP = 0x10000; -// -// private static double inverseOverflow(double input, double estimate) { -// // convert to uint16 -// int real = (int) input & 0xffff; -// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits -// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window -// real += ((real % 20) / 4) * CPS_STEP; -// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by -// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; -// return real; -// } -// -// public enum Direction { -// FORWARD(1), -// REVERSE(-1); -// -// private int multiplier; -// -// Direction(int multiplier) { -// this.multiplier = multiplier; -// } -// -// public int getMultiplier() { -// return multiplier; -// } -// } -// -// private DcMotorEx motor; -// private NanoClock clock; -// -// private Direction direction; -// -// private int lastPosition; -// private int velocityEstimateIdx; -// private double[] velocityEstimates; -// private double lastUpdateTime; -// -// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { -// this.motor = motor; -// this.clock = clock; -// -// this.direction = Direction.FORWARD; -// -// this.lastPosition = 0; -// this.velocityEstimates = new double[3]; -// this.lastUpdateTime = clock.seconds(); -// } -// -// public RoadRunnerEncoder(DcMotorEx motor) { -// this(motor, NanoClock.system()); -// } -// -// public Direction getDirection() { -// return direction; -// } -// -// private int getMultiplier() { -// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); -// } -// -// /** -// * 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 -// */ -// public void setDirection(Direction direction) { -// this.direction = direction; -// } -// -// /** -// * Gets the position from the underlying motor and adjusts for the set direction. -// * Additionally, this method updates the velocity estimates used for compensated velocity -// * -// * @return encoder position -// */ -// public int getCurrentPosition() { -// int multiplier = getMultiplier(); -// int currentPosition = motor.getCurrentPosition() * multiplier; -// if (currentPosition != lastPosition) { -// double currentTime = clock.seconds(); -// double dt = currentTime - lastUpdateTime; -// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; -// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; -// lastPosition = currentPosition; -// lastUpdateTime = currentTime; -// } -// return currentPosition; -// } -// -// /** -// * Gets the velocity directly from the underlying motor and compensates for the direction -// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) -// * -// * @return raw velocity -// */ -// public double getRawVelocity() { -// int multiplier = getMultiplier(); -// return motor.getVelocity() * multiplier; -// } -// -// /** -// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity -// * that are lost in overflow due to velocity being transmitted as 16 bits. -// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. -// * -// * @return corrected velocity -// */ -// public double getCorrectedVelocity() { -// double median = velocityEstimates[0] > velocityEstimates[1] -// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) -// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); -// return inverseOverflow(getRawVelocity(), median); -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java deleted file mode 100644 index 2ee75a9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java +++ /dev/null @@ -1,123 +0,0 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import androidx.annotation.NonNull; -// -//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 java.util.Arrays; -//import java.util.List; -// -///* -// * Sample tracking wheel localizer implementation assuming the standard configuration: -// * -// * left on robot is y pos -// * -// * front of robot is x pos -// * -// * /--------------\ -// * | ____ | -// * | ---- | -// * | || || | -// * | || || | -// * | | -// * | | -// * \--------------/ -// * -// */ -// -///** -// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will -// * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an -// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to -// * Pedro Pathing to avoid having imports. -// * -// * @author Road Runner dev team -// * @author Anyi Lin - 10158 Scott's Bots -// * @version 1.0, 5/9/2024 -// */ -//@Config -//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { -// public static double TICKS_PER_REV = 8192; -// public static double WHEEL_RADIUS = 1.37795; // in -// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed -// -// public static double X_MULTIPLIER = 0.5008239963; -// public static double Y_MULTIPLIER = 0.5018874659; -// -// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; -// -// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; -// -// private List lastEncPositions, lastEncVels; -// -// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { -// super(Arrays.asList( -// new Pose2d(leftX, leftY, 0), // left -// new Pose2d(rightX, rightY, 0), // right -// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe -// )); -// -// lastEncPositions = lastTrackingEncPositions; -// lastEncVels = lastTrackingEncVels; -// -// // TODO: redo the configs here -// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); -// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); -// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); -// -// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) -// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); -// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); -// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); -// } -// -// public void resetHeading(double heading) { -// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); -// } -// -// public static double encoderTicksToInches(double ticks) { -// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; -// } -// -// @NonNull -// @Override -// public List getWheelPositions() { -// int leftPos = leftEncoder.getCurrentPosition(); -// int rightPos = rightEncoder.getCurrentPosition(); -// int frontPos = strafeEncoder.getCurrentPosition(); -// -// lastEncPositions.clear(); -// lastEncPositions.add(leftPos); -// lastEncPositions.add(rightPos); -// lastEncPositions.add(frontPos); -// -// return Arrays.asList( -// encoderTicksToInches(leftPos) * X_MULTIPLIER, -// encoderTicksToInches(rightPos) * X_MULTIPLIER, -// encoderTicksToInches(frontPos) * Y_MULTIPLIER -// ); -// } -// -// @NonNull -// @Override -// public List getWheelVelocities() { -// int leftVel = (int) leftEncoder.getCorrectedVelocity(); -// int rightVel = (int) rightEncoder.getCorrectedVelocity(); -// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); -// -// lastEncVels.clear(); -// lastEncVels.add(leftVel); -// lastEncVels.add(rightVel); -// lastEncVels.add(frontVel); -// -// return Arrays.asList( -// encoderTicksToInches(leftVel) * X_MULTIPLIER, -// encoderTicksToInches(rightVel) * X_MULTIPLIER, -// encoderTicksToInches(frontVel) * Y_MULTIPLIER -// ); -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java deleted file mode 100644 index 83c172e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ /dev/null @@ -1,326 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the three wheel odometry set up with the IMU to have more accurate heading - * readings. The diagram below, which is modified from Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || || | - * left (y positive) <--- | || || | - * | ____ | - * | ---- | - * \--------------/ - * - * @author Logan Nash - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/9/2024 - */ -@Config -public class ThreeWheelIMULocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder leftEncoder; - private Encoder rightEncoder; - private Encoder strafeEncoder; - private Pose leftEncoderPose; - private Pose rightEncoderPose; - private Pose strafeEncoderPose; - - public final IMU imu; - private double previousIMUOrientation; - private double deltaRadians; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5; - - public static boolean useIMU = true; - - /** - * This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public ThreeWheelIMULocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; - imu = hardwareMap.get(IMU.class, "imu"); - - // TODO: replace this with your IMU's orientation - imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP))); - - // TODO: replace these with your encoder positions - leftEncoderPose = new Pose(-3, 5.7, 0); - rightEncoderPose = new Pose(-3, -5.7, 0); - strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90)); - - - // TODO: replace these with your encoder ports - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb")); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf")); - strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rf")); - - // TODO: reverse any encoders necessary - leftEncoder.setDirection(Encoder.REVERSE); - rightEncoder.setDirection(Encoder.FORWARD); - strafeEncoder.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - totalHeading = 0; - - resetEncoders(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders. Then, the robot's global change in position is calculated - * using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders. - */ - public void updateEncoders() { - leftEncoder.update(); - rightEncoder.update(); - strafeEncoder.update(); - - double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); - previousIMUOrientation = currentIMUOrientation; - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - leftEncoder.reset(); - rightEncoder.reset(); - strafeEncoder.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); - // theta/turning - if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) { - returnMatrix.set(2, 0, deltaRadians); - } else { - returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); - } - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return TURN_TICKS_TO_RADIANS; - } - - /** - * This resets the IMU. - */ - public void resetIMU() { - imu.resetYaw(); - } - - /** - * This is returns the IMU. - * - * @return returns the IMU - */ - @Override - public IMU getIMU() { - return imu; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java deleted file mode 100644 index 960a366..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ /dev/null @@ -1,292 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the three wheel odometry set up. The diagram below, which is modified from - * Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || || | - * left (y positive) <--- | || || | - * | ____ | - * | ---- | - * \--------------/ - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -@Config -public class ThreeWheelLocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder leftEncoder; - private Encoder rightEncoder; - private Encoder strafeEncoder; - private Pose leftEncoderPose; - private Pose rightEncoderPose; - private Pose strafeEncoderPose; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5; - - /** - * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public ThreeWheelLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new ThreeWheelLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { - // TODO: replace these with your encoder positions - leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); - rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0); - strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); - - hardwareMap = map; - - // TODO: replace these with your encoder ports - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); - strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); - - // TODO: reverse any encoders necessary - leftEncoder.setDirection(Encoder.REVERSE); - rightEncoder.setDirection(Encoder.REVERSE); - strafeEncoder.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - totalHeading = 0; - - resetEncoders(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders. Then, the robot's global change in position is calculated - * using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders. - */ - public void updateEncoders() { - leftEncoder.update(); - rightEncoder.update(); - strafeEncoder.update(); - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - leftEncoder.reset(); - rightEncoder.reset(); - strafeEncoder.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); - // theta/turning - returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return TURN_TICKS_TO_RADIANS; - } - - /** - * This does nothing since this localizer does not use the IMU. - */ - public void resetIMU() { - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java deleted file mode 100644 index 72cc63b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ /dev/null @@ -1,309 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from - * Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * -* forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || | - * left (y positive) <--- | || | - * | ____ | - * | ---- | - * \--------------/ - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -@Config -public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work - private HardwareMap hardwareMap; - private IMU imu; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder forwardEncoder; - private Encoder strafeEncoder; - private Pose forwardEncoderPose; - private Pose strafeEncoderPose; - private double previousIMUOrientation; - private double deltaRadians; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - - /** - * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public TwoWheelLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) { - // TODO: replace these with your encoder positions - forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); - strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); - - hardwareMap = map; - - imu = hardwareMap.get(IMU.class, "imu"); - // TODO: replace this with your IMU's orientation - imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT))); - - // TODO: replace these with your encoder ports - forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); - strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); - - // TODO: reverse any encoders necessary - forwardEncoder.setDirection(Encoder.REVERSE); - strafeEncoder.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - - previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - deltaRadians = 0; - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders and the IMU readings. Then, the robot's global change in - * position is calculated using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders as well as the IMU. - */ - public void updateEncoders() { - forwardEncoder.update(); - strafeEncoder.update(); - - double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); - previousIMUOrientation = currentIMUOrientation; - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - forwardEncoder.reset(); - strafeEncoder.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders and IMU. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); - // theta/turning - returnMatrix.set(2,0, deltaRadians); - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return 1; - } - - /** - * This resets the IMU. - */ - public void resetIMU() { - imu.resetYaw(); - } - - /** - * This is returns the IMU. - * - * @return returns the IMU - */ - @Override - public IMU getIMU() { - return imu; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java deleted file mode 100644 index 9858caa..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ /dev/null @@ -1,321 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - - -/** - * This is the TwoWheelPinpointIMULocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry with the Pinpoint IMU set up. The diagram below, which is modified from - * Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * -* forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || | - * left (y positive) <--- | || | - * | ____ | - * | ---- | - * \--------------/ - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Havish Sripada - 12808 RevAmped Robotics - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 11/28/2024 - */ -@Config -public class TwoWheelPinpointIMULocalizer extends Localizer { - private HardwareMap hardwareMap; - private GoBildaPinpointDriver pinpoint; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder forwardEncoder; - private Encoder strafeEncoder; - private Pose forwardEncoderPose; - private Pose strafeEncoderPose; - private double previousHeading; - private double deltaRadians; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - - /** - * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public TwoWheelPinpointIMULocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) { - // TODO: replace these with your encoder positions - forwardEncoderPose = new Pose(-0.82, 6.47, 0); - strafeEncoderPose = new Pose(-4, -0.273, Math.toRadians(90)); - - hardwareMap = map; - - pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); - pinpoint.resetPosAndIMU(); - - // TODO: replace these with your encoder ports - forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); - strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); - - // TODO: reverse any encoders necessary - forwardEncoder.setDirection(Encoder.REVERSE); - strafeEncoder.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - previousHeading = startPose.getHeading(); - deltaRadians = 0; - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return new Pose(startPose.getX()+displacementPose.getX(), startPose.getY()+displacementPose.getY(),displacementPose.getHeading()); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3, 3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders and the IMU readings. Then, the robot's global change in - * position is calculated using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3, 3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders as well as the IMU. - */ - public void updateEncoders() { - forwardEncoder.update(); - strafeEncoder.update(); - - pinpoint.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); - double currentHeading = startPose.getHeading() + MathFunctions.normalizeAngle(pinpoint.getHeading()); - deltaRadians = MathFunctions.getTurnDirection(previousHeading, currentHeading) * MathFunctions.getSmallestAngleDifference(currentHeading, previousHeading); - previousHeading = currentHeading; - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - forwardEncoder.reset(); - strafeEncoder.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders and IMU. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3, 1); - // x/forward movement - returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); - //y/strafe movement - returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); - // theta/turning - returnMatrix.set(2, 0, deltaRadians); - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return 1; - } - - /** - * This resets the IMU. - */ - - @Override - public void resetIMU() throws InterruptedException { - pinpoint.recalibrateIMU(); - - try { - Thread.sleep(300); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } - - /** - * This resets the pinpoint. - */ - private void resetPinpoint() throws InterruptedException{ - pinpoint.resetPosAndIMU(); - - try { - Thread.sleep(300); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java deleted file mode 100644 index f5bb12a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java +++ /dev/null @@ -1,348 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - - -import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; - -import java.util.ArrayList; -import java.util.Arrays; - -/** - * This is the BezierCurve class. This class handles the creation of Bezier curves, which are used - * as the basis of the path for the Path class. Bezier curves are parametric curves defined by a set - * of control points. So, they take in one input, t, that ranges from [0, 1] and that returns a point - * on the curve. Essentially, Bezier curves are a way of defining a parametric line easily. You can - * read more on Bezier curves here: https://en.wikipedia.org/wiki/Bézier_curve - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/5/2024 - */ -public class BezierCurve { - // This contains the coefficients for the curve points - private ArrayList pointCoefficients = new ArrayList<>(); - - // This contains the control points for the Bezier curve - private ArrayList controlPoints = new ArrayList<>(); - - private Vector endTangent = new Vector(); - - private final int APPROXIMATION_STEPS = FollowerConstants.APPROXIMATION_STEPS; - - private final int DASHBOARD_DRAWING_APPROXIMATION_STEPS = 100; - - private double[][] dashboardDrawingPoints; - - private double UNIT_TO_TIME; - private double length; - - /** - * This creates an empty BezierCurve. - * IMPORTANT NOTE: Only use this for the constructors of classes extending this. If you try to - * run the robot on a Path containing an empty BezierCurve, then it will explode. - */ - public BezierCurve() { - } - - /** - * This creates a new BezierCurve with an ArrayList of control points and generates the curve. - * IMPORTANT NOTE: The order of the control points is important. That's the order the code will - * process them in, with the 0 index being the start point and the final index being the end point - * - * @param controlPoints This is the ArrayList of control points that define the BezierCurve. - */ - public BezierCurve(ArrayList controlPoints) { - if (controlPoints.size()<3) { - try { - throw new Exception("Too few control points"); - } catch (Exception e) { - e.printStackTrace(); - } - } - this.controlPoints = controlPoints; - initialize(); - } - - /** - * This creates a new Bezier curve with some specified control points and generates the curve. - * IMPORTANT NOTE: The order of the control points is important. That's the order the code will - * process them in, with the 0 index being the start point and the final index being the end point. - * - * @param controlPoints This is the specified control points that define the BezierCurve. - */ - public BezierCurve(Point... controlPoints) { - for (Point controlPoint : controlPoints) { - this.controlPoints.add(controlPoint); - } - if (this.controlPoints.size()<3) { - try { - throw new Exception("Too few control points"); - } catch (Exception e) { - e.printStackTrace(); - } - } - initialize(); - } - - /** - * This handles most of the initialization of the BezierCurve that is called from the constructor. - */ - public void initialize() { - generateBezierCurve(); - length = approximateLength(); - UNIT_TO_TIME = 1/length; - endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); - endTangent = MathFunctions.normalizeVector(endTangent); - initializeDashboardDrawingPoints(); - } - - /** - * This creates the Array that holds the Points to draw on the Dashboard. - */ - public void initializeDashboardDrawingPoints() { - dashboardDrawingPoints = new double[2][DASHBOARD_DRAWING_APPROXIMATION_STEPS + 1]; - for (int i = 0; i <= DASHBOARD_DRAWING_APPROXIMATION_STEPS; i++) { - Point currentPoint = getPoint(i/(double) (DASHBOARD_DRAWING_APPROXIMATION_STEPS)); - dashboardDrawingPoints[0][i] = currentPoint.getX(); - dashboardDrawingPoints[1][i] = currentPoint.getY(); - } - } - - /** - * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC - * Dashboard. - * - * @return returns the 2D Array to draw on FTC Dashboard - */ - public double[][] getDashboardDrawingPoints() { - return dashboardDrawingPoints; - } - - /** - * This generates the Bezier curve. It assumes that the ArrayList of control points has been set. - * Well, this actually generates the coefficients for each control point on the Bezier curve. - * These coefficients can then be used to calculate a position, velocity, or accleration on the - * Bezier curve on the fly without much computational expense. - * - * See https://en.wikipedia.org/wiki/Bézier_curve for the explicit formula for Bezier curves - */ - public void generateBezierCurve() { - int n = controlPoints.size()-1; - for (int i = 0; i <= n; i++) { - pointCoefficients.add(new BezierCurveCoefficients(n, i)); - } - } - - /** - * This returns the unit tangent Vector at the end of the BezierCurve. - * - * @return returns the end tangent Vector. - */ - public Vector getEndTangent() { - return MathFunctions.copyVector(endTangent); - } - - /** - * This approximates the length of the BezierCurve in APPROXIMATION_STEPS number of steps. It's - * like a Riemann's sum, but for a parametric function's arc length. - * - * @return returns the approximated length of the BezierCurve. - */ - public double approximateLength() { - Point previousPoint = getPoint(0); - Point currentPoint; - double approxLength = 0; - for (int i = 1; i <= APPROXIMATION_STEPS; i++) { - currentPoint = getPoint(i/(double)APPROXIMATION_STEPS); - approxLength += previousPoint.distanceFrom(currentPoint); - previousPoint = currentPoint; - } - return approxLength; - } - - /** - * This returns the point on the Bezier curve that is specified by the parametric t value. A - * Bezier curve is a parametric function that returns points along it with t ranging from [0, 1], - * with 0 being the beginning of the curve and 1 being at the end. The Follower will follow - * BezierCurves from 0 to 1, in terms of t. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the point requested. - */ - public Point getPoint(double t) { - t = MathFunctions.clamp(t, 0, 1); - double xCoordinate = 0; - double yCoordinate = 0; - - // calculates the x coordinate of the point requested - for (int i = 0; i < controlPoints.size(); i++) { - xCoordinate += pointCoefficients.get(i).getValue(t) * controlPoints.get(i).getX(); - } - - // calculates the y coordinate of the point requested - for (int i = 0; i < controlPoints.size(); i++) { - yCoordinate += pointCoefficients.get(i).getValue(t) * controlPoints.get(i).getY(); - } - return new Point(xCoordinate, yCoordinate, Point.CARTESIAN); - } - - /** - * This returns the curvature of the Bezier curve at a specified t-value. - * - * @param t the parametric t input. - * @return returns the curvature. - */ - public double getCurvature(double t) { - t = MathFunctions.clamp(t, 0, 1); - Vector derivative = getDerivative(t); - Vector secondDerivative = getSecondDerivative(t); - - if (derivative.getMagnitude() == 0) return 0; - return (MathFunctions.crossProduct(derivative, secondDerivative))/Math.pow(derivative.getMagnitude(),3); - } - - /** - * This returns the derivative on the BezierCurve that is specified by the parametric t value. - * This is returned as a Vector, and this Vector is the tangent to the BezierCurve. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the derivative requested. - */ - public Vector getDerivative(double t) { - t = MathFunctions.clamp(t, 0, 1); - double xCoordinate = 0; - double yCoordinate = 0; - Vector returnVector = new Vector(); - - // calculates the x coordinate of the point requested - for (int i = 0; i < controlPoints.size()-1; i++) { - xCoordinate += pointCoefficients.get(i).getDerivativeValue(t) * (MathFunctions.subtractPoints(controlPoints.get(i+1), controlPoints.get(i)).getX()); - } - - // calculates the y coordinate of the point requested - for (int i = 0; i < controlPoints.size()-1; i++) {; - yCoordinate += pointCoefficients.get(i).getDerivativeValue(t) * (MathFunctions.subtractPoints(controlPoints.get(i+1), controlPoints.get(i)).getY()); - } - - returnVector.setOrthogonalComponents(xCoordinate, yCoordinate); - - return returnVector; - } - - /** - * This returns the second derivative on the BezierCurve that is specified by the parametric t value. - * This is returned as a Vector, and this Vector is the acceleration on the BezierCurve. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the second derivative requested. - */ - public Vector getSecondDerivative(double t) { - t = MathFunctions.clamp(t, 0, 1); - double xCoordinate = 0; - double yCoordinate = 0; - Vector returnVector = new Vector(); - - // calculates the x coordinate of the point requested - for (int i = 0; i < controlPoints.size()-2; i++) { - xCoordinate += pointCoefficients.get(i).getSecondDerivativeValue(t) * (MathFunctions.addPoints(MathFunctions.subtractPoints(controlPoints.get(i+2), new Point(2*controlPoints.get(i+1).getX(), 2*controlPoints.get(i+1).getY(), Point.CARTESIAN)), controlPoints.get(i)).getX()); - } - - // calculates the y coordinate of the point requested - for (int i = 0; i < controlPoints.size()-2; i++) { - yCoordinate += pointCoefficients.get(i).getSecondDerivativeValue(t) * (MathFunctions.addPoints(MathFunctions.subtractPoints(controlPoints.get(i+2), new Point(2*controlPoints.get(i+1).getX(), 2*controlPoints.get(i+1).getY(), Point.CARTESIAN)), controlPoints.get(i)).getY()); - } - - returnVector.setOrthogonalComponents(xCoordinate, yCoordinate); - - return returnVector; - } - - /** - * Because, for whatever reason, the second derivative returned by the getSecondDerivative(double t) - * method doesn't return the correct heading of the second derivative, this gets an approximate - * second derivative essentially using the limit method. I use this for its heading only. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the approximated second derivative. - */ - public Vector getApproxSecondDerivative(double t) { - double current = getDerivative(t).getTheta(); - double deltaCurrent = getDerivative(t + 0.0001).getTheta(); - - return new Vector(1, deltaCurrent - current); - } - - /** - * Returns the ArrayList of control points for this BezierCurve. - * - * @return This returns the control points. - */ - public ArrayList getControlPoints() { - return controlPoints; - } - - /** - * Returns the first control point for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getFirstControlPoint() { - return controlPoints.get(0); - } - - /** - * Returns the second control point, or the one after the start, for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getSecondControlPoint() { - return controlPoints.get(1); - } - - /** - * Returns the second to last control point for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getSecondToLastControlPoint() { - return controlPoints.get(controlPoints.size()-2); - } - - /** - * Returns the last control point for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getLastControlPoint() { - return controlPoints.get(controlPoints.size()-1); - } - - /** - * Returns the approximate length of this BezierCurve. - * - * @return This returns the length. - */ - public double length() { - return length; - } - - /** - * Returns the conversion factor of one unit of distance into t-value. Since parametric functions - * are defined by t, which can mean time, I use "time" in some method names for conciseness. - * - * @return returns the conversion factor. - */ - public double UNIT_TO_TIME() { - return UNIT_TO_TIME; - } - - /** - * Returns the type of path. This is used in case we need to identify the type of BezierCurve - * this is. - * - * @return returns the type of path. - */ - public String pathType() { - return "curve"; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java deleted file mode 100644 index f8fc51d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -/** - * This is the BezierCurveCoefficients class. This class handles holding the coefficients for each - * control point for the BezierCurve class to allow for faster on the fly calculations of points, - * derivatives, and second derivatives on Bezier curves. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/8/2024 - */ -public class BezierCurveCoefficients { - private double coefficient; - private double derivativeCoefficient; - private double secondDerivativeCoefficient; - - private int n; - private int i; - - /** - * This creates the coefficients within the summation equations for calculating positions, - * derivatives, and second derivatives on Bezier curves. - * - * @param n this is the degree of the Bezier curve function. - * @param i this is the i within the summation equation, so basically which place it is in the - * summation. - */ - public BezierCurveCoefficients(int n, int i) { - this.n = n; - this.i = i; - coefficient = MathFunctions.nCr(n, i); - derivativeCoefficient = MathFunctions.nCr(n - 1, i); - secondDerivativeCoefficient = MathFunctions.nCr(n - 2, i); - } - - /** - * This returns the coefficient for the summation to calculate a position on BezierCurves. - * - * @param t this is the t value within the parametric equation for the Bezier curve. - * @return this returns the coefficient. - */ - public double getValue(double t) { - return coefficient * Math.pow(1 - t, n - i) * Math.pow(t, i); - } - - /** - * This returns the coefficient for the summation to calculate a derivative on BezierCurves. - * - * @param t this is the t value within the parametric equation for the Bezier curve. - * @return this returns the coefficient. - */ - public double getDerivativeValue(double t) { - return n * derivativeCoefficient * Math.pow(t, i) * Math.pow(1 - t, n - i - 1); - } - - /** - * This returns the coefficient for the summation to calculate a second derivative on BezierCurves. - * - * @param t this is the t value within the parametric equation for the Bezier curve. - * @return this returns the coefficient. - */ - public double getSecondDerivativeValue(double t) { - return n * (n - 1) * secondDerivativeCoefficient * Math.pow(t, i) * Math.pow(1 - t, n - i - 2); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java deleted file mode 100644 index 04c7979..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java +++ /dev/null @@ -1,209 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import java.util.ArrayList; - -/** - * This is the BezierLine class. This class handles the creation of BezierLines, which is what I - * call Bezier curves with only two control points. The parent BezierCurve class cannot handle Bezier - * curves with less than three control points, so this class handles lines. Additionally, it makes - * the calculations done on the fly a little less computationally expensive and more streamlined. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/9/2024 - */ -public class BezierLine extends BezierCurve { - - private Point startPoint; - private Point endPoint; - - private Vector endTangent; - - private double UNIT_TO_TIME; - private double length; - - /** - * This creates a new BezierLine with specified start and end Points. - * This is just a line but it extends the BezierCurve class so things work. - * - * @param startPoint start point of the line. - * @param endPoint end point of the line. - */ - public BezierLine(Point startPoint, Point endPoint) { - super(); - this.startPoint = startPoint; - this.endPoint = endPoint; - length = approximateLength(); - UNIT_TO_TIME = 1 / length; - endTangent = MathFunctions.normalizeVector(getDerivative(1)); - super.initializeDashboardDrawingPoints(); - } - - /** - * This returns the unit tangent Vector at the end of the BezierLine. - * - * @return returns the tangent Vector. - */ - @Override - public Vector getEndTangent() { - return MathFunctions.copyVector(endTangent); - } - - /** - * This gets the length of the BezierLine. - * - * @return returns the length of the BezierLine. - */ - @Override - public double approximateLength() { - return Math.sqrt(Math.pow(startPoint.getX() - endPoint.getX(), 2) + Math.pow(startPoint.getY() - endPoint.getY(), 2)); - } - - /** - * This returns the Point on the Bezier line that is specified by the parametric t value. - * - * @param t this is the t value of the parametric line. t is clamped to be between 0 and 1 inclusive. - * @return this returns the Point requested. - */ - @Override - public Point getPoint(double t) { - t = MathFunctions.clamp(t, 0, 1); - return new Point((endPoint.getX() - startPoint.getX()) * t + startPoint.getX(), (endPoint.getY() - startPoint.getY()) * t + startPoint.getY(), Point.CARTESIAN); - } - - /** - * This returns the curvature of the BezierLine, which is zero. - * - * @param t the parametric t value. - * @return returns the curvature. - */ - @Override - public double getCurvature(double t) { - return 0.0; - } - - /** - * This returns the derivative on the BezierLine as a Vector, which is a constant slope. - * The t value doesn't really do anything, but it's there so I can override methods. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the derivative requested. - */ - @Override - public Vector getDerivative(double t) { - Vector returnVector = new Vector(); - - returnVector.setOrthogonalComponents(endPoint.getX() - startPoint.getX(), endPoint.getY() - startPoint.getY()); - - return returnVector; - } - - /** - * This returns the second derivative on the Bezier line, which is a zero Vector. - * Once again, the t is only there for the override. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the second derivative requested. - */ - @Override - public Vector getSecondDerivative(double t) { - return new Vector(); - } - - /** - * This returns the zero Vector, but it's here so I can override the method in the BezierCurve - * class. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the approximated second derivative, which is the zero Vector. - */ - @Override - public Vector getApproxSecondDerivative(double t) { - return new Vector(); - } - - /** - * Returns the ArrayList of control points for this BezierLine. - * - * @return This returns the control points. - */ - @Override - public ArrayList getControlPoints() { - ArrayList returnList = new ArrayList<>(); - returnList.add(startPoint); - returnList.add(endPoint); - return returnList; - } - - /** - * Returns the first control point for this BezierLine. - * - * @return This returns the Point. - */ - @Override - public Point getFirstControlPoint() { - return startPoint; - } - - /** - * Returns the second control point, or the one after the start, for this BezierLine. - * - * @return This returns the Point. - */ - @Override - public Point getSecondControlPoint() { - return endPoint; - } - - /** - * Returns the second to last control point for this BezierLine. - * - * @return This returns the Point. - */ - @Override - public Point getSecondToLastControlPoint() { - return startPoint; - } - - /** - * Returns the last control point for this BezierLine. - * - * @return This returns the Point. - */ - @Override - public Point getLastControlPoint() { - return endPoint; - } - - /** - * Returns the length of this BezierLine. - * - * @return This returns the length. - */ - @Override - public double length() { - return length; - } - - /** - * Returns the conversion factor of one unit of distance into t value. - * - * @return returns the conversion factor. - */ - @Override - public double UNIT_TO_TIME() { - return UNIT_TO_TIME; - } - - /** - * Returns the type of path. This is used in case we need to identify the type of BezierCurve - * this is. - * - * @return returns the type of path. - */ - @Override - public String pathType() { - return "line"; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java deleted file mode 100644 index cd2382e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java +++ /dev/null @@ -1,209 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import java.util.ArrayList; - -/** - * This is the BezierPoint class. This class handles the creation of BezierPoints, which is what I - * call Bezier curves with only one control point. The parent BezierCurve class cannot handle Bezier - * curves with less than three control points, so this class handles points. Additionally, it makes - * the calculations done on the fly a little less computationally expensive and more streamlined. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/9/2024 - */ -public class BezierPoint extends BezierCurve { - - private Point point; - - private Vector endTangent = new Vector(); - - private double UNIT_TO_TIME; - private double length; - - /** - * This creates a new BezierPoint with a specified Point. - * This is just a point but it extends the BezierCurve class so things work. - * - * @param point the specified point. - */ - public BezierPoint(Point point) { - super(); - this.point = point; - length = approximateLength(); - super.initializeDashboardDrawingPoints(); - } - - /** - * This supposedly returns the unit tangent Vector at the end of the path, but since there is - * no end tangent of a point, this returns a zero Vector instead. Holding BezierPoints in the - * Follower doesn't use the drive Vector, so the end tangent Vector is not needed or truly used. - * - * @return returns the zero Vector. - */ - @Override - public Vector getEndTangent() { - return MathFunctions.copyVector(endTangent); - } - - /** - * This gets the length of the BezierPoint. Since points don't have length, this returns zero. - * - * @return returns the length of the BezierPoint. - */ - @Override - public double approximateLength() { - return 0.0; - } - - /** - * This returns the point on the BezierPoint that is specified by the parametric t value. Since - * this is a Point, this just returns the one control point's position. - * - * @param t this is the t value of the parametric line. t is clamped to be between 0 and 1 inclusive. - * @return this returns the Point requested. - */ - @Override - public Point getPoint(double t) { - return new Point(point.getX(), point.getY(), Point.CARTESIAN); - } - - /** - * This returns the curvature of the BezierPoint, which is zero since this is a Point. - * - * @param t the parametric t value. - * @return returns the curvature, which is zero. - */ - @Override - public double getCurvature(double t) { - return 0.0; - } - - /** - * This returns the derivative on the BezierPoint, which is the zero Vector since this is a Point. - * The t value doesn't really do anything, but it's there so I can override methods. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the derivative requested, which is the zero Vector. - */ - @Override - public Vector getDerivative(double t) { - return MathFunctions.copyVector(endTangent); - } - - /** - * This returns the second derivative on the Bezier line, which is the zero Vector since this - * is a Point. - * Once again, the t is only there for the override. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the second derivative requested, which is the zero Vector. - */ - @Override - public Vector getSecondDerivative(double t) { - return new Vector(); - } - - /** - * This returns the zero Vector, but it's here so I can override the method in the BezierCurve - * class. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the approximated second derivative, which is the zero Vector. - */ - @Override - public Vector getApproxSecondDerivative(double t) { - return new Vector(); - } - - /** - * Returns the ArrayList of control points for this BezierPoint - * - * @return This returns the control point. - */ - @Override - public ArrayList getControlPoints() { - ArrayList returnList = new ArrayList<>(); - returnList.add(point); - return returnList; - } - - /** - * Returns the first, and only, control point for this BezierPoint - * - * @return This returns the Point. - */ - @Override - public Point getFirstControlPoint() { - return point; - } - - /** - * Returns the second control point, or the one after the start, for this BezierPoint. This - * returns the one control point of the BezierPoint, and there are several redundant methods - * that return the same control point, but it's here so I can override methods. - * - * @return This returns the Point. - */ - @Override - public Point getSecondControlPoint() { - return point; - } - - /** - * Returns the second to last control point for this BezierPoint. This - * returns the one control point of the BezierPoint, and there are several redundant methods - * that return the same control point, but it's here so I can override methods. - * - * @return This returns the Point. - */ - @Override - public Point getSecondToLastControlPoint() { - return point; - } - - /** - * Returns the last control point for this BezierPoint. This - * returns the one control point of the BezierPoint, and there are several redundant methods - * that return the same control point, but it's here so I can override methods. - * - * @return This returns the Point. - */ - @Override - public Point getLastControlPoint() { - return point; - } - - /** - * Returns the length of this BezierPoint, which is zero since Points don't have length. - * - * @return This returns the length. - */ - @Override - public double length() { - return length; - } - - /** - * Returns the conversion factor of one unit of distance into t value. There is no length or - * conversion factor to speak of for Points. - * - * @return returns the conversion factor. - */ - @Override - public double UNIT_TO_TIME() { - return 0; - } - - /** - * Returns the type of path. This is used in case we need to identify the type of BezierCurve - * this is. - * - * @return returns the type of path. - */ - @Override - public String pathType() { - return "point"; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java deleted file mode 100644 index bd4e393..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ /dev/null @@ -1,323 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; - -/** - * This is the MathFunctions class. This contains many useful math related methods that I use in - * other classes to simplify code elsewhere. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/9/2024 - */ -public class MathFunctions { - - /** - * This is a simple implementation of the choose function in math. It's equivalent to the number - * of ways you can choose r items from n total items, if only which items got picked and not the - * order of picking the items mattered. - * - * @param n this is how many you want to choose from. - * @param r this is how many you want to choose. - * @return returns the result of the "n choose r" function. - */ - public static double nCr(int n, int r) { - double num = 1; - double denom = 1; - - // this multiplies up the numerator of the nCr function - for (int i = n; i > n - r; i--) { - num *= i; - } - - // this multiplies up the denominator of the nCr function - for (int i = 1; i <= r; i++) { - denom *= i; - } - - return num / denom; - } - - /** - * This returns the sign (positive/negative) of a number. - * - * @param get the number. - * @return returns the sign of the number. - */ - public static double getSign(double get) { - if (get == 0) return 0; - if (get > 0) return 1; - return -1; - } - - /** - * This clamps down a value to between the lower and upper bounds inclusive. - * - * @param num the number to be clamped. - * @param lower the lower bound. - * @param upper the upper bound. - * @return returns the clamped number. - */ - public static double clamp(double num, double lower, double upper) { - if (num < lower) return lower; - if (num > upper) return upper; - return num; - } - - /** - * This normalizes an angle to be between 0 and 2 pi radians, inclusive. - *

- * IMPORTANT NOTE: This method operates in radians. - * - * @param angleRadians the angle to be normalized. - * @return returns the normalized angle. - */ - public static double normalizeAngle(double angleRadians) { - double angle = angleRadians; - while (angle < 0) angle += 2 * Math.PI; - while (angle > 2 * Math.PI) angle -= 2 * Math.PI; - return angle; - } - - /** - * This returns the smallest angle between two angles. This operates in radians. - * - * @param one one of the angles. - * @param two the other one. - * @return returns the smallest angle. - */ - public static double getSmallestAngleDifference(double one, double two) { - return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one)); - } - - /** - * This gets the direction to turn between a start heading and an end heading. Positive is left - * and negative is right. This operates in radians. - * - * @return returns the turn direction. - */ - public static double getTurnDirection(double startHeading, double endHeading) { - if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) { - return 1; // counter clock wise - } - return -1; // clock wise - } - - /** - * This returns the distance between a Pose and a Point, - * - * @param pose this is the Pose. - * @param point this is the Point. - * @return returns the distance between the two. - */ - public static double distance(Pose pose, Point point) { - return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2)); - } - - /** - * This returns the distance between a Pose and another Pose. - * - * @param one this is the first Pose. - * @param two this is the second Pose. - * @return returns the distance between the two. - */ - public static double distance(Pose one, Pose two) { - return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2)); - } - - /** - * This returns a Point that is the sum of the two input Point. - * - * @param one the first Point - * @param two the second Point - * @return returns the sum of the two Points. - */ - public static Point addPoints(Point one, Point two) { - return new Point(one.getX() + two.getX(), one.getY() + two.getY(), Point.CARTESIAN); - } - - /** - * This returns a Pose that is the sum of the two input Pose. - * - * @param one the first Pose - * @param two the second Pose - * @return returns the sum of the two Pose. - */ - public static Pose addPoses(Pose one, Pose two) { - return new Pose(one.getX() + two.getX(), one.getY() + two.getY(), one.getHeading() + two.getHeading()); - } - - /** - * This subtracts the second Point from the first Point and returns the result as a Point. - * Do note that order matters here. - * - * @param one the first Point. - * @param two the second Point. - * @return returns the difference of the two Points. - */ - public static Point subtractPoints(Point one, Point two) { - return new Point(one.getX() - two.getX(), one.getY() - two.getY(), Point.CARTESIAN); - } - - /** - * This subtracts the second Pose from the first Pose and returns the result as a Pose. - * Do note that order matters here. - * - * @param one the first Pose. - * @param two the second Pose. - * @return returns the difference of the two Pose. - */ - public static Pose subtractPoses(Pose one, Pose two) { - return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); - } - - /** - * This rotates the given pose by the given theta, - * - * @param pose the Pose to rotate. - * @param theta the angle to rotate by. - * @param rotateHeading whether to adjust the Pose heading too. - * @return the rotated Pose. - */ - public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { - double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); - double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); - double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); - - return new Pose(x, y, heading); - } - - /** - * This multiplies a Point by a scalar and returns the result as a Point - * - * @param point the Point being multiplied. - * @param scalar the scalar multiplying into the Point. - * @return returns the scaled Point. - */ - public static Point scalarMultiplyPoint(Point point, double scalar) { - return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN); - } - - /** - * Copies a Point, but with a different reference location in the memory. So basically a deep - * copy. - * - * @param point the Point to be deep copied. - * @return returns the copied Point. - */ - public static Point copyPoint(Point point) { - return new Point(point.getX(), point.getY(), Point.CARTESIAN); - } - - /** - * Copies a Vector, but with a different reference location in the memory. So basically a deep - * copy. - * - * @param vector Vector to be deep copied. - * @return returns the copied Vector. - */ - public static Vector copyVector(Vector vector) { - return new Vector(vector.getMagnitude(), vector.getTheta()); - } - - /** - * This multiplies a Vector by a scalar and returns the result as a Vector. - * - * @param vector the Vector being multiplied. - * @param scalar the scalar multiplying into the Vector. - * @return returns the scaled Vector. - */ - public static Vector scalarMultiplyVector(Vector vector, double scalar) { - return new Vector(vector.getMagnitude() * scalar, vector.getTheta()); - } - - /** - * This normalizes a Vector to be of magnitude 1, unless the Vector is the zero Vector. - * In that case, it just returns back the zero Vector but with a different memory location. - * - * @param vector the Vector being normalized. - * @return returns the normalized (or zero) Vector. - */ - public static Vector normalizeVector(Vector vector) { - if (vector.getMagnitude() == 0) { - return new Vector(0.0, vector.getTheta()); - } else { - return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta()); - } - } - - /** - * This returns a Vector that is the sum of the two input Vectors. - * - * @param one the first Vector. - * @param two the second Vector. - * @return returns the sum of the Vectors. - */ - public static Vector addVectors(Vector one, Vector two) { - Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent()); - return returnVector; - } - - /** - * This subtracts the second Vector from the first Vector and returns the result as a Vector. - * Do note that order matters here. - * - * @param one the first Vector. - * @param two the second Vector. - * @return returns the second Vector subtracted from the first Vector. - */ - public static Vector subtractVectors(Vector one, Vector two) { - Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent()); - return returnVector; - } - - /** - * This computes the dot product of the two Vectors. - * - * @param one the first Vector. - * @param two the second Vector. - * @return returns the dot product of the two Vectors. - */ - public static double dotProduct(Vector one, Vector two) { - return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent(); - } - - /** - * This computes the first Vector crossed with the second Vector, so a cross product. - * Do note that order matters here. - * - * @param one the first Vector. - * @param two the second Vector. - * @return returns the cross product of the two Vectors. - */ - public static double crossProduct(Vector one, Vector two) { - return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent(); - } - - /** - * This returns whether a specified value is within a second specified value by plus/minus a - * specified accuracy amount. - * - * @param one first number specified. - * @param two Second number specified. - * @param accuracy the level of accuracy specified. - * @return returns if the two numbers are within the specified accuracy of each other. - */ - public static boolean roughlyEquals(double one, double two, double accuracy) { - return (one < two + accuracy && one > two - accuracy); - } - - /** - * This returns whether a specified number is within a second specified number by plus/minus 0.0001. - * - * @param one first number specified. - * @param two second number specified. - * @return returns if a specified number is within plus/minus 0.0001 of the second specified number. - */ - public static boolean roughlyEquals(double one, double two) { - return roughlyEquals(one, two, 0.0001); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java deleted file mode 100644 index 1539a49..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ /dev/null @@ -1,486 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; - -import java.util.ArrayList; - -/** - * This is the Path class. This class handles containing information on the actual path the Follower - * will follow, not just the shape of the path that the BezierCurve class handles. This contains - * information on the stop criteria for a Path, the heading interpolation, and deceleration. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/10/2024 - */ -public class Path { - private BezierCurve curve; - - private double startHeading; - private double endHeading; - private double closestPointCurvature; - private double closestPointTValue; - private double linearInterpolationEndTime; - - private Vector closestPointTangentVector; - private Vector closestPointNormalVector; - - private boolean isTangentHeadingInterpolation = true; - private boolean followTangentReversed; - - // A multiplier for the zero power acceleration to change the speed the robot decelerates at - // the end of paths. - // Increasing this will cause the robot to try to decelerate faster, at the risk of overshoots - // or localization slippage. - // Decreasing this will cause the deceleration at the end of the Path to be slower, making the - // robot slower but reducing risk of end-of-path overshoots or localization slippage. - // This can be set individually for each Path, but this is the default. - private double zeroPowerAccelerationMultiplier = FollowerConstants.zeroPowerAccelerationMultiplier; - - // When the robot is at the end of its current Path or PathChain and the velocity goes - // this value, then end the Path. This is in inches/second. - // This can be custom set for each Path. - private double pathEndVelocityConstraint = FollowerConstants.pathEndVelocityConstraint; - - // When the robot is at the end of its current Path or PathChain and the translational error - // goes below this value, then end the Path. This is in inches. - // This can be custom set for each Path. - private double pathEndTranslationalConstraint = FollowerConstants.pathEndTranslationalConstraint; - - // When the robot is at the end of its current Path or PathChain and the heading error goes - // below this value, then end the Path. This is in radians. - // This can be custom set for each Path. - private double pathEndHeadingConstraint = FollowerConstants.pathEndHeadingConstraint; - - // When the t-value of the closest point to the robot on the Path is greater than this value, - // then the Path is considered at its end. - // This can be custom set for each Path. - private double pathEndTValueConstraint = FollowerConstants.pathEndTValueConstraint; - - // When the Path is considered at its end parametrically, then the Follower has this many - // milliseconds to further correct by default. - // This can be custom set for each Path. - private double pathEndTimeoutConstraint = FollowerConstants.pathEndTimeoutConstraint; - - /** - * Creates a new Path from a BezierCurve. The default heading interpolation is tangential. - * - * @param curve the BezierCurve. - */ - public Path(BezierCurve curve) { - this.curve = curve; - } - - /** - * This sets the heading interpolation to linear with a specified start heading and end heading - * for the Path. This will interpolate across the entire length of the Path, so there may be - * some issues with end heading accuracy and precision if this is used. If a precise end heading - * is necessary, then use the setLinearHeadingInterpolation(double startHeading, - * double endHeading, double endTime) method. - * - * @param startHeading The start of the linear heading interpolation. - * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. - */ - public void setLinearHeadingInterpolation(double startHeading, double endHeading) { - linearInterpolationEndTime = 1; - isTangentHeadingInterpolation = false; - this.startHeading = startHeading; - this.endHeading = endHeading; - } - - /** - * This sets the heading interpolation to linear with a specified start heading and end heading - * for the Path. This will interpolate from the start of the Path to the specified end time. - * This ensures high accuracy and precision than interpolating across the entire Path. However, - * interpolating too quickly can cause undesired oscillations and inaccuracies of its own, so - * generally interpolating to something like 0.8 of your Path should work best. - * - * @param startHeading The start of the linear heading interpolation. - * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. - * @param endTime The end time on the Path that the linear heading interpolation will finish. - * This value ranges from [0, 1] since Bezier curves are parametric functions. - */ - public void setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { - linearInterpolationEndTime = MathFunctions.clamp(endTime, 0.000000001, 1); - isTangentHeadingInterpolation = false; - this.startHeading = startHeading; - this.endHeading = endHeading; - } - - /** - * This sets the heading interpolation to maintain a constant heading. - * - * @param setHeading the constant heading for the Path. - */ - public void setConstantHeadingInterpolation(double setHeading) { - linearInterpolationEndTime = 1; - isTangentHeadingInterpolation = false; - startHeading = setHeading; - endHeading = setHeading; - } - - /** - * This gets the closest Point from a specified pose to the BezierCurve with a binary search - * that is limited to some specified step limit. - * - * @param pose the pose. - * @param searchStepLimit the binary search step limit. - * @return returns the closest Point. - */ - public Pose getClosestPoint(Pose pose, int searchStepLimit) { - double lower = 0; - double upper = 1; - Point returnPoint; - - // we don't need to calculate the midpoint, so we start off at the 1/4 and 3/4 point - for (int i = 0; i < searchStepLimit; i++) { - if (MathFunctions.distance(pose, getPoint(lower + 0.25 * (upper-lower))) > MathFunctions.distance(pose, getPoint(lower + 0.75 * (upper-lower)))) { - lower += (upper-lower)/2.0; - } else { - upper -= (upper-lower)/2.0; - } - } - - closestPointTValue = lower + 0.5 * (upper-lower); - - returnPoint = getPoint(closestPointTValue); - - closestPointTangentVector = curve.getDerivative(closestPointTValue); - - closestPointNormalVector = curve.getApproxSecondDerivative(closestPointTValue); - - closestPointCurvature = curve.getCurvature(closestPointTValue); - - return new Pose(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); - } - - /** - * This sets whether to follow the tangent heading facing away from (reverse) or towards the - * tangent. This will also set your heading interpolation to tangential. - * - * @param set sets tangential heading reversed or not. - */ - public void setReversed(boolean set) { - isTangentHeadingInterpolation = true; - followTangentReversed = set; - } - - /** - * This sets the heading interpolation to tangential. - */ - public void setTangentHeadingInterpolation() { - isTangentHeadingInterpolation = true; - followTangentReversed = false; - } - - /** - * This returns the unit tangent Vector at the end of the BezierCurve. - * - * @return returns the end tangent Vector. - */ - public Vector getEndTangent() { - return curve.getEndTangent(); - } - - /** - * This returns the point on the Bezier curve that is specified by the parametric t value. A - * Bezier curve is a parametric function that returns points along it with t ranging from [0, 1], - * with 0 being the beginning of the curve and 1 being at the end. The Follower will follow - * BezierCurves from 0 to 1, in terms of t. - * - * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. - * @return this returns the point requested. - */ - public Point getPoint(double t) { - return curve.getPoint(t); - } - - /** - * This returns the t-value of the closest Point on the BezierCurve. - * - * @return returns the closest Point t-value. - */ - public double getClosestPointTValue() { - return closestPointTValue; - } - - /** - * This returns the approximated length of the BezierCurve. - * - * @return returns the length of the BezierCurve. - */ - public double length() { - return curve.length(); - } - - /** - * This returns the curvature of the BezierCurve at a specified t-value. - * - * @param t the specified t-value. - * @return returns the curvature of the BezierCurve at the specified t-value. - */ - public double getCurvature(double t) { - return curve.getCurvature(t); - } - - /** - * This returns the curvature of the BezierCurve at the closest Point. - * - * @return returns the curvature of the BezierCurve at the closest Point. - */ - public double getClosestPointCurvature() { - return closestPointCurvature; - } - - /** - * This returns the normal Vector at the closest Point. - * - * @return returns the normal Vector at the closest Point. - */ - public Vector getClosestPointNormalVector() { - return MathFunctions.copyVector(closestPointNormalVector); - } - - /** - * This returns the tangent Vector at the closest Point. - * - * @return returns the tangent Vector at the closest Point. - */ - public Vector getClosestPointTangentVector() { - return MathFunctions.copyVector(closestPointTangentVector); - } - - /** - * This returns the heading goal at the closest Point. - * - * @return returns the heading goal at the closest Point. - */ - public double getClosestPointHeadingGoal() { - if (isTangentHeadingInterpolation) { - if (followTangentReversed) return MathFunctions.normalizeAngle(closestPointTangentVector.getTheta() + Math.PI); - return closestPointTangentVector.getTheta(); - } else { - return getHeadingGoal(closestPointTValue); - } - } - - /** - * This gets the heading goal at a specified t-value. - * - * @param t the specified t-value. - * @return returns the heading goal at the specified t-value. - */ - public double getHeadingGoal(double t) { - if (isTangentHeadingInterpolation) { - if (followTangentReversed) return MathFunctions.normalizeAngle(curve.getDerivative(t).getTheta() + Math.PI); - return curve.getDerivative(t).getTheta(); - } else { - if (t > linearInterpolationEndTime) { - return MathFunctions.normalizeAngle(endHeading); - } - return MathFunctions.normalizeAngle(startHeading + MathFunctions.getTurnDirection(startHeading, endHeading) * MathFunctions.getSmallestAngleDifference(endHeading, startHeading) * (t / linearInterpolationEndTime)); - } - } - - /** - * This returns if the robot is at the end of the Path, according to the parametric t-value. - * - * @return returns if at end. - */ - public boolean isAtParametricEnd() { - if (closestPointTValue >= pathEndTValueConstraint) return true; - return false; - } - - /** - * This returns if the robot is at the beginning of the Path, according to the parametric t-value. - * - * @return returns if at start. - */ - public boolean isAtParametricStart() { - if (closestPointTValue <= 1- pathEndTValueConstraint) return true; - return false; - } - - /** - * Returns the ArrayList of control points for this BezierCurve. - * - * @return This returns the control points. - */ - public ArrayList getControlPoints() { - return curve.getControlPoints(); - } - - /** - * Returns the first control point for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getFirstControlPoint() { - return curve.getFirstControlPoint(); - } - - /** - * Returns the second control point, or the one after the start, for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getSecondControlPoint() { - return curve.getSecondControlPoint(); - } - - /** - * Returns the second to last control point for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getSecondToLastControlPoint() { - return curve.getSecondToLastControlPoint(); - } - - /** - * Returns the last control point for this BezierCurve. - * - * @return This returns the Point. - */ - public Point getLastControlPoint() { - return curve.getLastControlPoint(); - } - - /** - * This sets the path's deceleration factor in terms of the natural deceleration of the robot - * when power is cut from the drivetrain. - * - * @param set This sets the multiplier. - */ - public void setZeroPowerAccelerationMultiplier(double set) { - zeroPowerAccelerationMultiplier = set; - } - - /** - * This sets the velocity stop criteria. When velocity is below this amount, then this is met. - * - * @param set This sets the velocity end constraint. - */ - public void setPathEndVelocityConstraint(double set) { - pathEndVelocityConstraint = set; - } - - /** - * This sets the translational stop criteria. When the translational error, or how far off the - * end point the robot is, goes below this, then the translational end criteria is met. - * - * @param set This sets the translational end constraint. - */ - public void setPathEndTranslationalConstraint(double set) { - pathEndTranslationalConstraint = set; - } - - /** - * This sets the heading stop criteria. When the heading error is less than this amount, then - * the heading end criteria is met. - * - * @param set This sets the heading end constraint. - */ - public void setPathEndHeadingConstraint(double set) { - pathEndHeadingConstraint = set; - } - - /** - * This sets the parametric end criteria. When the t-value of the closest Point on the Path is - * greater than this amount, then the parametric end criteria is met. - * - * @param set This sets the t-value end constraint. - */ - public void setPathEndTValueConstraint(double set) { - pathEndTValueConstraint = set; - } - - /** - * This sets the Path end timeout. If the Path is at its end parametrically, then the Follower - * has this many milliseconds to correct before the Path gets ended anyways. - * - * @param set This sets the Path end timeout. - */ - public void setPathEndTimeoutConstraint(double set) { - pathEndTimeoutConstraint = set; - } - - /** - * This gets the deceleration multiplier. - * - * @return This returns the deceleration multiplier. - */ - public double getZeroPowerAccelerationMultiplier() { - return zeroPowerAccelerationMultiplier; - } - - /** - * This gets the velocity stop criteria. - * - * @return This returns the velocity stop criteria. - */ - public double getPathEndVelocityConstraint() { - return pathEndVelocityConstraint; - } - - /** - * This gets the translational stop criteria. - * - * @return This returns the translational stop criteria. - */ - public double getPathEndTranslationalConstraint() { - return pathEndTranslationalConstraint; - } - - /** - * This gets the heading stop criteria. - * - * @return This returns the heading stop criteria. - */ - public double getPathEndHeadingConstraint() { - return pathEndHeadingConstraint; - } - - /** - * This gets the parametric end criteria. - * - * @return This returns the parametric end criteria. - */ - public double getPathEndTValueConstraint() { - return pathEndTValueConstraint; - } - - /** - * This gets the Path end correction time. - * - * @return This returns the Path end correction time. - */ - public double getPathEndTimeoutConstraint() { - return pathEndTimeoutConstraint; - } - - /** - * Returns the type of path. This is used in case we need to identify the type of BezierCurve - * this is. - * - * @return returns the type of path. - */ - public String pathType() { - return curve.pathType(); - } - - /** - * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC - * Dashboard. - * - * @return returns the 2D Array to draw on FTC Dashboard - */ - public double[][] getDashboardDrawingPoints() { - return curve.getDashboardDrawingPoints(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java deleted file mode 100644 index f044ce7..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java +++ /dev/null @@ -1,249 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import java.util.ArrayList; - -/** - * This is the PathBuilder class. This class makes it easier to create PathChains, so you don't have - * to individually create Path instances to create a PathChain. A PathBuilder can be accessed - * through running the pathBuilder() method on an instance of the Follower class, or just creating - * an instance of PathBuilder regularly. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/11/2024 - */ -public class PathBuilder { - private ArrayList paths = new ArrayList<>(); - - private ArrayList callbacks = new ArrayList<>(); - - /** - * This is an empty constructor for the PathBuilder class so it can get started. - * The PathBuilder allows for easier construction of PathChains. - * The proper usage is using an instance of the Follower class: - * Follower follower = new Follower(hardwareMap); - * Then calling "follower.pathBuilder.[INSERT PATH BUILDING METHODS].build(); - * Of course, you can split up the method calls onto separate lines for readability. - */ - public PathBuilder() { - } - - /** - * This adds a Path to the PathBuilder. - * - * @param path The Path being added. - * @return This returns itself with the updated data. - */ - public PathBuilder addPath(Path path) { - this.paths.add(path); - return this; - } - - /** - * This adds a default Path defined by a specified BezierCurve to the PathBuilder. - * - * @param curve The curve is turned into a Path and added. - * @return This returns itself with the updated data. - */ - public PathBuilder addPath(BezierCurve curve) { - this.paths.add(new Path(curve)); - return this; - } - - /** - * This adds a default Path defined by a specified BezierCurve to the PathBuilder. - * - * @param controlPoints This is the specified control points that define the BezierCurve. - * @return This returns itself with the updated data. - */ - public PathBuilder addBezierCurve(Point... controlPoints) { - return addPath(new BezierCurve(controlPoints)); - } - - /** - * This adds a default Path defined by a specified BezierCurve to the PathBuilder. - * - * @param controlPoints This is the specified control points that define the BezierCurve. - * @return This returns itself with the updated data. - */ - public PathBuilder addBezierCurve(ArrayList controlPoints) { - return addPath(new BezierCurve(controlPoints)); - } - - /** - * This adds a default Path defined by a specified BezierLine to the PathBuilder. - * - * @param startPoint start point of the line. - * @param endPoint end point of the line. - * @return This returns itself with the updated data. - */ - public PathBuilder addBezierLine(Point startPoint, Point endPoint) { - return addPath(new BezierLine(startPoint, endPoint)); - } - - /** - * This sets a linear heading interpolation on the last Path added to the PathBuilder. - * - * @param startHeading The start of the linear heading interpolation. - * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. - * @return This returns itself with the updated data. - */ - public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { - this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading); - return this; - } - - /** - * This sets a linear heading interpolation on the last Path added to the PathBuilder. - * - * @param startHeading The start of the linear heading interpolation. - * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. - * @param endTime The end time on the Path that the linear heading interpolation will end. - * This value goes from [0, 1] since Bezier curves are parametric functions. - * @return This returns itself with the updated data. - */ - public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { - this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); - return this; - } - - /** - * This sets a constant heading interpolation on the last Path added to the PathBuilder. - * - * @param setHeading The constant heading specified. - * @return This returns itself with the updated data. - */ - public PathBuilder setConstantHeadingInterpolation(double setHeading) { - this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading); - return this; - } - - /** - * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. - * - * @param set This sets the heading to reversed tangent following if this parameter is true. - * Conversely, this sets a tangent following if this parameter is false. - * @return This returns itself with the updated data. - */ - public PathBuilder setReversed(boolean set) { - this.paths.get(paths.size() - 1).setReversed(set); - return this; - } - - /** - * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. - * There really shouldn't be a reason to use this since the default heading interpolation is - * tangential but it's here. - */ - public PathBuilder setTangentHeadingInterpolation() { - this.paths.get(paths.size() - 1).setTangentHeadingInterpolation(); - return this; - } - - /** - * This sets the deceleration multiplier on the last Path added to the PathBuilder. - * - * @param set This sets the multiplier for the goal for the deceleration of the robot. - * @return This returns itself with the updated data. - */ - public PathBuilder setZeroPowerAccelerationMultiplier(double set) { - this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set); - return this; - } - - /** - * This sets the path end velocity constraint on the last Path added to the PathBuilder. - * - * @param set This sets the path end velocity constraint. - * @return This returns itself with the updated data. - */ - public PathBuilder setPathEndVelocityConstraint(double set) { - this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set); - return this; - } - - /** - * This sets the path end translational constraint on the last Path added to the PathBuilder. - * - * @param set This sets the path end translational constraint. - * @return This returns itself with the updated data. - */ - public PathBuilder setPathEndTranslationalConstraint(double set) { - this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set); - return this; - } - - /** - * This sets the path end heading constraint on the last Path added to the PathBuilder. - * - * @param set This sets the path end heading constraint. - * @return This returns itself with the updated data. - */ - public PathBuilder setPathEndHeadingConstraint(double set) { - this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set); - return this; - } - - /** - * This sets the path end t-value (parametric time) constraint on the last Path added to the PathBuilder. - * - * @param set This sets the path end t-value (parametric time) constraint. - * @return This returns itself with the updated data. - */ - public PathBuilder setPathEndTValueConstraint(double set) { - this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set); - return this; - } - - /** - * This sets the path end timeout constraint on the last Path added to the PathBuilder. - * - * @param set This sets the path end timeout constraint. - * @return This returns itself with the updated data. - */ - public PathBuilder setPathEndTimeoutConstraint(double set) { - this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set); - return this; - } - - /** - * This adds a temporal callback on the last Path added to the PathBuilder. - * This callback is set to run at a specified number of milliseconds after the start of the path. - * - * @param time This sets the number of milliseconds of wait between the start of the Path and - * the calling of the callback. - * @param runnable This sets the code for the callback to run. Use lambda statements for this. - * @return This returns itself with the updated data. - */ - public PathBuilder addTemporalCallback(double time, Runnable runnable) { - this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1)); - return this; - } - - /** - * This adds a parametric callback on the last Path added to the PathBuilder. - * This callback is set to run at a certain point on the Path. - * - * @param t This sets the t-value (parametric time) on the Path for when to run the callback. - * @param runnable This sets the code for the callback to run. Use lambda statements for this. - * @return This returns itself with the updated data. - */ - public PathBuilder addParametricCallback(double t, Runnable runnable) { - this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1)); - return this; - } - - /** - * This builds all the Path and callback information together into a PathChain. - * - * @return This returns a PathChain made of all the specified paths and callbacks. - */ - public PathChain build() { - PathChain returnChain = new PathChain(paths); - returnChain.setCallbacks(callbacks); - return returnChain; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java deleted file mode 100644 index c02e1d5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java +++ /dev/null @@ -1,78 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import org.firstinspires.ftc.teamcode.pedroPathing.util.SingleRunAction; - -/** - * This is the PathCallback class. This class handles callbacks of Runnables in PathChains. - * Basically, this allows you to run non-blocking code in the middle of PathChains. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/11/2024 - */ -public class PathCallback extends SingleRunAction { - - private double startCondition; - - private int type; - private int index; - - public static final int TIME = 0; - public static final int PARAMETRIC = 1; - - /** - * This creates a new PathCallback with a specified start condition (either time or parametric), - * a Runnable of code to run (preferably a lambda statement), a type (using the class constants), - * and an index for which Path within a PathChain the callback is to run on. - * - * @param startCondition This defines when the callback is to be run, either as a wait time in - * milliseconds or a t-value (parametric time) point. - * @param runnable This contains the code to run when the callback is called. - * @param type This defines the type of callback using the class constants. - * @param index This defines which Path within the PathChain the callback is to run on. - */ - public PathCallback(double startCondition, Runnable runnable, int type, int index) { - super(runnable); - this.startCondition = startCondition; - this.type = type; - if (this.type != TIME || this.type != PARAMETRIC) { - this.type = PARAMETRIC; - } - if (this.type == TIME && this.startCondition < 0) { - this.startCondition = 0.0; - } - if (this.type == PARAMETRIC) { - this.startCondition = MathFunctions.clamp(this.startCondition, 0, 1); - } - this.index = index; - } - - /** - * This returns the type of callback this is (time or parametric). - * - * @return This returns the type of callback. - */ - public int getType() { - return type; - } - - /** - * This returns the start condition for this callback. This will be the wait time in milliseconds - * if this is a time callback or a t-value if this is a parametric callback. - * - * @return This returns the start condition. - */ - public double getStartCondition() { - return startCondition; - } - - /** - * This returns the index of which Path the callback is to run on within the PathChain. - * - * @return This returns the Path index of this callback. - */ - public int getIndex() { - return index; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java deleted file mode 100644 index f350f5e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java +++ /dev/null @@ -1,94 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import java.util.ArrayList; - -/** - * This is the PathChain class. This class handles chaining together multiple Paths into a larger - * collection of Paths that can be run continuously. Additionally, this allows for the addition of - * PathCallbacks to specific Paths in the PathChain, allowing for non-blocking code to be run in - * the middle of a PathChain. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/11/2024 - */ -public class PathChain { - private ArrayList pathChain = new ArrayList<>(); - - private ArrayList callbacks = new ArrayList<>(); - - /** - * This creates a new PathChain from some specified Paths. - * - * IMPORTANT NOTE: Order matters here. The order in which the Paths are input is the order in - * which they will be run. - * - * @param paths the specified Paths. - */ - public PathChain(Path... paths) { - for (Path path : paths) { - pathChain.add(path); - } - } - - /** - * This creates a new PathChain from an ArrayList of Paths. - * - * IMPORTANT NOTE: Order matters here. The order in which the Paths are input is the order in - * which they will be run. - * - * @param paths the ArrayList of Paths. - */ - public PathChain(ArrayList paths) { - pathChain = paths; - } - - /** - * This returns the Path on the PathChain at a specified index. - * - * @param index the index. - * @return returns the Path at the index. - */ - public Path getPath(int index) { - return pathChain.get(index); - } - - /** - * This returns the size of the PathChain. - * - * @return returns the size of the PathChain. - */ - public int size() { - return pathChain.size(); - } - - /** - * This sets the PathCallbacks of the PathChain with some specified PathCallbacks. - * - * @param callbacks the specified PathCallbacks. - */ - public void setCallbacks(PathCallback... callbacks) { - for (PathCallback callback : callbacks) { - this.callbacks.add(callback); - } - } - - /** - * This sets the PathCallbacks of the PathChain with an ArrayList of PathCallbacks. - * - * @param callbacks the ArrayList of PathCallbacks. - */ - public void setCallbacks(ArrayList callbacks) { - this.callbacks = callbacks; - } - - /** - * This returns the PathCallbacks of this PathChain in an ArrayList. - * - * @return returns the PathCallbacks. - */ - public ArrayList getCallbacks() { - return callbacks; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java deleted file mode 100644 index 95a5f05..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ /dev/null @@ -1,203 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -import androidx.annotation.NonNull; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; - -/** - * This is the Point class. This class handles storing information about the location of points in - * 2D space in both Cartesian, or rectangular, and polar coordinates. Additionally, this contains - * the method to find the distance between two Points. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/11/2024 - */ -public class Point { - - // IMPORTANT NOTE: theta is defined in radians. - // These are the values of the coordinate defined by this Point, in both polar and - // Cartesian systems. - private double r; - private double theta; - private double x; - private double y; - - // these are used for ease of changing/setting identification - public static final int POLAR = 0; - public static final int CARTESIAN = 1; - - - /** - * This creates a new Point with coordinate inputs and a specified coordinate system. - * - * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. - * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. - * @param identifier this specifies what coordinate system the coordinate inputs are in. - */ - public Point(double rOrX, double thetaOrY, int identifier) { - setCoordinates(rOrX, thetaOrY, identifier); - } - - /** - * This creates a new Point from a Pose. - * - * @param pose the Pose. - */ - public Point(Pose pose) { - setCoordinates(pose.getX(), pose.getY(), CARTESIAN); - } - - /** - * This creates a new Point from a X and Y value. - * - * @param setX the X value. - * @param setY the Y value. - */ - public Point(double setX, double setY) { - setCoordinates(setX, setY, CARTESIAN); - } - - /** - * This sets the coordinates of the Point using the specified coordinate system. - * - * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. - * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. - * @param identifier this specifies what coordinate system to use when setting values. - */ - public void setCoordinates(double rOrX, double thetaOrY, int identifier) { - double[] setOtherCoordinates; - switch (identifier) { // this detects which coordinate system to use - // there is no POLAR case since that's covered by the default - case CARTESIAN: - x = rOrX; - y = thetaOrY; - setOtherCoordinates = cartesianToPolar(x, y); - r = setOtherCoordinates[0]; - theta = setOtherCoordinates[1]; - break; - default: - if (rOrX < 0) { - r = -rOrX; - theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI); - } else { - r = rOrX; - theta = MathFunctions.normalizeAngle(thetaOrY); - } - setOtherCoordinates = polarToCartesian(r, theta); - x = setOtherCoordinates[0]; - y = setOtherCoordinates[1]; - break; - } - } - - /** - * Calculates the distance between this Point and some other specified Point. - * - * @param otherPoint the other specified Point. - * @return returns the distance between the two Points. - */ - public double distanceFrom(Point otherPoint) { - return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2)); - } - - /** - * This takes in an r and theta value and converts them to Cartesian coordinates. - * - * @param r this is the r value of the Point being converted. - * @param theta this is the theta value of the Point being converted. - * @return this returns the x and y values, in that order, in an Array of doubles. - */ - public static double[] polarToCartesian(double r, double theta) { - return new double[]{r * Math.cos(theta), r * Math.sin(theta)}; - } - - /** - * This takes in an x and y value and converts them to polar coordinates. - * - * @param x this is the x value of the Point being converted. - * @param y this is the y value of the Point being converted. - * @return this returns the r and theta values, in that order, in an Array of doubles. - */ - public static double[] cartesianToPolar(double x, double y) { - if (x == 0) { - if (y > 0) { - return new double[]{Math.abs(y), Math.PI / 2}; - } else { - return new double[]{Math.abs(y), (3 * Math.PI) / 2}; - } - } - double r = Math.sqrt(x * x + y * y); - if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)}; - if (y > 0) { - return new double[]{r, Math.atan(y / x)}; - } else { - return new double[]{r, (2 * Math.PI) + Math.atan(y / x)}; - } - } - - /** - * Returns the r value of this Point. This is a polar coordinate value. - * - * @return returns the r value. - */ - public double getR() { - return r; - } - - /** - * Returns the theta value of this Point. This is a polar coordinate value. - * - * @return returns the theta value. - */ - public double getTheta() { - return theta; - } - - /** - * Returns the x value of this Point. This is a Cartesian coordinate value. - * - * @return returns the x value. - */ - public double getX() { - return x; - } - - /** - * Returns the y value of this Point. This is a Cartesian coordinate value. - * - * @return returns the y value. - */ - public double getY() { - return y; - } - - /** - * This creates a new Point with the same information as this Point, just pointing to a different - * memory location. In other words, a deep copy. - * - * @return returns a copy of this Point. - */ - public Point copy() { - return new Point(getX(), getY(), CARTESIAN); - } - - @NonNull - @Override - public String toString() { - return "(" + getX() + ", " + getY() + ")"; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java deleted file mode 100644 index f1a093b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java +++ /dev/null @@ -1,141 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; - -/** - * This is the Point class. This class handles storing information about vectors, which are - * basically Points but using polar coordinates as the default. The main reason this class exists - * is because some vector math needs to be done in the Follower, and dot products and cross - * products of Points just don't seem right. Also, there are a few more methods in here that make - * using Vectors a little easier than using a Point in polar coordinates. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/11/2024 - */ -public class Vector { - - // IMPORTANT NOTE: theta is defined in radians. - // These are the values of the coordinate defined by this Point, in both polar and - // Cartesian systems. - private double magnitude; - private double theta; - private double xComponent; - private double yComponent; - - /** - * This creates a new Vector with zero magnitude and direction. - */ - public Vector() { - setComponents(0, 0); - } - - /** - * This creates a new Vector with a specified magnitude and direction. - * - * @param magnitude magnitude of the Vector. - * @param theta the direction of the Vector in radians. - */ - public Vector(double magnitude, double theta) { - setComponents(magnitude, theta); - } - - /** - * This sets the components of the Vector in regular vector coordinates. - * - * @param magnitude sets the magnitude of this Vector. - * @param theta sets the theta value of this Vector. - */ - public void setComponents(double magnitude, double theta) { - double[] orthogonalComponents; - if (magnitude<0) { - this.magnitude = -magnitude; - this.theta = MathFunctions.normalizeAngle(theta+Math.PI); - } else { - this.magnitude = magnitude; - this.theta = MathFunctions.normalizeAngle(theta); - } - orthogonalComponents = Point.polarToCartesian(magnitude, theta); - xComponent = orthogonalComponents[0]; - yComponent = orthogonalComponents[1]; - } - - /** - * This sets only the magnitude of the Vector. - * - * @param magnitude sets the magnitude of this Vector. - */ - public void setMagnitude(double magnitude) { - setComponents(magnitude, theta); - } - - /** - * This sets only the angle, theta, of the Vector. - * - * @param theta sets the angle, or theta value, of this Vector. - */ - public void setTheta(double theta) { - setComponents(magnitude, theta); - } - - /** - * This rotates the Vector by an angle, theta. - * - * @param theta2 the angle to be added. - */ - public void rotateVector(double theta2) { - setTheta(theta+theta2); - } - - /** - * This sets the orthogonal components of the Vector. These orthogonal components are assumed - * to be in the direction of the x-axis and y-axis. In other words, this is setting the - * coordinates of the Vector using x and y coordinates instead of a direction and magnitude. - * - * @param xComponent sets the x component of this Vector. - * @param yComponent sets the y component of this Vector. - */ - public void setOrthogonalComponents(double xComponent, double yComponent) { - double[] polarComponents; - this.xComponent = xComponent; - this.yComponent = yComponent; - polarComponents = Point.cartesianToPolar(xComponent, yComponent); - magnitude = polarComponents[0]; - theta = polarComponents[1]; - } - - /** - * Returns the magnitude of this Vector. - * - * @return returns the magnitude. - */ - public double getMagnitude() { - return magnitude; - } - - /** - * Returns the theta value, or angle, of this Vector. - * - * @return returns the theta value. - */ - public double getTheta() { - return theta; - } - - /** - * Returns the x component of this Vector. - * - * @return returns the x component. - */ - public double getXComponent() { - return xComponent; - } - - /** - * Returns the y component of this Vector. - * - * @return returns the y component. - */ - public double getYComponent() { - return yComponent; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardVelocityTuner.java similarity index 79% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardVelocityTuner.java index cb64063..00bd67a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardVelocityTuner.java @@ -1,17 +1,20 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorName; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorName; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.pathgen.MathFunctions; +import com.pedropathing.pathgen.Vector; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -19,16 +22,15 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; import java.util.ArrayList; import java.util.Arrays; import java.util.List; /** - * This is the ForwardVelocityTuner autonomous tuning OpMode. This runs the robot forwards at max + * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max * power until it reaches some specified distance. It records the most recent velocities, and on * reaching the end of the distance, it averages them and prints out the velocity obtained. It is * recommended to run this multiple times on a full battery to get the best results. What this does @@ -43,9 +45,9 @@ * @version 1.0, 3/13/2024 */ @Config -@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Velocity Tuner", group = "Automatic Tuners") public class ForwardVelocityTuner extends OpMode { - private ArrayList velocities = new ArrayList<>(); + private final ArrayList velocities = new ArrayList<>(); private DcMotorEx leftFront; private DcMotorEx leftRear; @@ -68,7 +70,7 @@ public class ForwardVelocityTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); @@ -100,6 +102,7 @@ public void init() { telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.addData("pose", poseUpdater.getPose()); telemetryA.update(); } @@ -157,7 +160,7 @@ public void loop() { for (Double velocity : velocities) { average += velocity; } - average /= (double) velocities.size(); + average /= velocities.size(); telemetryA.addData("forward velocity:", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardZeroPowerAccelerationTuner.java similarity index 80% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardZeroPowerAccelerationTuner.java index e774504..5a29a12 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardZeroPowerAccelerationTuner.java @@ -1,17 +1,21 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorName; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorName; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.pathgen.MathFunctions; +import com.pedropathing.pathgen.Vector; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -19,16 +23,15 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; import java.util.ArrayList; import java.util.Arrays; import java.util.List; /** - * This is the ForwardZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting * them to zero power. The deceleration, or negative acceleration, is then measured until the robot * stops. The accelerations across the entire time the robot is slowing down is then averaged and @@ -43,9 +46,9 @@ * @version 1.0, 3/13/2024 */ @Config -@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Automatic Tuners") public class ForwardZeroPowerAccelerationTuner extends OpMode { - private ArrayList accelerations = new ArrayList<>(); + private final ArrayList accelerations = new ArrayList<>(); private DcMotorEx leftFront; private DcMotorEx leftRear; @@ -71,7 +74,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); @@ -156,7 +159,7 @@ public void loop() { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double) accelerations.size(); + average /= accelerations.size(); telemetryA.addData("forward zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/LateralZeroPowerAccelerationTuner.java similarity index 80% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/LateralZeroPowerAccelerationTuner.java index db4e903..c1e3cb2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/LateralZeroPowerAccelerationTuner.java @@ -1,17 +1,21 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorName; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorName; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.pathgen.MathFunctions; +import com.pedropathing.pathgen.Vector; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -19,16 +23,15 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; import java.util.ArrayList; import java.util.Arrays; import java.util.List; /** - * This is the LateralZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting * them to zero power. The deceleration, or negative acceleration, is then measured until the robot * stops. The accelerations across the entire time the robot is slowing down is then averaged and @@ -43,9 +46,9 @@ * @version 1.0, 3/13/2024 */ @Config -@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Automatic Tuners") public class LateralZeroPowerAccelerationTuner extends OpMode { - private ArrayList accelerations = new ArrayList<>(); + private final ArrayList accelerations = new ArrayList<>(); private DcMotorEx leftFront; private DcMotorEx leftRear; @@ -71,7 +74,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); @@ -156,7 +159,7 @@ public void loop() { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double) accelerations.size(); + average /= accelerations.size(); telemetryA.addData("lateral zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/StrafeVelocityTuner.java similarity index 79% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/StrafeVelocityTuner.java index 8d51c08..61d20e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/StrafeVelocityTuner.java @@ -1,17 +1,21 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; + +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorName; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorName; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.pathgen.MathFunctions; +import com.pedropathing.pathgen.Vector; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -19,16 +23,15 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; import java.util.ArrayList; import java.util.Arrays; import java.util.List; /** - * This is the StrafeVelocityTuner autonomous tuning OpMode. This runs the robot right at max + * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot right at max * power until it reaches some specified distance. It records the most recent velocities, and on * reaching the end of the distance, it averages them and prints out the velocity obtained. It is * recommended to run this multiple times on a full battery to get the best results. What this does @@ -43,9 +46,9 @@ * @version 1.0, 3/13/2024 */ @Config -@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Strafe Velocity Tuner", group = "Automatic Tuners") public class StrafeVelocityTuner extends OpMode { - private ArrayList velocities = new ArrayList<>(); + private final ArrayList velocities = new ArrayList<>(); private DcMotorEx leftFront; private DcMotorEx leftRear; @@ -68,7 +71,7 @@ public class StrafeVelocityTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); @@ -155,7 +158,7 @@ public void loop() { for (Double velocity : velocities) { average += velocity; } - average /= (double) velocities.size(); + average /= velocities.size(); telemetryA.addData("strafe velocity:", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/ForwardTuner.java similarity index 83% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/ForwardTuner.java index d876746..cac5cf8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/ForwardTuner.java @@ -1,15 +1,17 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.util.DashboardPoseTracker; +import com.pedropathing.util.Drawing; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; -import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; /** * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the @@ -25,7 +27,7 @@ * @version 1.0, 5/6/2024 */ @Config -@Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Localizer Tuner", group = ".Localization") public class ForwardTuner extends OpMode { private PoseUpdater poseUpdater; private DashboardPoseTracker dashboardPoseTracker; @@ -39,7 +41,7 @@ public class ForwardTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LateralTuner.java similarity index 83% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LateralTuner.java index dfa39dc..aba8865 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LateralTuner.java @@ -1,16 +1,17 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.util.DashboardPoseTracker; +import com.pedropathing.util.Drawing; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; -import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; /** * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the @@ -26,7 +27,7 @@ * @version 1.0, 5/6/2024 */ @Config -@Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Lateral Localizer Tuner", group = ".Localization") public class LateralTuner extends OpMode { private PoseUpdater poseUpdater; private DashboardPoseTracker dashboardPoseTracker; @@ -40,7 +41,7 @@ public class LateralTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LocalizationTest.java similarity index 78% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LocalizationTest.java index 8da3c3e..332fa82 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LocalizationTest.java @@ -1,17 +1,20 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.leftRearMotorName; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection; +import static com.pedropathing.follower.FollowerConstants.rightRearMotorName; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.util.DashboardPoseTracker; +import com.pedropathing.util.Drawing; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -19,9 +22,8 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; -import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; import java.util.Arrays; import java.util.List; @@ -35,7 +37,7 @@ * @version 1.0, 5/6/2024 */ @Config -@TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test") +@TeleOp(group = "Teleop Test", name = "Localization Test") public class LocalizationTest extends OpMode { private PoseUpdater poseUpdater; private DashboardPoseTracker dashboardPoseTracker; @@ -52,7 +54,7 @@ public class LocalizationTest extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/SensorGoBildaPinpointExample.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/SensorGoBildaPinpointExample.java index 9fbac95..56b315b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/SensorGoBildaPinpointExample.java @@ -20,16 +20,15 @@ * SOFTWARE. */ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.pedropathing.localization.GoBildaPinpointDriver; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; import java.util.Locale; @@ -59,9 +58,8 @@ -Ethan Doak */ -//TODO: If tuning comment out the @Disabled -@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode") -@Disabled + +@TeleOp(name = "goBILDA® PinPoint Odometry Example", group = "Teleop Test") public class SensorGoBildaPinpointExample extends LinearOpMode { @@ -76,7 +74,7 @@ public void runOpMode() { // Initialize the hardware variables. Note that the strings used here must correspond // to the names assigned during the robot configuration step on the DS or RC devices. - odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); + odo = hardwareMap.get(GoBildaPinpointDriver.class, "odo"); /* Set the odometry pod positions relative to the point that the odometry computer tracks around. @@ -145,11 +143,11 @@ pull any other data. Only the heading (which you can pull with getHeading() or i //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); - if (gamepad1.a){ + if (gamepad1.a) { odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU } - if (gamepad1.b){ + if (gamepad1.b) { odo.recalibrateIMU(); //recalibrates the IMU without resetting position } @@ -160,8 +158,8 @@ of time each cycle takes and finds the frequency (number of updates per second) that cycle time. */ double newTime = getRuntime(); - double loopTime = newTime-oldTime; - double frequency = 1/loopTime; + double loopTime = newTime - oldTime; + double frequency = 1 / loopTime; oldTime = newTime; @@ -176,7 +174,7 @@ gets the current Position (x & y in mm, and heading in degrees) of the robot, an gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it. */ Pose2D vel = odo.getVelocity(); - String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES)); + String velocity = String.format(Locale.US, "{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES)); telemetry.addData("Velocity", velocity); @@ -197,4 +195,5 @@ gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and print telemetry.update(); } - }} \ No newline at end of file + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/TurnTuner.java similarity index 83% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/TurnTuner.java index 1693990..8afab57 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/TurnTuner.java @@ -1,16 +1,17 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.pedropathing.localization.PoseUpdater; +import com.pedropathing.util.DashboardPoseTracker; +import com.pedropathing.util.Drawing; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; -import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; -import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; /** * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the @@ -26,7 +27,7 @@ * @version 1.0, 5/6/2024 */ @Config -@Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Turn Localizer Tuner", group = ".Localization") public class TurnTuner extends OpMode { private PoseUpdater poseUpdater; private DashboardPoseTracker dashboardPoseTracker; @@ -40,7 +41,7 @@ public class TurnTuner extends OpMode { */ @Override public void init() { - poseUpdater = new PoseUpdater(hardwareMap); + poseUpdater = new PoseUpdater(hardwareMap, FConstants.class, LConstants.class); dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/CurvedBackAndForth.java similarity index 70% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/CurvedBackAndForth.java index 6ff548f..88e4746 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/CurvedBackAndForth.java @@ -1,16 +1,18 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.pid; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.pathgen.BezierCurve; +import com.pedropathing.pathgen.Path; +import com.pedropathing.pathgen.Point; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; /** * This is the CurvedBackAndForth autonomous OpMode. It runs the robot in a specified distance @@ -26,7 +28,7 @@ * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Curved Back And Forth", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Curved Back And Forth", group = "PIDF Testing") public class CurvedBackAndForth extends OpMode { private Telemetry telemetryA; @@ -45,10 +47,10 @@ public class CurvedBackAndForth extends OpMode { */ @Override public void init() { - follower = new Follower(hardwareMap); + follower = new Follower(hardwareMap, FConstants.class, LConstants.class); - forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN))); - backwards = new Path(new BezierCurve(new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + forwards = new Path(new BezierCurve(new Point(0, 0, Point.CARTESIAN), new Point(Math.abs(DISTANCE), 0, Point.CARTESIAN), new Point(Math.abs(DISTANCE), DISTANCE, Point.CARTESIAN))); + backwards = new Path(new BezierCurve(new Point(Math.abs(DISTANCE), DISTANCE, Point.CARTESIAN), new Point(Math.abs(DISTANCE), 0, Point.CARTESIAN), new Point(0, 0, Point.CARTESIAN))); backwards.setReversed(true); @@ -56,9 +58,9 @@ public void init() { telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("This will run the robot in a curve going " + DISTANCE + " inches" - + " to the left and the same number of inches forward. The robot will go" - + "forward and backward continuously along the path. Make sure you have" - + "enough room."); + + " to the left and the same number of inches forward. The robot will go" + + "forward and backward continuously along the path. Make sure you have" + + "enough room."); telemetryA.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/StraightBackAndForth.java similarity index 72% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/StraightBackAndForth.java index 6224a92..b605d93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/StraightBackAndForth.java @@ -1,16 +1,18 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.pid; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.pathgen.BezierLine; +import com.pedropathing.pathgen.Path; +import com.pedropathing.pathgen.Point; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; /** * This is the StraightBackAndForth autonomous OpMode. It runs the robot in a specified distance @@ -26,7 +28,7 @@ * @version 1.0, 3/12/2024 */ @Config -@Autonomous (name = "Straight Back And Forth", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Straight Back And Forth", group = "PIDF Tuning") public class StraightBackAndForth extends OpMode { private Telemetry telemetryA; @@ -45,19 +47,19 @@ public class StraightBackAndForth extends OpMode { */ @Override public void init() { - follower = new Follower(hardwareMap); + follower = new Follower(hardwareMap, FConstants.class, LConstants.class); - forwards = new Path(new BezierLine(new Point(0,0, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN))); + forwards = new Path(new BezierLine(new Point(0, 0, Point.CARTESIAN), new Point(DISTANCE, 0, Point.CARTESIAN))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + backwards = new Path(new BezierLine(new Point(DISTANCE, 0, Point.CARTESIAN), new Point(0, 0, Point.CARTESIAN))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("This will run the robot in a straight line going " + DISTANCE - + " inches forward. The robot will go forward and backward continuously" - + " along the path. Make sure you have enough room."); + + " inches forward. The robot will go forward and backward continuously" + + " along the path. Make sure you have enough room."); telemetryA.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java deleted file mode 100644 index e3f25b4..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ /dev/null @@ -1,217 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.tuning; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; -import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; - -/** - * This is the FollowerConstants class. It holds many constants and parameters for various parts of - * the Follower. This is here to allow for easier tuning of Pedro Pathing, as well as concentrate - * everything tunable for the Paths themselves in one place. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/4/2024 - */ -@Config -public class FollowerConstants { - - // This section is for configuring your motors - public static String leftFrontMotorName = "leftFront"; - public static String leftRearMotorName = "leftRear"; - public static String rightFrontMotorName = "rightFront"; - public static String rightRearMotorName = "rightRear"; - - public static DcMotorSimple.Direction leftFrontMotorDirection = DcMotorSimple.Direction.REVERSE; - public static DcMotorSimple.Direction rightFrontMotorDirection = DcMotorSimple.Direction.REVERSE; - public static DcMotorSimple.Direction leftRearMotorDirection = DcMotorSimple.Direction.FORWARD; - public static DcMotorSimple.Direction rightRearMotorDirection = DcMotorSimple.Direction.FORWARD; - - // This section is for setting the actual drive vector for the front left wheel, if the robot - // is facing a heading of 0 radians with the wheel centered at (0,0) - private static double xMovement = 81.34056; - private static double yMovement = 65.43028; - private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); - public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1])); - - - // Translational PIDF coefficients (don't use integral) - public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients( - 0.1, - 0, - 0, - 0); - - // Translational Integral - public static CustomPIDFCoefficients translationalIntegral = new CustomPIDFCoefficients( - 0, - 0, - 0, - 0); - - // Feed forward constant added on to the translational PIDF - public static double translationalPIDFFeedForward = 0.015; - - - // Heading error PIDF coefficients - public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( - 1, - 0, - 0, - 0); - - // Feed forward constant added on to the heading PIDF - public static double headingPIDFFeedForward = 0.01; - - - // Drive PIDF coefficients - public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients( - 0.025, - 0, - 0.00001, - 0.6, - 0); - - // Feed forward constant added on to the drive PIDF - public static double drivePIDFFeedForward = 0.01; - - // Kalman filter parameters for the drive error Kalman filter - public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( - 6, - 1); - - - // Mass of robot in kilograms - public static double mass = 10.65942; - - // Centripetal force to power scaling - public static double centripetalScaling = 0.0005; - - - // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) - // if not negative, then the robot thinks that its going to go faster under 0 power - public static double forwardZeroPowerAcceleration = -34.62719; - - // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) - // if not negative, then the robot thinks that its going to go faster under 0 power - public static double lateralZeroPowerAcceleration = -78.15554; - - // A multiplier for the zero power acceleration to change the speed the robot decelerates at - // the end of paths. - // Increasing this will cause the robot to try to decelerate faster, at the risk of overshoots - // or localization slippage. - // Decreasing this will cause the deceleration at the end of the Path to be slower, making the - // robot slower but reducing risk of end-of-path overshoots or localization slippage. - // This can be set individually for each Path, but this is the default. - public static double zeroPowerAccelerationMultiplier = 4; - - - // When the robot is at the end of its current Path or PathChain and the velocity goes below - // this value, then end the Path. This is in inches/second. - // This can be custom set for each Path. - public static double pathEndVelocityConstraint = 0.1; - - // When the robot is at the end of its current Path or PathChain and the translational error - // goes below this value, then end the Path. This is in inches. - // This can be custom set for each Path. - public static double pathEndTranslationalConstraint = 0.1; - - // When the robot is at the end of its current Path or PathChain and the heading error goes - // below this value, then end the Path. This is in radians. - // This can be custom set for each Path. - public static double pathEndHeadingConstraint = 0.007; - - // When the t-value of the closest point to the robot on the Path is greater than this value, - // then the Path is considered at its end. - // This can be custom set for each Path. - public static double pathEndTValueConstraint = 0.995; - - // When the Path is considered at its end parametrically, then the Follower has this many - // milliseconds to further correct by default. - // This can be custom set for each Path. - public static double pathEndTimeoutConstraint = 500; - - // This is how many steps the BezierCurve class uses to approximate the length of a BezierCurve. - public static int APPROXIMATION_STEPS = 1000; - - // This is scales the translational error correction power when the Follower is holding a Point. - public static double holdPointTranslationalScaling = 0.45; - - // This is scales the heading error correction power when the Follower is holding a Point. - public static double holdPointHeadingScaling = 0.35; - - // This is the number of times the velocity is recorded for averaging when approximating a first - // and second derivative for on the fly centripetal correction. The velocity is calculated using - // half of this number of samples, and the acceleration uses all of this number of samples. - public static int AVERAGED_VELOCITY_SAMPLE_NUMBER = 8; - - // This is the number of steps the binary search for closest point uses. More steps is more - // accuracy, and this increases at an exponential rate. However, more steps also does take more - // time. - public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10; - - - // These activate / deactivate the secondary PIDs. These take over at errors under a set limit for - // the translational, heading, and drive PIDs. - public static boolean useSecondaryTranslationalPID = false; - public static boolean useSecondaryHeadingPID = false; - public static boolean useSecondaryDrivePID = false; - - - // the limit at which the translational PIDF switches between the main and secondary translational PIDFs, - // if the secondary PID is active - public static double translationalPIDFSwitch = 3; - - // Secondary translational PIDF coefficients (don't use integral) - public static CustomPIDFCoefficients secondaryTranslationalPIDFCoefficients = new CustomPIDFCoefficients( - 0.3, - 0, - 0.01, - 0); - - // Secondary translational Integral value - public static CustomPIDFCoefficients secondaryTranslationalIntegral = new CustomPIDFCoefficients( - 0, - 0, - 0, - 0); - - // Feed forward constant added on to the small translational PIDF - public static double secondaryTranslationalPIDFFeedForward = 0.015; - - - // the limit at which the heading PIDF switches between the main and secondary heading PIDFs - public static double headingPIDFSwitch = Math.PI / 20; - - // Secondary heading error PIDF coefficients - public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( - 5, - 0, - 0.08, - 0); - - // Feed forward constant added on to the secondary heading PIDF - public static double secondaryHeadingPIDFFeedForward = 0.01; - - - // the limit at which the heading PIDF switches between the main and secondary drive PIDFs - public static double drivePIDFSwitch = 20; - - // Secondary drive PIDF coefficients - public static CustomFilteredPIDFCoefficients secondaryDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( - 0.02, - 0, - 0.000005, - 0.6, - 0); - - // Feed forward constant added on to the secondary drive PIDF - public static double secondaryDrivePIDFFeedForward = 0.01; -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java deleted file mode 100644 index 0b02174..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java +++ /dev/null @@ -1,70 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -import kotlin.jvm.JvmField; - -/** - * This is the CustomFilteredPIDFCoefficients class. This class handles holding coefficients for filtered PIDF - * controllers. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/15/2024 - */ -public class CustomFilteredPIDFCoefficients { - @JvmField public double P; - @JvmField public double I; - @JvmField public double D; - @JvmField public double T; - @JvmField public double F; - - public FeedForwardConstant feedForwardConstantEquation; - - private boolean usingEquation; - - /** - * This creates a new CustomFilteredPIDFCoefficients with constant coefficients. - * - * @param p the coefficient for the proportional factor. - * @param i the coefficient for the integral factor. - * @param d the coefficient for the derivative factor. - * @param t the time constant for the filter - * @param f the coefficient for the feedforward factor. - */ - public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, double f) { - P = p; - I = i; - D = d; - T = t; - F = f; - } - - /** - * This creates a new CustomFilteredPIDFCoefficients with constant PID coefficients and a variable - * feedforward equation using a FeedForwardConstant. - * - * @param p the coefficient for the proportional factor. - * @param i the coefficient for the integral factor. - * @param d the coefficient for the derivative factor. - * @param t the time constant for the filter - * @param f the equation for the feedforward factor. - */ - public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, FeedForwardConstant f) { - usingEquation = true; - P = p; - I = i; - D = d; - T = t; - feedForwardConstantEquation = f; - } - - /** - * This returns the coefficient for the feedforward factor. - * - * @param input this is inputted into the feedforward equation, if applicable. If there's no - * equation, then any input can be used. - * @return This returns the coefficient for the feedforward factor. - */ - public double getCoefficient(double input) { - if (!usingEquation) return F; - return feedForwardConstantEquation.getConstant(input); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java deleted file mode 100644 index 65a783a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -import kotlin.jvm.JvmField; - -/** - * This is the CustomPIDFCoefficients class. This class handles holding coefficients for PIDF - * controllers. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/5/2024 - */ -public class CustomPIDFCoefficients { - @JvmField public double P; - @JvmField public double I; - @JvmField public double D; - @JvmField public double F; - - public FeedForwardConstant feedForwardConstantEquation; - - private boolean usingEquation; - - /** - * This creates a new CustomPIDFCoefficients with constant coefficients. - * - * @param p the coefficient for the proportional factor. - * @param i the coefficient for the integral factor. - * @param d the coefficient for the derivative factor. - * @param f the coefficient for the feedforward factor. - */ - public CustomPIDFCoefficients(double p, double i, double d, double f) { - P = p; - I = i; - D = d; - F = f; - } - - /** - * This creates a new CustomPIDFCoefficients with constant PID coefficients and a variable - * feedforward equation using a FeedForwardConstant. - * - * @param p the coefficient for the proportional factor. - * @param i the coefficient for the integral factor. - * @param d the coefficient for the derivative factor. - * @param f the equation for the feedforward factor. - */ - public CustomPIDFCoefficients(double p, double i, double d, FeedForwardConstant f) { - usingEquation = true; - P = p; - I = i; - D = d; - feedForwardConstantEquation = f; - } - - /** - * This returns the coefficient for the feedforward factor. - * - * @param input this is inputted into the feedforward equation, if applicable. If there's no - * equation, then any input can be used. - * @return This returns the coefficient for the feedforward factor. - */ - public double getCoefficient(double input) { - if (!usingEquation) return F; - return feedForwardConstantEquation.getConstant(input); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java deleted file mode 100644 index 3c54156..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java +++ /dev/null @@ -1,76 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; - -import java.util.ArrayList; - -/** - * This is the DashboardPoseTracker class. This tracks the pose history of the robot through a - * PoseUpdater, adding to the pose history at specified increments of time and storing the history - * for a specified length of time. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 5/13/2024 - */ -public class DashboardPoseTracker { - private double[] xPositions; - private double[] yPositions; - private PoseUpdater poseUpdater; - private long lastUpdateTime; - private final int TRACKING_LENGTH = 1500; - private final long UPDATE_TIME = 50; - private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; - - /** - * This creates a new DashboardPoseTracker from a PoseUpdater. - * - * @param poseUpdater the PoseUpdater - */ - public DashboardPoseTracker(PoseUpdater poseUpdater) { - this.poseUpdater = poseUpdater; - xPositions = new double[TRACKING_SIZE]; - yPositions = new double[TRACKING_SIZE]; - - for (int i = 0; i < TRACKING_SIZE; i++) { - xPositions[i] = poseUpdater.getPose().getX(); - yPositions[i] = poseUpdater.getPose().getY(); - } - - lastUpdateTime = System.currentTimeMillis() - UPDATE_TIME; - } - - /** - * This updates the DashboardPoseTracker. When the specified update time has passed from the last - * pose history log, another pose can be logged. The least recent log is also removed. - */ - public void update() { - if (System.currentTimeMillis() - lastUpdateTime > UPDATE_TIME) { - lastUpdateTime = System.currentTimeMillis(); - for (int i = TRACKING_SIZE - 1; i > 0; i--) { - xPositions[i] = xPositions[i - 1]; - yPositions[i] = yPositions[i - 1]; - } - xPositions[0] = poseUpdater.getPose().getX(); - yPositions[0] = poseUpdater.getPose().getY(); - } - } - - /** - * This returns the x positions of the pose history as an Array of doubles. - * - * @return returns the x positions of the pose history - */ - public double[] getXPositionsArray() { - return xPositions; - } - - /** - * This returns the y positions of the pose history as an Array of doubles. - * - * @return returns the y positions of the pose history - */ - public double[] getYPositionsArray() { - return yPositions; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java deleted file mode 100644 index 9af9662..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ /dev/null @@ -1,155 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.canvas.Canvas; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; - -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot. - * - * @author Logan Nash - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/22/2024 - */ -public class Drawing { - public static final double ROBOT_RADIUS = 9; - - private static TelemetryPacket packet; - - /** - * This draws everything that will be used in the Follower's telemetryDebug() method. This takes - * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. - * - * @param follower - */ - public static void drawDebug(Follower follower) { - if (follower.getCurrentPath() != null) { - drawPath(follower.getCurrentPath(), "#3F51B5"); - Point closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); - drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5"); - } - drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); - drawRobot(follower.getPose(), "#4CAF50"); - - sendPacket(); - } - - /** - * This adds instructions to the current packet to draw a robot at a specified Pose with a specified - * color. If no packet exists, then a new one is created. - * - * @param pose the Pose to draw the robot at - * @param color the color to draw the robot with - */ - public static void drawRobot(Pose pose, String color) { - if (packet == null) packet = new TelemetryPacket(); - - packet.fieldOverlay().setStroke(color); - Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy()); - } - - /** - * This adds instructions to the current packet to draw a Path with a specified color. If no - * packet exists, then a new one is created. - * - * @param path the Path to draw - * @param color the color to draw the Path with - */ - public static void drawPath(Path path, String color) { - if (packet == null) packet = new TelemetryPacket(); - - packet.fieldOverlay().setStroke(color); - Drawing.drawPath(packet.fieldOverlay(), path.getDashboardDrawingPoints()); - } - - /** - * This adds instructions to the current packet to draw all the Paths in a PathChain with a - * specified color. If no packet exists, then a new one is created. - * - * @param pathChain the PathChain to draw - * @param color the color to draw the PathChain with - */ - public static void drawPath(PathChain pathChain, String color) { - for (int i = 0; i < pathChain.size(); i++) { - drawPath(pathChain.getPath(i), color); - } - } - - /** - * This adds instructions to the current packet to draw the pose history of the robot. If no - * packet exists, then a new one is created. - * - * @param poseTracker the DashboardPoseTracker to get the pose history from - * @param color the color to draw the pose history with - */ - public static void drawPoseHistory(DashboardPoseTracker poseTracker, String color) { - if (packet == null) packet = new TelemetryPacket(); - - packet.fieldOverlay().setStroke(color); - packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray()); - } - - /** - * This tries to send the current packet to FTC Dashboard. - * - * @return returns if the operation was successful. - */ - public static boolean sendPacket() { - if (packet != null) { - FtcDashboard.getInstance().sendTelemetryPacket(packet); - packet = null; - return true; - } - return false; - } - - /** - * This draws a robot on the Dashboard at a specified Point. - * - * @param c the Canvas on the Dashboard on which this will draw - * @param t the Point to draw at - */ - public static void drawRobotOnCanvas(Canvas c, Point t) { - c.setStrokeWidth(1); - c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); - - Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta()); - Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); - Vector p2 = MathFunctions.addVectors(p1, halfv); - c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); - } - - /** - * This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the - * actual robot, since the Pose contains the direction the robot is facing as well as its position. - * - * @param c the Canvas on the Dashboard on which this will draw - * @param t the Pose to draw at - */ - public static void drawRobotOnCanvas(Canvas c, Pose t) { - c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); - Vector v = t.getHeadingVector(); - v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); - double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; - double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); - c.strokeLine(x1, y1, x2, y2); - } - - /** - * This draws a Path on the Dashboard from a specified Array of Points. - * - * @param c the Canvas on the Dashboard on which this will draw - * @param points the Points to draw - */ - public static void drawPath(Canvas c, double[][] points) { - c.strokePolyline(points[0], points[1]); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java deleted file mode 100644 index 0085e7c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -/** - * This is the FeedForwardConstant interface. This interface holds a feedforward equation for PIDFs. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/5/2024 - */ -public interface FeedForwardConstant { - - /** - * This returns the coefficient for the feedforward factor. - * - * @param input this is inputted into the feedforward equation, if applicable. If there's no - * equation, then any input can be used. - * @return This returns the coefficient for the feedforward factor. - */ - double getConstant(double input); -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java deleted file mode 100644 index 07747fe..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java +++ /dev/null @@ -1,245 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -/** - * This is the FilteredPIDFController class. This class handles the running of filtered filtered PIDFs. This - * behaves very similarly to a regular filtered PIDF controller, but the derivative portion is filtered with - * a low pass filter to reduce high frequency noise that could affect results. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/15/2024 - */ -public class FilteredPIDFController { - private CustomFilteredPIDFCoefficients coefficients; - - private double previousError; - private double error; - private double position; - private double targetPosition; - private double errorIntegral; - private double errorDerivative; - private double previousDerivative; - private double filteredDerivative; - private double feedForwardInput; - - private long previousUpdateTimeNano; - private long deltaTimeNano; - - /** - * This creates a new filtered PIDFController from a CustomPIDFCoefficients. - * - * @param set the coefficients to use. - */ - public FilteredPIDFController(CustomFilteredPIDFCoefficients set) { - setCoefficients(set); - reset(); - } - - /** - * This takes the current error and runs the filtered PIDF on it. - * - * @return this returns the value of the filtered PIDF from the current error. - */ - public double runPIDF() { - return error * P() + filteredDerivative * D() + errorIntegral * I() + F(); - } - - /** - * This can be used to update the filtered PIDF's current position when inputting a current position and - * a target position to calculate error. This will update the error from the current position to - * the target position specified. - * - * @param update This is the current position. - */ - public void updatePosition(double update) { - position = update; - previousError = error; - error = targetPosition - position; - - deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; - previousUpdateTimeNano = System.nanoTime(); - - errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); - previousDerivative = filteredDerivative; - errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); - filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative; - } - - /** - * As opposed to updating position against a target position, this just sets the error to some - * specified value. - * - * @param error The error specified. - */ - public void updateError(double error) { - previousError = this.error; - this.error = error; - - deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; - previousUpdateTimeNano = System.nanoTime(); - - errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); - previousDerivative = errorDerivative; - errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); - filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative; - } - - /** - * This can be used to update the feedforward equation's input, if applicable. - * - * @param input the input into the feedforward equation. - */ - public void updateFeedForwardInput(double input) { - feedForwardInput = input; - } - - /** - * This resets all the filtered PIDF's error and position values, as well as the time stamps. - */ - public void reset() { - previousError = 0; - error = 0; - position = 0; - targetPosition = 0; - errorIntegral = 0; - errorDerivative = 0; - previousDerivative = 0; - filteredDerivative = 0; - previousUpdateTimeNano = System.nanoTime(); - } - - /** - * This is used to set the target position if the filtered PIDF is being run with current position and - * target position inputs rather than error inputs. - * - * @param set this sets the target position. - */ - public void setTargetPosition(double set) { - targetPosition = set; - } - - /** - * This returns the target position of the filtered PIDF. - * - * @return this returns the target position. - */ - public double getTargetPosition() { - return targetPosition; - } - - /** - * This is used to set the coefficients of the filtered PIDF. - * - * @param set the coefficients that the filtered PIDF will use. - */ - public void setCoefficients(CustomFilteredPIDFCoefficients set) { - coefficients = set; - } - - /** - * This returns the filtered PIDF's current coefficients. - * - * @return this returns the current coefficients. - */ - public CustomFilteredPIDFCoefficients getCoefficients() { - return coefficients; - } - - /** - * This sets the proportional (P) coefficient of the filtered PIDF only. - * - * @param set this sets the P coefficient. - */ - public void setP(double set) { - coefficients.P = set; - } - - /** - * This returns the proportional (P) coefficient of the filtered PIDF. - * - * @return this returns the P coefficient. - */ - public double P() { - return coefficients.P; - } - - /** - * This sets the integral (I) coefficient of the filtered PIDF only. - * - * @param set this sets the I coefficient. - */ - public void setI(double set) { - coefficients.I = set; - } - - /** - * This returns the integral (I) coefficient of the filtered PIDF. - * - * @return this returns the I coefficient. - */ - public double I() { - return coefficients.I; - } - - /** - * This sets the derivative (D) coefficient of the filtered PIDF only. - * - * @param set this sets the D coefficient. - */ - public void setD(double set) { - coefficients.D = set; - } - - /** - * This returns the derivative (D) coefficient of the filtered PIDF. - * - * @return this returns the D coefficient. - */ - public double D() { - return coefficients.D; - } - - /** - * This sets the time constant (T) of the filtered PIDF only. - * - * @param set this sets the time constant. - */ - public void setT(double set) { - coefficients.T = set; - } - - /** - * This returns the time constant (T) of the filtered PIDF. - * - * @return this returns the time constant. - */ - public double T() { - return coefficients.T; - } - - /** - * This sets the feedforward (F) constant of the filtered PIDF only. - * - * @param set this sets the F constant. - */ - public void setF(double set) { - coefficients.F = set; - } - - /** - * This returns the feedforward (F) constant of the filtered PIDF. - * - * @return this returns the F constant. - */ - public double F() { - return coefficients.getCoefficient(feedForwardInput); - } - - /** - * This returns the current error of the filtered PIDF. - * - * @return this returns the error. - */ - public double getError() { - return error; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java deleted file mode 100644 index 41aa630..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -import com.acmerobotics.dashboard.config.Config; - -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/** - * This is the KalmanFilter class. This creates a Kalman filter that is used to smooth out data. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/17/2024 - */ -public class KalmanFilter { - private KalmanFilterParameters parameters; - private double state; - private double variance; - private double kalmanGain; - private double previousState; - private double previousVariance; - - public KalmanFilter(KalmanFilterParameters parameters) { - this.parameters = parameters; - reset(); - } - - public KalmanFilter(KalmanFilterParameters parameters, double startState, double startVariance, double startGain) { - this.parameters = parameters; - reset(startState, startVariance, startGain); - } - - public void reset(double startState, double startVariance, double startGain) { - state = startState; - previousState = startState; - variance = startVariance; - previousVariance = startVariance; - kalmanGain = startGain; - } - - public void reset() { - reset(0, 1, 1); - } - - public void update(double updateData, double updateProjection) { - state = previousState + updateData; - variance = previousVariance + parameters.modelCovariance; - kalmanGain = variance / (variance + parameters.dataCovariance); - state += kalmanGain * (updateProjection - state); - variance *= (1.0 - kalmanGain); - previousState = state; - previousVariance = variance; - } - - public double getState() { - return state; - } - - public void debug(Telemetry telemetry) { - telemetry.addData("state", state); - telemetry.addData("variance", variance); - telemetry.addData("Kalman gain", kalmanGain); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java deleted file mode 100644 index e31260b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -import kotlin.jvm.JvmField; - -/** - * This is the KalmanFilterParameters class. This class handles holding parameters Kalman filters. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/17/2024 - */ -public class KalmanFilterParameters { - @JvmField public double modelCovariance; - @JvmField public double dataCovariance; - - /** - * This creates a new KalmanFilterParameters with a specified model and data covariance. - * - * @param modelCovariance the covariance of the model. - * @param dataCovariance the covariance of the data. - */ - public KalmanFilterParameters(double modelCovariance, double dataCovariance) { - this.modelCovariance = modelCovariance; - this.dataCovariance = dataCovariance; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java deleted file mode 100644 index 6501093..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -/** - * This is the NanoTimer class. It is an elapsed time clock with nanosecond precision, or at least - * as precise as the System.nanoTime() is. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/5/2024 - */ -public class NanoTimer { - private long startTime; - - /** - * This creates a new NanoTimer with the start time set to its creation time. - */ - public NanoTimer() { - resetTimer(); - } - - /** - * This resets the NanoTimer's start time to the current time using System.nanoTime(). - */ - public void resetTimer() { - startTime = System.nanoTime(); - } - - /** - * This returns the elapsed time in nanoseconds since the start time of the NanoTimer. - * - * @return this returns the elapsed time in nanoseconds. - */ - public long getElapsedTime() { - return System.nanoTime() - startTime; - } - - /** - * This returns the elapsed time in seconds since the start time of the NanoTimer. - * - * @return this returns the elapsed time in seconds. - */ - public double getElapsedTimeSeconds() { - return (getElapsedTime() / Math.pow(10.0,9)); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java deleted file mode 100644 index 0b59050..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java +++ /dev/null @@ -1,223 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -/** - * This is the PIDFController class. This class handles the running of PIDFs. PIDF stands for - * proportional, integral, derivative, and feedforward. PIDFs take the error of a system as an input. - * Coefficients multiply into the error, the integral of the error, the derivative of the error, and - * a feedforward value. Then, these values are added up and returned. In this way, error in the - * system is corrected. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/5/2024 - */ -public class PIDFController { - private CustomPIDFCoefficients coefficients; - - private double previousError; - private double error; - private double position; - private double targetPosition; - private double errorIntegral; - private double errorDerivative; - private double feedForwardInput; - - private long previousUpdateTimeNano; - private long deltaTimeNano; - - /** - * This creates a new PIDFController from a CustomPIDFCoefficients. - * - * @param set the coefficients to use. - */ - public PIDFController(CustomPIDFCoefficients set) { - setCoefficients(set); - reset(); - } - - /** - * This takes the current error and runs the PIDF on it. - * - * @return this returns the value of the PIDF from the current error. - */ - public double runPIDF() { - return error * P() + errorDerivative * D() + errorIntegral * I() + F(); - } - - /** - * This can be used to update the PIDF's current position when inputting a current position and - * a target position to calculate error. This will update the error from the current position to - * the target position specified. - * - * @param update This is the current position. - */ - public void updatePosition(double update) { - position = update; - previousError = error; - error = targetPosition - position; - - deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; - previousUpdateTimeNano = System.nanoTime(); - - errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); - errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); - } - - /** - * As opposed to updating position against a target position, this just sets the error to some - * specified value. - * - * @param error The error specified. - */ - public void updateError(double error) { - previousError = this.error; - this.error = error; - - deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; - previousUpdateTimeNano = System.nanoTime(); - - errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); - errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); - } - - /** - * This can be used to update the feedforward equation's input, if applicable. - * - * @param input the input into the feedforward equation. - */ - public void updateFeedForwardInput(double input) { - feedForwardInput = input; - } - - /** - * This resets all the PIDF's error and position values, as well as the time stamps. - */ - public void reset() { - previousError = 0; - error = 0; - position = 0; - targetPosition = 0; - errorIntegral = 0; - errorDerivative = 0; - previousUpdateTimeNano = System.nanoTime(); - } - - /** - * This is used to set the target position if the PIDF is being run with current position and - * target position inputs rather than error inputs. - * - * @param set this sets the target position. - */ - public void setTargetPosition(double set) { - targetPosition = set; - } - - /** - * This returns the target position of the PIDF. - * - * @return this returns the target position. - */ - public double getTargetPosition() { - return targetPosition; - } - - /** - * This is used to set the coefficients of the PIDF. - * - * @param set the coefficients that the PIDF will use. - */ - public void setCoefficients(CustomPIDFCoefficients set) { - coefficients = set; - } - - /** - * This returns the PIDF's current coefficients. - * - * @return this returns the current coefficients. - */ - public CustomPIDFCoefficients getCoefficients() { - return coefficients; - } - - /** - * This sets the proportional (P) coefficient of the PIDF only. - * - * @param set this sets the P coefficient. - */ - public void setP(double set) { - coefficients.P = set; - } - - /** - * This returns the proportional (P) coefficient of the PIDF. - * - * @return this returns the P coefficient. - */ - public double P() { - return coefficients.P; - } - - /** - * This sets the integral (I) coefficient of the PIDF only. - * - * @param set this sets the I coefficient. - */ - public void setI(double set) { - coefficients.I = set; - } - - /** - * This returns the integral (I) coefficient of the PIDF. - * - * @return this returns the I coefficient. - */ - public double I() { - return coefficients.I; - } - - /** - * This sets the derivative (D) coefficient of the PIDF only. - * - * @param set this sets the D coefficient. - */ - public void setD(double set) { - coefficients.D = set; - } - - /** - * This returns the derivative (D) coefficient of the PIDF. - * - * @return this returns the D coefficient. - */ - public double D() { - return coefficients.D; - } - - /** - * This sets the feedforward (F) constant of the PIDF only. - * - * @param set this sets the F constant. - */ - public void setF(double set) { - coefficients.F = set; - } - - /** - * This returns the feedforward (F) constant of the PIDF. - * - * @return this returns the F constant. - */ - public double F() { - return coefficients.getCoefficient(feedForwardInput); - } - - /** - * This returns the current error of the PIDF. - * - * @return this returns the error. - */ - public double getError() { - return error; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java deleted file mode 100644 index 48a5702..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java +++ /dev/null @@ -1,60 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -/** - * This is the SingleRunAction class. It handles running Runnables once, until a reset is called. - * It also forms the basis of the PathCallback class. Basically, if you want to run a certain action - * once despite looping through a section of code multiple times, then this is for you. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/6/2024 - */ -public class SingleRunAction { - private boolean hasBeenRun; - - private Runnable runnable; - - /** - * This creates a new SingleRunAction with a Runnable containing the code to be run for this action. - * The name is a slight bit misleading, as this can actually be run multiple times. However, the - * run() method only runs once before the reset() method needs to be called to allow the - * SingleRunAction to run again. - * - * @param runnable This is a Runnable containing the code to be run. Preferably, use lambda statements. - */ - public SingleRunAction(Runnable runnable) { - this.runnable = runnable; - } - - /** - * This returns if the SingleRunAction has been run yet. Running reset() will reset this. - * - * @return This returns if it has been run. - */ - public boolean hasBeenRun() { - return hasBeenRun; - } - - /** - * This runs the Runnable of the SingleRunAction. It will only run once before requiring a reset. - * - * @return This returns if the Runnable was successfully run. If not, then a reset is needed to run again. - */ - public boolean run() { - if (!hasBeenRun) { - hasBeenRun = true; - runnable.run(); - return true; - } - return false; - } - - /** - * This resets the SingleRunAction and makes it able to run again. The SingleRunAction is set - * to "has not been run", allowing for multiple uses of the Runnable. - */ - public void reset() { - hasBeenRun = false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java deleted file mode 100644 index a606043..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.util; - -/** - * This is the Timer class. It is an elapsed time clock with millisecond precision, or at least as - * precise as the System.currentTimeMillis() is. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/5/2024 - */ -public class Timer { - private long startTime; - - /** - * This creates a new Timer with the start time set to its creation time. - */ - public Timer() { - resetTimer(); - } - - /** - * This resets the Timer's start time to the current time using System.currentTimeMillis(). - */ - public void resetTimer() { - startTime = System.currentTimeMillis(); - } - - /** - * This returns the elapsed time in milliseconds since the start time of the Timer. - * - * @return this returns the elapsed time in milliseconds. - */ - public long getElapsedTime() { - return System.currentTimeMillis() - startTime; - } - - /** - * This returns the elapsed time in seconds since the start time of the Timer. - * - * @return this returns the elapsed time in seconds. - */ - public double getElapsedTimeSeconds() { - return (getElapsedTime() / 1000.0); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index 897aaf4..b0eef03 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -5,10 +5,12 @@ import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.command.SubsystemBase; import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.pedropathing.follower.Follower; +import com.pedropathing.localization.Pose; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants; @Config public class Drivetrain extends SubsystemBase { @@ -32,7 +34,7 @@ public class Drivetrain extends SubsystemBase { public Drivetrain(final HardwareMap hwMap, final MultipleTelemetry telemetry, Pose startPose) { this.telemetry = telemetry; - this.follower = new Follower(hwMap); + this.follower = new Follower(hwMap, FConstants.class, LConstants.class); this.follower.setStartingPose(startPose); } @@ -49,7 +51,6 @@ private static double calculateAccel(double accel, double deaccel, double prevPo public void startThread(final GamepadEx gamepad, CommandOpMode opMode) { new Thread(() -> { this.follower.startTeleopDrive(); - this.follower.brakeMode(); while (opMode.opModeIsActive()) try { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java index 175352c..faf9ab5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.utils; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import com.pedropathing.localization.Pose; +import com.pedropathing.pathgen.BezierCurve; +import com.pedropathing.pathgen.BezierLine; +import com.pedropathing.pathgen.Path; +import com.pedropathing.pathgen.Point; public class AutonomousHelpers { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java index f9fa870..3a765a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java @@ -1,10 +1,9 @@ package org.firstinspires.ftc.teamcode.utils.commands; import com.arcrobotics.ftclib.command.CommandBase; - -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import com.pedropathing.follower.Follower; +import com.pedropathing.pathgen.Path; +import com.pedropathing.pathgen.PathChain; public class PathChainCommand extends CommandBase { private final PathChain pathChain; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java index 2e82154..163369e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java @@ -1,9 +1,8 @@ package org.firstinspires.ftc.teamcode.utils.commands; import com.arcrobotics.ftclib.command.CommandBase; - -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import com.pedropathing.follower.Follower; +import com.pedropathing.pathgen.Path; public class PathCommand extends CommandBase { private final Path path; diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 52eb0ae..9ec0d49 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,7 @@ repositories { mavenCentral() maven { url = 'https://maven.brott.dev/' } + maven { url = 'https://maven.pedropathing.com/' } google() // Needed for androidx } @@ -14,6 +15,10 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:10.1.1' implementation 'org.firstinspires.ftc:Vision:10.1.1' implementation 'androidx.appcompat:appcompat:1.2.0' + + implementation 'com.acmerobotics.dashboard:dashboard:0.4.16' + + implementation 'com.pedropathing:pedro:1.0.1' implementation 'com.acmerobotics.dashboard:dashboard:0.4.16' } From eed8205bbbf84cdc39f54a5baa4a5ffa80b10145 Mon Sep 17 00:00:00 2001 From: Bi1ku <2008owenshi@gmail.com> Date: Mon, 30 Dec 2024 22:01:33 -0500 Subject: [PATCH 006/107] move opmodes out of lift directory --- .../{commands/lift => }/opmodes/auton/blue/CloseBasket.java | 2 +- .../{commands/lift => }/opmodes/auton/blue/FarBasket.java | 2 +- .../lift => }/opmodes/auton/positions/blue/close-basket.json | 0 .../lift => }/opmodes/auton/positions/blue/far-basket.json | 0 .../{commands/lift => }/opmodes/auton/positions/notes.txt | 0 .../lift => }/opmodes/auton/positions/red/close-basket.json | 0 .../lift => }/opmodes/auton/positions/red/far-basket.json | 0 .../{commands/lift => }/opmodes/auton/red/CloseBasket.java | 2 +- .../{commands/lift => }/opmodes/auton/red/FarBasket.java | 2 +- .../ftc/teamcode/{commands/lift => }/opmodes/teleop/Main.java | 2 +- .../teamcode/{commands/lift => }/opmodes/teleop/OwenMain.java | 2 +- .../teamcode/{commands/lift => }/opmodes/teleop/RyanMain.java | 2 +- .../ftc/teamcode/{commands/lift => }/opmodes/test/ClawTest.java | 2 +- .../{commands/lift => }/opmodes/test/ColorSensorTest.java | 2 +- .../{commands/lift => }/opmodes/test/DrivetrainTest.java | 2 +- .../teamcode/{commands/lift => }/opmodes/test/ExtendoTest.java | 2 +- .../ftc/teamcode/{commands/lift => }/opmodes/test/LiftTest.java | 2 +- .../{commands/lift => }/opmodes/test/LimelightTest.java | 2 +- .../{commands/lift => }/opmodes/test/ManualLiftTest.java | 2 +- .../teamcode/{commands/lift => }/opmodes/test/MotorTest.java | 2 +- .../teamcode/{commands/lift => }/opmodes/test/OdometryTest.java | 2 +- 21 files changed, 16 insertions(+), 16 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/blue/CloseBasket.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/blue/FarBasket.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/positions/blue/close-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/positions/blue/far-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/positions/notes.txt (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/positions/red/close-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/positions/red/far-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/red/CloseBasket.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/auton/red/FarBasket.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/teleop/Main.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/teleop/OwenMain.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/teleop/RyanMain.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/ClawTest.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/ColorSensorTest.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/DrivetrainTest.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/ExtendoTest.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/LiftTest.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/LimelightTest.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/ManualLiftTest.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/MotorTest.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{commands/lift => }/opmodes/test/OdometryTest.java (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/blue/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/blue/CloseBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java index 1250df6..035f11e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/blue/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.auton.blue; +package org.firstinspires.ftc.teamcode.opmodes.auton.blue; import com.arcrobotics.ftclib.command.CommandScheduler; import com.pedropathing.follower.Follower; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/blue/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/blue/FarBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java index 904811f..e20cdb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/blue/FarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.auton.blue; +package org.firstinspires.ftc.teamcode.opmodes.auton.blue; import com.arcrobotics.ftclib.command.CommandScheduler; import com.pedropathing.localization.Pose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/blue/close-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/close-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/blue/close-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/close-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/blue/far-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/far-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/blue/far-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/far-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/notes.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/notes.txt similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/notes.txt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/notes.txt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/red/close-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/close-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/red/close-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/close-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/red/far-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/far-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/positions/red/far-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/far-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/red/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/red/CloseBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java index 7abf2a3..0d42089 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/red/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.auton.red; +package org.firstinspires.ftc.teamcode.opmodes.auton.red; import com.arcrobotics.ftclib.command.CommandScheduler; import com.pedropathing.localization.Pose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/red/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/red/FarBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java index a582773..902dcd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/auton/red/FarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.auton.red; +package org.firstinspires.ftc.teamcode.opmodes.auton.red; import com.arcrobotics.ftclib.command.CommandScheduler; import com.pedropathing.localization.Pose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/Main.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java index aafc2da..cbd8d26 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/Main.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.teleop; +package org.firstinspires.ftc.teamcode.opmodes.teleop; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/OwenMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/OwenMain.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/OwenMain.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/OwenMain.java index fb618f2..510d5b6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/OwenMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/OwenMain.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.teleop; +package org.firstinspires.ftc.teamcode.opmodes.teleop; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/RyanMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/RyanMain.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/RyanMain.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/RyanMain.java index 24e007b..3452639 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/teleop/RyanMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/RyanMain.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.teleop; +package org.firstinspires.ftc.teamcode.opmodes.teleop; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ClawTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java index 01fb79a..02787ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.arcrobotics.ftclib.command.CommandScheduler; import com.arcrobotics.ftclib.gamepad.GamepadEx; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ColorSensorTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ColorSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ColorSensorTest.java index 9c7fe94..8c7f05c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ColorSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ColorSensorTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/DrivetrainTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/DrivetrainTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java index a177af1..3255b52 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/DrivetrainTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.arcrobotics.ftclib.command.CommandScheduler; import com.arcrobotics.ftclib.gamepad.GamepadEx; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ExtendoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ExtendoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java index 6e76cb9..4feab28 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ExtendoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.arcrobotics.ftclib.command.CommandScheduler; import com.arcrobotics.ftclib.gamepad.GamepadEx; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/LiftTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java index d0e4880..1e841e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.arcrobotics.ftclib.command.CommandScheduler; import com.arcrobotics.ftclib.gamepad.GamepadEx; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/LimelightTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java index d08b924..27769d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.arcrobotics.ftclib.command.CommandScheduler; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ManualLiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ManualLiftTest.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ManualLiftTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ManualLiftTest.java index 1a1e697..bb7b84f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/ManualLiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ManualLiftTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.command.CommandScheduler; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/MotorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/MotorTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/MotorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/MotorTest.java index 196f467..f949fd4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/MotorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/MotorTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.command.CommandScheduler; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/OdometryTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/OdometryTest.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/OdometryTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/OdometryTest.java index b98eb5a..3c3b2a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/opmodes/test/OdometryTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/OdometryTest.java @@ -20,7 +20,7 @@ * SOFTWARE. */ -package org.firstinspires.ftc.teamcode.commands.lift.opmodes.test; +package org.firstinspires.ftc.teamcode.opmodes.test; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; From 1676a9f888e21d2c08423b669878a9e636eec8aa Mon Sep 17 00:00:00 2001 From: Bi1ku <2008owenshi@gmail.com> Date: Mon, 30 Dec 2024 22:38:52 -0500 Subject: [PATCH 007/107] auto position stuff --- PathVisualizer/.gitignore | 1 - PathVisualizer/build.gradle | 17 --- .../com/example/meepmeep/PathVisualizer.java | 37 ------ .../trajectories/BlueCloseBasket.java | 115 ------------------ .../meepmeep/trajectories/BlueFarPush.java | 94 -------------- .../meepmeep/trajectories/RedCloseBasket.java | 115 ------------------ .../meepmeep/trajectories/RedFarPush.java | 94 -------------- TeamCode/build.gradle | 2 +- .../blue/trajectories/BlueCloseBasket.java | 80 ++++++++++++ .../blue}/trajectories/BlueFarBasket.java | 74 +++-------- .../auton/blue/trajectories/BlueFarPush.java | 67 ++++++++++ .../trajectories/positions}/close-basket.json | 0 .../trajectories/positions}/far-basket.json | 0 .../opmodes/auton/{positions => }/notes.txt | 0 .../red/trajectories/RedCloseBasket.java | 80 ++++++++++++ .../auton/red}/trajectories/RedFarBasket.java | 73 +++-------- .../auton/red/trajectories/RedFarPush.java | 67 ++++++++++ .../trajectories/positions}/close-basket.json | 0 .../trajectories/positions}/far-basket.json | 0 19 files changed, 335 insertions(+), 581 deletions(-) delete mode 100644 PathVisualizer/.gitignore delete mode 100644 PathVisualizer/build.gradle delete mode 100644 PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java delete mode 100644 PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java delete mode 100644 PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java delete mode 100644 PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java delete mode 100644 PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueCloseBasket.java rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue}/trajectories/BlueFarBasket.java (50%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarPush.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/{positions/blue => blue/trajectories/positions}/close-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/{positions/blue => blue/trajectories/positions}/far-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/{positions => }/notes.txt (100%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java rename {PathVisualizer/src/main/java/com/example/meepmeep => TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red}/trajectories/RedFarBasket.java (50%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarPush.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/{positions/red => red/trajectories/positions}/close-basket.json (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/{positions/red => red/trajectories/positions}/far-basket.json (100%) diff --git a/PathVisualizer/.gitignore b/PathVisualizer/.gitignore deleted file mode 100644 index 42afabf..0000000 --- a/PathVisualizer/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/build \ No newline at end of file diff --git a/PathVisualizer/build.gradle b/PathVisualizer/build.gradle deleted file mode 100644 index 1e3e68c..0000000 --- a/PathVisualizer/build.gradle +++ /dev/null @@ -1,17 +0,0 @@ -plugins { - id 'java-library' -} - -java { - sourceCompatibility = JavaVersion.VERSION_1_7 - targetCompatibility = JavaVersion.VERSION_1_7 -} - -repositories { - maven { url = 'https://maven.brott.dev/' } -} - -dependencies { - implementation 'com.acmerobotics.roadrunner:MeepMeep:0.1.6' - implementation 'org.json:json:20240303' -} \ No newline at end of file diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java b/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java deleted file mode 100644 index ef211d2..0000000 --- a/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java +++ /dev/null @@ -1,37 +0,0 @@ -package com.example.meepmeep; - -import com.example.meepmeep.trajectories.BlueCloseBasket; -import com.example.meepmeep.trajectories.BlueFarBasket; -import com.example.meepmeep.trajectories.BlueFarPush; -import com.example.meepmeep.trajectories.RedCloseBasket; -import com.example.meepmeep.trajectories.RedFarBasket; -import com.example.meepmeep.trajectories.RedFarPush; -import com.noahbres.meepmeep.MeepMeep; -import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder; -import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity; - -public class PathVisualizer { - public static void main(String[] args) { - MeepMeep meepMeep = new MeepMeep(700); - - RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) - .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) - .build(); - - - // BlueCloseBasket trajectories = new BlueCloseBasket(0); - // BlueFarBasket trajectories = new BlueFarBasket(0); - // BlueFarPush trajectories = new BlueFarPush(0); - // RedCloseBasket trajectories = new RedCloseBasket(0); - // RedFarBasket trajectories = new RedFarBasket(0); - RedFarPush trajectories = new RedFarPush(0); - - myBot.runAction(trajectories.start(myBot.getDrive().actionBuilder(trajectories.getStart()))); - - meepMeep.setBackground(MeepMeep.Background.FIELD_INTO_THE_DEEP_JUICE_DARK) - .setDarkMode(true) - .setBackgroundAlpha(0.95f) - .addEntity(myBot) - .start(); - } -} \ No newline at end of file diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java deleted file mode 100644 index 3cc57be..0000000 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java +++ /dev/null @@ -1,115 +0,0 @@ -package com.example.meepmeep.trajectories; - -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; - -import org.json.JSONObject; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.Scanner; - -public class BlueCloseBasket { - private final Pose2d START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK; - - public BlueCloseBasket(int type) { - String jsonString = ""; - - try { - File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json" : "/sdcard/FIRST/positions/blue/close-basket.json")); - Scanner reader = new Scanner(file); - - while (reader.hasNextLine()) { - jsonString += reader.nextLine(); - } - } catch ( - FileNotFoundException e) { - e.printStackTrace(); - } - - JSONObject positions = new JSONObject(jsonString); - - this.START = new Pose2d( - positions.getJSONObject("START").getDouble("x"), - positions.getJSONObject("START").getDouble("y"), - Math.toRadians(positions.getJSONObject("START").getDouble("heading")) - ); - - this.RUNGS = new Pose2d( - positions.getJSONObject("RUNG").getDouble("x"), - positions.getJSONObject("RUNG").getDouble("y"), - Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) - ); - - this.LEFT_SPIKEMARK = new Pose2d( - positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), - positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading")) - ); - - this.MID_SPIKEMARK = new Pose2d( - positions.getJSONObject("MID_SPIKEMARK").getDouble("x"), - positions.getJSONObject("MID_SPIKEMARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading")) - ); - - this.RIGHT_SPIKEMARK = new Pose2d( - positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"), - positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) - ); - - this.SCORE = new Pose2d( - positions.getJSONObject("SCORE").getDouble("x"), - positions.getJSONObject("SCORE").getDouble("y"), - Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) - ); - - this.SUBMERSIBLE = new Pose2d( - positions.getJSONObject("SUBMERSIBLE").getDouble("x"), - positions.getJSONObject("SUBMERSIBLE").getDouble("y"), - Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading")) - ); - - this.PARK = new Pose2d( - positions.getJSONObject("PARK").getDouble("x"), - positions.getJSONObject("PARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) - ); - - } - - public Action start(TrajectoryActionBuilder builder) { - return builder.splineToLinearHeading(RUNGS, Math.toRadians(180)) - .setTangent(Math.toRadians(0)) - .splineToLinearHeading(MID_SPIKEMARK, Math.toRadians(30)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(SCORE, Math.toRadians(93)) - .splineToLinearHeading(LEFT_SPIKEMARK, Math.toRadians(0)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(SCORE, Math.toRadians(93)) - .splineToLinearHeading(RIGHT_SPIKEMARK, Math.toRadians(0)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(SCORE, Math.toRadians(93)) - .build(); - } - - public Action cycle(TrajectoryActionBuilder builder){ - return builder.setTangent(Math.toRadians(180)) - .splineToLinearHeading(SUBMERSIBLE, Math.toRadians(270)) - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(SCORE, Math.toRadians(0)) - .build(); - } - - public Action park(TrajectoryActionBuilder builder){ - return builder.setTangent(Math.toRadians(180)) - .splineToLinearHeading(PARK, Math.toRadians(180)) - .build(); - } - - public Pose2d getStart() { - return this.START; - } -} diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java deleted file mode 100644 index bb94d18..0000000 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java +++ /dev/null @@ -1,94 +0,0 @@ -package com.example.meepmeep.trajectories; - -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; - -import org.json.JSONObject; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.Scanner; - -public class BlueFarPush { - private final Pose2d START, PUSH1, PUSH2, PUSH3, SCOREPUSH, PARK; - - public BlueFarPush(int type) { - String jsonString = ""; - - try { - File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json" : "/sdcard/FIRST/positions/blue/far-basket.json")); - Scanner reader = new Scanner(file); - - while (reader.hasNextLine()) { - jsonString += reader.nextLine(); - } - } catch ( - FileNotFoundException e) { - e.printStackTrace(); - } - - JSONObject positions = new JSONObject(jsonString); - - this.START = new Pose2d( - positions.getJSONObject("START").getDouble("x"), - positions.getJSONObject("START").getDouble("y"), - Math.toRadians(positions.getJSONObject("START").getDouble("heading")) - ); - - this.PUSH1 = new Pose2d( - positions.getJSONObject("PUSH1").getDouble("x"), - positions.getJSONObject("PUSH1").getDouble("y"), - Math.toRadians(positions.getJSONObject("PUSH1").getDouble("heading")) - ); - - this.PUSH2 = new Pose2d( - positions.getJSONObject("PUSH2").getDouble("x"), - positions.getJSONObject("PUSH2").getDouble("y"), - Math.toRadians(positions.getJSONObject("PUSH2").getDouble("heading")) - ); - - this.PUSH3 = new Pose2d( - positions.getJSONObject("PUSH3").getDouble("x"), - positions.getJSONObject("PUSH3").getDouble("y"), - Math.toRadians(positions.getJSONObject("PUSH3").getDouble("heading")) - ); - - this.SCOREPUSH = new Pose2d( - positions.getJSONObject("SCOREPUSH").getDouble("x"), - positions.getJSONObject("SCOREPUSH").getDouble("y"), - Math.toRadians(positions.getJSONObject("SCOREPUSH").getDouble("heading")) - ); - - this.PARK = new Pose2d( - positions.getJSONObject("PARK").getDouble("x"), - positions.getJSONObject("PARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) - ); - } - - public Action start(TrajectoryActionBuilder builder) { - return builder.splineToLinearHeading(this.SCOREPUSH, Math.toRadians((270))) - .turnTo(Math.toRadians(270)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.PUSH1, Math.toRadians(180)) - .strafeTo(new Vector2d(-46, 55)) - .setTangent(Math.toRadians(270)) - .splineToLinearHeading(this.PUSH2, Math.toRadians(180)) - .strafeTo(new Vector2d(-56, 55)) - .setTangent(Math.toRadians(270)) - .splineToLinearHeading(this.PUSH3, Math.toRadians(180)) - .strafeTo(new Vector2d(-62, 55)) - .build(); - } - public Action park(TrajectoryActionBuilder builder){ - return builder.setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.PARK, Math.toRadians(90)) - .build(); - } - - public Pose2d getStart() { - return this.START; - } -} diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java deleted file mode 100644 index 6f9bed1..0000000 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java +++ /dev/null @@ -1,115 +0,0 @@ -package com.example.meepmeep.trajectories; - -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; - -import org.json.JSONObject; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.Scanner; - -public class RedCloseBasket { - private final Pose2d START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK; - - public RedCloseBasket(int type) { - String jsonString = ""; - - try { - File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json" : "/sdcard/FIRST/positions/red/far-basket.json")); - Scanner reader = new Scanner(file); - - while (reader.hasNextLine()) { - jsonString += reader.nextLine(); - } - } catch ( - FileNotFoundException e) { - e.printStackTrace(); - } - - JSONObject positions = new JSONObject(jsonString); - - this.START = new Pose2d( - positions.getJSONObject("START").getDouble("x"), - positions.getJSONObject("START").getDouble("y"), - Math.toRadians(positions.getJSONObject("START").getDouble("heading")) - ); - - this.RUNGS = new Pose2d( - positions.getJSONObject("RUNG").getDouble("x"), - positions.getJSONObject("RUNG").getDouble("y"), - Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) - ); - - this.LEFT_SPIKEMARK = new Pose2d( - positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), - positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading")) - ); - - this.MID_SPIKEMARK = new Pose2d( - positions.getJSONObject("MID_SPIKEMARK").getDouble("x"), - positions.getJSONObject("MID_SPIKEMARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading")) - ); - - this.RIGHT_SPIKEMARK = new Pose2d( - positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"), - positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) - ); - - this.SCORE = new Pose2d( - positions.getJSONObject("SCORE").getDouble("x"), - positions.getJSONObject("SCORE").getDouble("y"), - Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) - ); - - this.SUBMERSIBLE = new Pose2d( - positions.getJSONObject("SUBMERSIBLE").getDouble("x"), - positions.getJSONObject("SUBMERSIBLE").getDouble("y"), - Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading")) - ); - - this.PARK = new Pose2d( - positions.getJSONObject("PARK").getDouble("x"), - positions.getJSONObject("PARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) - ); - - } - - public Action start(TrajectoryActionBuilder builder) { - return builder.splineToLinearHeading(this.RUNGS, Math.toRadians(0)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(210)) - .setTangent(0) - .splineToLinearHeading(this.SCORE, Math.toRadians(273)) - .splineToLinearHeading(this.LEFT_SPIKEMARK, Math.toRadians(180)) - .setTangent(0) - .splineToLinearHeading(this.SCORE, Math.toRadians(273)) - .splineToLinearHeading(this.RIGHT_SPIKEMARK, Math.toRadians(180)) - .setTangent(0) - .splineToLinearHeading(this.SCORE, Math.toRadians(273)) - .build(); - } - - public Action cycle(TrajectoryActionBuilder builder) { - return builder.setTangent(Math.toRadians(0)) - .splineToLinearHeading(this.SUBMERSIBLE, Math.toRadians(90)) - .setTangent(Math.toRadians(270)) - .splineToLinearHeading(this.SCORE, Math.toRadians(180)) - .build(); - } - - public Action park(TrajectoryActionBuilder builder){ - return builder.setTangent(0) - .splineToLinearHeading(PARK, Math.toRadians(0)) - .build(); - } - - public Pose2d getStart() { - return this.START; - } -} diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java deleted file mode 100644 index c8a1ee8..0000000 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java +++ /dev/null @@ -1,94 +0,0 @@ -package com.example.meepmeep.trajectories; - -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; - -import org.json.JSONObject; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.Scanner; - -public class RedFarPush { - private final Pose2d START, PUSH1, PUSH2, PUSH3, SCOREPUSH, PARK; - - public RedFarPush(int type) { - String jsonString = ""; - - try { - File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json" : "/sdcard/FIRST/positions/blue/far-basket.json")); - Scanner reader = new Scanner(file); - - while (reader.hasNextLine()) { - jsonString += reader.nextLine(); - } - } catch ( - FileNotFoundException e) { - e.printStackTrace(); - } - - JSONObject positions = new JSONObject(jsonString); - - this.START = new Pose2d( - positions.getJSONObject("START").getDouble("x"), - positions.getJSONObject("START").getDouble("y"), - Math.toRadians(positions.getJSONObject("START").getDouble("heading")) - ); - - this.PUSH1 = new Pose2d( - positions.getJSONObject("PUSH1").getDouble("x"), - positions.getJSONObject("PUSH1").getDouble("y"), - Math.toRadians(positions.getJSONObject("PUSH1").getDouble("heading")) - ); - - this.PUSH2 = new Pose2d( - positions.getJSONObject("PUSH2").getDouble("x"), - positions.getJSONObject("PUSH2").getDouble("y"), - Math.toRadians(positions.getJSONObject("PUSH2").getDouble("heading")) - ); - - this.PUSH3 = new Pose2d( - positions.getJSONObject("PUSH3").getDouble("x"), - positions.getJSONObject("PUSH3").getDouble("y"), - Math.toRadians(positions.getJSONObject("PUSH3").getDouble("heading")) - ); - - this.SCOREPUSH = new Pose2d( - positions.getJSONObject("SCOREPUSH").getDouble("x"), - positions.getJSONObject("SCOREPUSH").getDouble("y"), - Math.toRadians(positions.getJSONObject("SCOREPUSH").getDouble("heading")) - ); - - this.PARK = new Pose2d( - positions.getJSONObject("PARK").getDouble("x"), - positions.getJSONObject("PARK").getDouble("y"), - Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) - ); - } - - public Action start(TrajectoryActionBuilder builder) { - return builder.splineToLinearHeading(this.SCOREPUSH, Math.toRadians((90))) - .turnTo(Math.toRadians(270)) - .setTangent(Math.toRadians(0)) - .splineToLinearHeading(this.PUSH1, Math.toRadians(0)) - .strafeTo(new Vector2d(46, -55)) - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(this.PUSH2, Math.toRadians(0)) - .strafeTo(new Vector2d(56, -55)) - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(this.PUSH3, Math.toRadians(0)) - .strafeTo(new Vector2d(62, -55)) - .build(); - } - public Action park(TrajectoryActionBuilder builder){ - return builder.setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.PARK, Math.toRadians(90)) - .build(); - } - - public Pose2d getStart() { - return this.START; - } -} diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 759edbd..0d6eaa4 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -39,6 +39,6 @@ dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - // FTCLIB implementation 'org.ftclib.ftclib:core:2.0.1' + implementation 'org.json:json:20240303' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueCloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueCloseBasket.java new file mode 100644 index 0000000..1f41a2f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueCloseBasket.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.opmodes.auton.blue.trajectories; + +import com.pedropathing.localization.Pose; + +import org.json.JSONException; +import org.json.JSONObject; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.Scanner; + +public class BlueCloseBasket { + private final Pose START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK; + + public BlueCloseBasket() throws JSONException, FileNotFoundException { + String jsonString = ""; + + File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/blue/close-basket.json")); + Scanner reader = new Scanner(file); + + while (reader.hasNextLine()) { + jsonString += reader.nextLine(); + } + + JSONObject positions = new JSONObject(jsonString); + + this.START = new Pose( + positions.getJSONObject("START").getDouble("x"), + positions.getJSONObject("START").getDouble("y"), + Math.toRadians(positions.getJSONObject("START").getDouble("heading")) + ); + + this.RUNGS = new Pose( + positions.getJSONObject("RUNG").getDouble("x"), + positions.getJSONObject("RUNG").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) + ); + + this.LEFT_SPIKEMARK = new Pose( + positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), + positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading")) + ); + + this.MID_SPIKEMARK = new Pose( + positions.getJSONObject("MID_SPIKEMARK").getDouble("x"), + positions.getJSONObject("MID_SPIKEMARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading")) + ); + + this.RIGHT_SPIKEMARK = new Pose( + positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"), + positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) + ); + + this.SCORE = new Pose( + positions.getJSONObject("SCORE").getDouble("x"), + positions.getJSONObject("SCORE").getDouble("y"), + Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) + ); + + this.SUBMERSIBLE = new Pose( + positions.getJSONObject("SUBMERSIBLE").getDouble("x"), + positions.getJSONObject("SUBMERSIBLE").getDouble("y"), + Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading")) + ); + + this.PARK = new Pose( + positions.getJSONObject("PARK").getDouble("x"), + positions.getJSONObject("PARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) + ); + + } + + public Pose getStart() { + return this.START; + } +} diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarBasket.java similarity index 50% rename from PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarBasket.java index ff49cd8..0fa711e 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarBasket.java @@ -1,9 +1,8 @@ -package com.example.meepmeep.trajectories; +package org.firstinspires.ftc.teamcode.opmodes.auton.blue.trajectories; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.pedropathing.localization.Pose; +import org.json.JSONException; import org.json.JSONObject; import java.io.File; @@ -11,115 +10,82 @@ import java.util.Scanner; public class BlueFarBasket { - private final Pose2d START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK; + private final Pose START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK; - public BlueFarBasket(int type) { + public BlueFarBasket() throws JSONException, FileNotFoundException { String jsonString = ""; - try { - File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json" : "/sdcard/FIRST/positions/blue/far-basket.json")); - Scanner reader = new Scanner(file); + File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/blue/far-basket.json")); + Scanner reader = new Scanner(file); - while (reader.hasNextLine()) { - jsonString += reader.nextLine(); - } - } catch ( - FileNotFoundException e) { - e.printStackTrace(); + while (reader.hasNextLine()) { + jsonString += reader.nextLine(); } JSONObject positions = new JSONObject(jsonString); - this.START = new Pose2d( + this.START = new Pose( positions.getJSONObject("START").getDouble("x"), positions.getJSONObject("START").getDouble("y"), Math.toRadians(positions.getJSONObject("START").getDouble("heading")) ); - this.RUNGS = new Pose2d( + this.RUNGS = new Pose( positions.getJSONObject("RUNG").getDouble("x"), positions.getJSONObject("RUNG").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) ); - this.RUNGS1 = new Pose2d( + this.RUNGS1 = new Pose( positions.getJSONObject("RUNG1").getDouble("x"), positions.getJSONObject("RUNG1").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG1").getDouble("heading")) ); - this.RUNGS2 = new Pose2d( + this.RUNGS2 = new Pose( positions.getJSONObject("RUNG2").getDouble("x"), positions.getJSONObject("RUNG2").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG2").getDouble("heading")) ); - this.RUNGS3 = new Pose2d( + this.RUNGS3 = new Pose( positions.getJSONObject("RUNG3").getDouble("x"), positions.getJSONObject("RUNG3").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG3").getDouble("heading")) ); - this.LEFT_SPIKEMARK = new Pose2d( + this.LEFT_SPIKEMARK = new Pose( positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading")) ); - this.MID_SPIKEMARK = new Pose2d( + this.MID_SPIKEMARK = new Pose( positions.getJSONObject("MID_SPIKEMARK").getDouble("x"), positions.getJSONObject("MID_SPIKEMARK").getDouble("y"), Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading")) ); - this.RIGHT_SPIKEMARK = new Pose2d( + this.RIGHT_SPIKEMARK = new Pose( positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"), positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"), Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) ); - this.SCORE = new Pose2d( + this.SCORE = new Pose( positions.getJSONObject("SCORE").getDouble("x"), positions.getJSONObject("SCORE").getDouble("y"), Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) ); - this.PARK = new Pose2d( + this.PARK = new Pose( positions.getJSONObject("PARK").getDouble("x"), positions.getJSONObject("PARK").getDouble("y"), Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) ); } - public Action start(TrajectoryActionBuilder builder) { - return builder.splineToLinearHeading(this.RUNGS, Math.toRadians(0)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(150)) - .turnTo(this.LEFT_SPIKEMARK.heading) - .turnTo(Math.toRadians(90)) - .turnTo(this.RIGHT_SPIKEMARK.heading) - .turnTo(Math.toRadians(90)) - .turnTo(Math.toRadians((270))) - // Start of the cycling - .setTangent(Math.toRadians(335)) - .splineToLinearHeading(this.RUNGS1, Math.toRadians(0)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.SCORE, Math.toRadians(150)) - .setTangent(Math.toRadians(315)) - .splineToLinearHeading(this.RUNGS2, Math.toRadians(0)) - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.SCORE, Math.toRadians(150)) - .setTangent(Math.toRadians(315)) - .splineToLinearHeading(this.RUNGS3, Math.toRadians(0)) - .build(); - } - public Action park(TrajectoryActionBuilder builder){ - return builder.setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.PARK, Math.toRadians(90)) - .build(); - } - - public Pose2d getStart() { + public Pose getStart() { return this.START; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarPush.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarPush.java new file mode 100644 index 0000000..81acfc6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/BlueFarPush.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.opmodes.auton.blue.trajectories; + +import com.pedropathing.localization.Pose; + +import org.json.JSONException; +import org.json.JSONObject; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.Scanner; + +public class BlueFarPush { + private final Pose START, PUSH1, PUSH2, PUSH3, SCOREPUSH, PARK; + + public BlueFarPush() throws JSONException, FileNotFoundException { + String jsonString = ""; + + File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/blue/far-basket.json")); + Scanner reader = new Scanner(file); + + while (reader.hasNextLine()) { + jsonString += reader.nextLine(); + } + + JSONObject positions = new JSONObject(jsonString); + + this.START = new Pose( + positions.getJSONObject("START").getDouble("x"), + positions.getJSONObject("START").getDouble("y"), + Math.toRadians(positions.getJSONObject("START").getDouble("heading")) + ); + + this.PUSH1 = new Pose( + positions.getJSONObject("PUSH1").getDouble("x"), + positions.getJSONObject("PUSH1").getDouble("y"), + Math.toRadians(positions.getJSONObject("PUSH1").getDouble("heading")) + ); + + this.PUSH2 = new Pose( + positions.getJSONObject("PUSH2").getDouble("x"), + positions.getJSONObject("PUSH2").getDouble("y"), + Math.toRadians(positions.getJSONObject("PUSH2").getDouble("heading")) + ); + + this.PUSH3 = new Pose( + positions.getJSONObject("PUSH3").getDouble("x"), + positions.getJSONObject("PUSH3").getDouble("y"), + Math.toRadians(positions.getJSONObject("PUSH3").getDouble("heading")) + ); + + this.SCOREPUSH = new Pose( + positions.getJSONObject("SCOREPUSH").getDouble("x"), + positions.getJSONObject("SCOREPUSH").getDouble("y"), + Math.toRadians(positions.getJSONObject("SCOREPUSH").getDouble("heading")) + ); + + this.PARK = new Pose( + positions.getJSONObject("PARK").getDouble("x"), + positions.getJSONObject("PARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) + ); + } + + public Pose getStart() { + return this.START; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/close-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/positions/close-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/close-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/positions/close-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/far-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/positions/far-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/blue/far-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/positions/far-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/notes.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/notes.txt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java new file mode 100644 index 0000000..352dab2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.opmodes.auton.red.trajectories; + +import com.pedropathing.localization.Pose; + +import org.json.JSONException; +import org.json.JSONObject; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.Scanner; + +public class RedCloseBasket { + private final Pose START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK; + + public RedCloseBasket(int type) throws JSONException, FileNotFoundException { + String jsonString = ""; + + File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/red/far-basket.json")); + Scanner reader = new Scanner(file); + + while (reader.hasNextLine()) { + jsonString += reader.nextLine(); + } + + JSONObject positions = new JSONObject(jsonString); + + this.START = new Pose( + positions.getJSONObject("START").getDouble("x"), + positions.getJSONObject("START").getDouble("y"), + Math.toRadians(positions.getJSONObject("START").getDouble("heading")) + ); + + this.RUNGS = new Pose( + positions.getJSONObject("RUNG").getDouble("x"), + positions.getJSONObject("RUNG").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) + ); + + this.LEFT_SPIKEMARK = new Pose( + positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), + positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading")) + ); + + this.MID_SPIKEMARK = new Pose( + positions.getJSONObject("MID_SPIKEMARK").getDouble("x"), + positions.getJSONObject("MID_SPIKEMARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading")) + ); + + this.RIGHT_SPIKEMARK = new Pose( + positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"), + positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) + ); + + this.SCORE = new Pose( + positions.getJSONObject("SCORE").getDouble("x"), + positions.getJSONObject("SCORE").getDouble("y"), + Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) + ); + + this.SUBMERSIBLE = new Pose( + positions.getJSONObject("SUBMERSIBLE").getDouble("x"), + positions.getJSONObject("SUBMERSIBLE").getDouble("y"), + Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading")) + ); + + this.PARK = new Pose( + positions.getJSONObject("PARK").getDouble("x"), + positions.getJSONObject("PARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) + ); + + } + + public Pose getStart() { + return this.START; + } +} diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarBasket.java similarity index 50% rename from PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarBasket.java index 89eee83..77fb5ca 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarBasket.java @@ -1,9 +1,8 @@ -package com.example.meepmeep.trajectories; +package org.firstinspires.ftc.teamcode.opmodes.auton.red.trajectories; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.pedropathing.localization.Pose; +import org.json.JSONException; import org.json.JSONObject; import java.io.File; @@ -11,114 +10,82 @@ import java.util.Scanner; public class RedFarBasket { - private final Pose2d START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK; + private final Pose START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK; - public RedFarBasket(int type) { + public RedFarBasket() throws JSONException, FileNotFoundException { String jsonString = ""; - try { - File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json" : "/sdcard/FIRST/positions/red/far-basket.json")); - Scanner reader = new Scanner(file); + File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/red/far-basket.json")); + Scanner reader = new Scanner(file); - while (reader.hasNextLine()) { - jsonString += reader.nextLine(); - } - } catch ( - FileNotFoundException e) { - e.printStackTrace(); + while (reader.hasNextLine()) { + jsonString += reader.nextLine(); } JSONObject positions = new JSONObject(jsonString); - this.START = new Pose2d( + this.START = new Pose( positions.getJSONObject("START").getDouble("x"), positions.getJSONObject("START").getDouble("y"), Math.toRadians(positions.getJSONObject("START").getDouble("heading")) ); - this.RUNGS = new Pose2d( + this.RUNGS = new Pose( positions.getJSONObject("RUNG").getDouble("x"), positions.getJSONObject("RUNG").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) ); - this.RUNGS1 = new Pose2d( + this.RUNGS1 = new Pose( positions.getJSONObject("RUNG1").getDouble("x"), positions.getJSONObject("RUNG1").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG1").getDouble("heading")) ); - this.RUNGS2 = new Pose2d( + this.RUNGS2 = new Pose( positions.getJSONObject("RUNG2").getDouble("x"), positions.getJSONObject("RUNG2").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG2").getDouble("heading")) ); - this.RUNGS3 = new Pose2d( + this.RUNGS3 = new Pose( positions.getJSONObject("RUNG3").getDouble("x"), positions.getJSONObject("RUNG3").getDouble("y"), Math.toRadians(positions.getJSONObject("RUNG3").getDouble("heading")) ); - this.LEFT_SPIKEMARK = new Pose2d( + this.LEFT_SPIKEMARK = new Pose( positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading")) ); - this.MID_SPIKEMARK = new Pose2d( + this.MID_SPIKEMARK = new Pose( positions.getJSONObject("MID_SPIKEMARK").getDouble("x"), positions.getJSONObject("MID_SPIKEMARK").getDouble("y"), Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading")) ); - this.RIGHT_SPIKEMARK = new Pose2d( + this.RIGHT_SPIKEMARK = new Pose( positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"), positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"), Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) ); - this.SCORE = new Pose2d( + this.SCORE = new Pose( positions.getJSONObject("SCORE").getDouble("x"), positions.getJSONObject("SCORE").getDouble("y"), Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) ); - this.PARK = new Pose2d( + this.PARK = new Pose( positions.getJSONObject("PARK").getDouble("x"), positions.getJSONObject("PARK").getDouble("y"), Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) ); } - public Action start(TrajectoryActionBuilder builder) { - return builder.splineToLinearHeading(this.RUNGS, Math.toRadians(180)) - .setTangent(Math.toRadians(0)) - .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(330)) - .turnTo(this.LEFT_SPIKEMARK.heading) - .turnTo(Math.toRadians(270)) - .turnTo(this.RIGHT_SPIKEMARK.heading) - .turnTo(Math.toRadians(270)) - .turnTo(Math.toRadians(90)) - .setTangent(Math.toRadians(155)) - .splineToLinearHeading(this.RUNGS1, Math.toRadians(180)) - .setTangent(Math.toRadians(0)) - .splineToLinearHeading(this.SCORE, Math.toRadians(330)) - .setTangent(Math.toRadians(135)) - .splineToLinearHeading(this.RUNGS2, Math.toRadians(180)) - .setTangent(Math.toRadians(0)) - .splineToLinearHeading(this.SCORE, Math.toRadians(330)) - .setTangent(Math.toRadians(135)) - .splineToLinearHeading(this.RUNGS3, Math.toRadians(180)) - .build(); - } - - public Action park(TrajectoryActionBuilder builder){ - return builder.setTangent(Math.toRadians(0)) - .splineToLinearHeading(this.PARK, Math.toRadians(270)).build(); - } - - public Pose2d getStart() { + public Pose getStart() { return this.START; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarPush.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarPush.java new file mode 100644 index 0000000..7e12af9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedFarPush.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.opmodes.auton.red.trajectories; + +import com.pedropathing.localization.Pose; + +import org.json.JSONException; +import org.json.JSONObject; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.Scanner; + +public class RedFarPush { + private final Pose START, PUSH1, PUSH2, PUSH3, SCOREPUSH, PARK; + + public RedFarPush() throws JSONException, FileNotFoundException { + String jsonString = ""; + + File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/red/far-basket.json")); + Scanner reader = new Scanner(file); + + while (reader.hasNextLine()) { + jsonString += reader.nextLine(); + } + + JSONObject positions = new JSONObject(jsonString); + + this.START = new Pose( + positions.getJSONObject("START").getDouble("x"), + positions.getJSONObject("START").getDouble("y"), + Math.toRadians(positions.getJSONObject("START").getDouble("heading")) + ); + + this.PUSH1 = new Pose( + positions.getJSONObject("PUSH1").getDouble("x"), + positions.getJSONObject("PUSH1").getDouble("y"), + Math.toRadians(positions.getJSONObject("PUSH1").getDouble("heading")) + ); + + this.PUSH2 = new Pose( + positions.getJSONObject("PUSH2").getDouble("x"), + positions.getJSONObject("PUSH2").getDouble("y"), + Math.toRadians(positions.getJSONObject("PUSH2").getDouble("heading")) + ); + + this.PUSH3 = new Pose( + positions.getJSONObject("PUSH3").getDouble("x"), + positions.getJSONObject("PUSH3").getDouble("y"), + Math.toRadians(positions.getJSONObject("PUSH3").getDouble("heading")) + ); + + this.SCOREPUSH = new Pose( + positions.getJSONObject("SCOREPUSH").getDouble("x"), + positions.getJSONObject("SCOREPUSH").getDouble("y"), + Math.toRadians(positions.getJSONObject("SCOREPUSH").getDouble("heading")) + ); + + this.PARK = new Pose( + positions.getJSONObject("PARK").getDouble("x"), + positions.getJSONObject("PARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) + ); + } + + public Pose getStart() { + return this.START; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/close-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/positions/close-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/close-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/positions/close-basket.json diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/far-basket.json b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/positions/far-basket.json similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/positions/red/far-basket.json rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/positions/far-basket.json From 18ea06fe1df25a90b123faec951942349703101e Mon Sep 17 00:00:00 2001 From: Bi1ku <2008owenshi@gmail.com> Date: Mon, 30 Dec 2024 22:49:25 -0500 Subject: [PATCH 008/107] add placeholder trajectories to opmodes --- .../teamcode/opmodes/auton/blue/CloseBasket.java | 13 +++++++++++++ .../ftc/teamcode/opmodes/auton/blue/FarBasket.java | 13 +++++++++++++ .../ftc/teamcode/opmodes/auton/red/CloseBasket.java | 13 +++++++++++++ .../ftc/teamcode/opmodes/auton/red/FarBasket.java | 13 +++++++++++++ .../auton/red/trajectories/RedCloseBasket.java | 2 +- 5 files changed, 53 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java index 035f11e..516bf96 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java @@ -6,17 +6,30 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.opmodes.auton.blue.trajectories.BlueCloseBasket; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; +import java.io.PrintWriter; +import java.io.StringWriter; + @Autonomous(name = "Blue Close Basket", preselectTeleOp = "Main") public class CloseBasket extends OpModeCore { private Follower follower; + private BlueCloseBasket trajectories; private CommandRobot robot; @Override public void initialize() { // TODO: Input correct starting position this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); + + try { + this.trajectories = new BlueCloseBasket(); + } catch (Exception e) { + StringWriter errors = new StringWriter(); + e.printStackTrace(new PrintWriter(errors)); + super.multipleTelemetry.addLine(errors.toString()); + } } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java index e20cdb4..3b4f5ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java @@ -5,16 +5,29 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.opmodes.auton.blue.trajectories.BlueFarBasket; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; +import java.io.PrintWriter; +import java.io.StringWriter; + @Autonomous(name = "Blue Far Bakset", preselectTeleOp = "Main") public class FarBasket extends OpModeCore { private CommandRobot robot; + private BlueFarBasket trajectories; @Override public void initialize() { // TODO: Input correct starting position this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); + + try { + this.trajectories = new BlueFarBasket(); + } catch (Exception e) { + StringWriter errors = new StringWriter(); + e.printStackTrace(new PrintWriter(errors)); + super.multipleTelemetry.addLine(errors.toString()); + } } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java index 0d42089..a02a43a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java @@ -5,16 +5,29 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.opmodes.auton.red.trajectories.RedCloseBasket; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; +import java.io.PrintWriter; +import java.io.StringWriter; + @Autonomous(name = "Red Close Basket", preselectTeleOp = "Main") public class CloseBasket extends OpModeCore { private CommandRobot robot; + private RedCloseBasket trajectories; @Override public void initialize() { // TODO: Input correct starting position this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); + + try { + this.trajectories = new RedCloseBasket(); + } catch (Exception e) { + StringWriter errors = new StringWriter(); + e.printStackTrace(new PrintWriter(errors)); + super.multipleTelemetry.addLine(errors.toString()); + } } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java index 902dcd3..d895c00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java @@ -5,16 +5,29 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.CommandRobot; +import org.firstinspires.ftc.teamcode.opmodes.auton.red.trajectories.RedFarBasket; import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; +import java.io.PrintWriter; +import java.io.StringWriter; + @Autonomous(name = "Red Far Basket", preselectTeleOp = "Main") public class FarBasket extends OpModeCore { private CommandRobot robot; + private RedFarBasket trajectories; @Override public void initialize() { // TODO: Input correct starting position this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); + + try { + this.trajectories = new RedFarBasket(); + } catch (Exception e) { + StringWriter errors = new StringWriter(); + e.printStackTrace(new PrintWriter(errors)); + super.multipleTelemetry.addLine(errors.toString()); + } } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java index 352dab2..0f0fa33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/RedCloseBasket.java @@ -12,7 +12,7 @@ public class RedCloseBasket { private final Pose START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK; - public RedCloseBasket(int type) throws JSONException, FileNotFoundException { + public RedCloseBasket() throws JSONException, FileNotFoundException { String jsonString = ""; File file = new File(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/red/far-basket.json")); From 9626a7190362c6b82ac91f7771c540555ecd522c Mon Sep 17 00:00:00 2001 From: Bi1ku <2008owenshi@gmail.com> Date: Mon, 30 Dec 2024 22:54:17 -0500 Subject: [PATCH 009/107] add example auto to notes --- .../ftc/teamcode/opmodes/auton/notes.txt | 139 ++++++++++++++++++ 1 file changed, 139 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt index 5a6600c..51aa413 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt @@ -20,3 +20,142 @@ FIELD_LENGTH = 72.0 | ____ | | ---- | \--------------/ + + +@Autonomous(name = "0+5", preselectTeleOp = "Solo") +public class ZeroPlusFive extends OpModeCore { + public static Path[] paths = new Path[16]; + + public void buildPaths() { + paths[0] = buildLine( + Constants.specimenStartPose, + new Pose(41.063, 65.625, 180), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[1] = buildCurve( + new Pose(41.063, 65.625, 180), + new Point(28.125, 50.063), + new Pose(46.500, 36.750, 270), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[2] = buildLine( + new Pose(46.500, 36.750, 270), + new Pose(46.500, 36.750, 200), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[3] = buildLine( + new Pose(46.500, 36.750, 200), + new Pose(46.500, 36.750, 270), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[4] = buildLine( + new Pose(46.500, 36.750, 270), + new Pose(46.500, 36.750, 200), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[5] = buildLine( + new Pose(46.500, 36.750, 200), + new Pose(46.500, 28.000, 270), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[6] = buildLine( + new Pose(46.500, 28.000, 270), + new Pose(46.500, 28.000, 200), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[7] = buildCurve( + new Pose(46.500, 28.000, 200), + new Point(26.625, 34.688), + new Pose(7.125, 36.375, 0), + AutonomousHelpers.HeadingInterpolation.LINEAR + ); + paths[8] = buildCurve( + new Pose(7.125, 36.375, 0), + new Point(28.313, 64.125), + new Pose(41.063, 64.500, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[9] = buildCurve( + new Pose(41.063, 64.500, 0), + new Point(28.313, 64.125), + new Pose(7.125, 36.375, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[10] = buildCurve( + new Pose(7.125, 36.375, 0), + new Point(28.313, 64.125), + new Pose(41.063, 67.500, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[11] = buildCurve( + new Pose(41.063, 67.500, 0), + new Point(28.313, 64.125), + new Pose(7.125, 36.375, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[12] = buildCurve( + new Pose(7.125, 36.375, 0), + new Point(28.313, 64.125), + new Pose(41.063, 69.500, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[13] = buildCurve( + new Pose(41.063, 69.500, 0), + new Point(28.313, 64.125), + new Pose(7.125, 36.375, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[14] = buildCurve( + new Pose(7.125, 36.375, 0), + new Point(28.313, 64.125), + new Pose(41.063, 71.500, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + paths[15] = buildCurve( + new Pose(41.063, 71.500, 0), + new Point(13.875, 47.625), + new Pose(6.750, 37.500, 0), + AutonomousHelpers.HeadingInterpolation.CONSTANT + ); + } + + public void initialize() { + } + + @Override + public void runOpMode() { + Drivetrain drivetrain = new Drivetrain(hardwareMap, super.multipleTelemetry, Constants.specimenStartPose); + CommandScheduler.getInstance().reset(); + + buildPaths(); + + while (!isStarted()) { + CommandScheduler.getInstance().run(); + } + + CommandScheduler.getInstance().schedule( + new SequentialCommandGroup( + new PathCommand(drivetrain.getFollower(), paths[0]), + new PathCommand(drivetrain.getFollower(), paths[1]), + new PathCommand(drivetrain.getFollower(), paths[2]), + new PathCommand(drivetrain.getFollower(), paths[3]), + new PathCommand(drivetrain.getFollower(), paths[4]), + new PathCommand(drivetrain.getFollower(), paths[5]), + new PathCommand(drivetrain.getFollower(), paths[6]), + new PathCommand(drivetrain.getFollower(), paths[7]), + new PathCommand(drivetrain.getFollower(), paths[8]), + new PathCommand(drivetrain.getFollower(), paths[9]), + new PathCommand(drivetrain.getFollower(), paths[10]), + new PathCommand(drivetrain.getFollower(), paths[11]), + new PathCommand(drivetrain.getFollower(), paths[12]), + new PathCommand(drivetrain.getFollower(), paths[13]), + new PathCommand(drivetrain.getFollower(), paths[14]), + new PathCommand(drivetrain.getFollower(), paths[15]) + ) + ); + + while (opModeIsActive() && !isStopRequested()) { + CommandScheduler.getInstance().run(); + } + } +} \ No newline at end of file From 5470141223b5308753439174843cc8da3d29ca01 Mon Sep 17 00:00:00 2001 From: Stephen Chen Date: Mon, 30 Dec 2024 23:03:01 -0500 Subject: [PATCH 010/107] copy quickstart is a go --- .../opmodes/auton/blue/CloseBasket.java | 204 ++++++++++++++++-- 1 file changed, 191 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java index 8e2a373..641aee8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java @@ -2,34 +2,212 @@ import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.CommandRobot; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer; + @Autonomous(name = "Blue Close Basket", preselectTeleOp = "Main") -public class CloseBasket extends OpModeCore { +public class CloseBasket extends OpMode { private Follower follower; private CommandRobot robot; + private Timer pathTimer, actionTimer, opmodeTimer; + + private int pathState; + + private final Pose startPose = new Pose(8, 113, Math.toRadians(0)); + + private final Pose scorePose = new Pose(14, 129, Math.toRadians(315)); + + /** Lowest (First) Sample from the Spike Mark */ + private final Pose pickup1Pose = new Pose(37, 121, Math.toRadians(0)); + + /** Middle (Second) Sample from the Spike Mark */ + private final Pose pickup2Pose = new Pose(37, 132, Math.toRadians(0)); + + /** Highest (Third) Sample from the Spike Mark */ + private final Pose pickup3Pose = new Pose(45.7, 133.2, Math.toRadians(90)); + + /** Park Pose for our robot, after we do all of the scoring. */ + private final Pose parkPose = new Pose(60, 98, Math.toRadians(90)); + + /** Park Control Pose for our robot, this is used to manipulate the bezier curve that we will create for the parking. + * The Robot will not go to this pose, it is used a control point for our bezier curve. */ + private final Pose parkControlPose = new Pose(65, 98, Math.toRadians(90)); + + private Path scorePreload, park; + private PathChain grabSpikemark1, grabSpikemark2, grabSpikemark3, scoreSpikemark1, scoreSpikemark2, scoreSpikemark3; + + /** This method is called once at the init of the OpMode. **/ @Override - public void initialize() { - // TODO: Input correct starting position - this.robot = new CommandRobot(super.hardwareMap, new Pose(0, 0, 0), super.multipleTelemetry, this); + public void init() { + pathTimer = new Timer(); + opmodeTimer = new Timer(); + opmodeTimer.resetTimer(); + + follower = new Follower(hardwareMap); + follower.setStartingPose(startPose); + buildPaths(); } + /** This is the main loop of the OpMode, it will run repeatedly after clicking "Play". **/ @Override - public void runOpMode() { - CommandScheduler.getInstance().enable(); - this.initialize(); + public void loop() { + + // These loop the movements of the robot + follower.update(); + autonomousPathUpdate(); + + // Feedback to Driver Hub + telemetry.addData("path state", pathState); + telemetry.addData("x", follower.getPose().getX()); + telemetry.addData("y", follower.getPose().getY()); + telemetry.addData("heading", follower.getPose().getHeading()); + telemetry.update(); + } + + public void buildPaths(){ + scorePreload = new Path(new BezierLine(new Point(startPose), new Point(scorePose))); + scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading()); + + grabSpikemark1 = follower.pathBuilder() + .addPath(new BezierLine(new Point(scorePose), new Point(pickup1Pose))) + .setLinearHeadingInterpolation(scorePose.getHeading(), pickup1Pose.getHeading()) + .build(); + + scoreSpikemark1 = follower.pathBuilder() + .addPath(new BezierLine(new Point(pickup1Pose), new Point(scorePose))) + .setLinearHeadingInterpolation(pickup1Pose.getHeading(), scorePose.getHeading()) + .build(); + + grabSpikemark2 = follower.pathBuilder() + .addPath(new BezierLine(new Point(scorePose), new Point(pickup2Pose))) + .setLinearHeadingInterpolation(scorePose.getHeading(), pickup2Pose.getHeading()) + .build(); + + scoreSpikemark2 = follower.pathBuilder() + .addPath(new BezierLine(new Point(pickup2Pose), new Point(scorePose))) + .setLinearHeadingInterpolation(pickup2Pose.getHeading(), scorePose.getHeading()) + .build(); - super.waitForStart(); + grabSpikemark3 = follower.pathBuilder() + .addPath(new BezierLine(new Point(scorePose), new Point(pickup3Pose))) + .setLinearHeadingInterpolation(scorePose.getHeading(), pickup3Pose.getHeading()) + .build(); - // TODO: Add auto pathing and functionality - // TODO: Make an end command for roadrunner (see kookybotz)? - // TODO: Research how to stop Action.runBlocking() without error + scoreSpikemark3 = follower.pathBuilder() + .addPath(new BezierLine(new Point(pickup3Pose), new Point(scorePose))) + .setLinearHeadingInterpolation(pickup3Pose.getHeading(), scorePose.getHeading()) + .build(); - super.end(); + /* This is our park path. We are using a BezierCurve with 3 points, which is a curved line that is curved based off of the control point */ + park = new Path(new BezierCurve(new Point(scorePose), /* Control Point */ new Point(parkControlPose), new Point(parkPose))); + park.setLinearHeadingInterpolation(scorePose.getHeading(), parkPose.getHeading()); + } + +//- Follower State: "if(!follower.isBusy() {}" (Though, I don't recommend this because it might not return due to holdEnd +//- Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}" +//- Robot Position: "if(follower.getPose().getX() > 36) {}" + public void autonomousPathUpdate() { + switch (pathState) { + case 0: + follower.followPath(scorePreload); + setPathState(1); + break; + case 1: + if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) { + /* Score Preload */ + + follower.followPath(grabSpikemark1, true); + setPathState(2); + } + break; + case 2: + if (follower.getPose().getX() > (pickup1Pose.getX() - 1) && follower.getPose().getY() > (pickup1Pose.getY() - 1)) { + /* Grab Sample */ + + follower.followPath(scoreSpikemark1, true); + setPathState(3); + } + break; + case 3: + if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) { + /* Score Sample */ + + follower.followPath(grabSpikemark2, true); + setPathState(4); + } + break; + case 4: + if (follower.getPose().getX() > (pickup2Pose.getX() - 1) && follower.getPose().getY() > (pickup2Pose.getY() - 1)) { + /* Grab Sample */ + + follower.followPath(scoreSpikemark2, true); + setPathState(5); + } + break; + case 5: + if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) { + /* Score Sample */ + + follower.followPath(grabSpikemark3, true); + setPathState(6); + } + break; + case 6: + if (follower.getPose().getX() > (pickup3Pose.getX() - 1) && follower.getPose().getY() > (pickup3Pose.getY() - 1)) { + /* Grab Sample */ + + follower.followPath(scoreSpikemark3, true); + setPathState(7); + } + break; + case 7: + if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) { + /* Score Sample */ + + follower.followPath(park, true); + setPathState(8); + } + break; + case 8: + if (follower.getPose().getX() > (parkPose.getX() - 1) && follower.getPose().getY() > (parkPose.getY() - 1)) { + /* Level 1 Ascent */ + + setPathState(-1); + } + break; + } + } + + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } + + /** This method is called continuously after Init while waiting for "play". **/ + @Override + public void init_loop() {} + + /** This method is called once at the start of the OpMode. + * It runs all the setup actions, including building paths and starting the path system **/ + @Override + public void start() { + opmodeTimer.resetTimer(); + setPathState(0); + } + + /** We do not use this because everything should automatically disable **/ + @Override + public void stop() { } } From 7de15b408c2bd546fac0ff1b0ac98a2d7206743c Mon Sep 17 00:00:00 2001 From: Bi1ku <2008owenshi@gmail.com> Date: Mon, 30 Dec 2024 23:37:53 -0500 Subject: [PATCH 011/107] rename + tested auto push (works) --- .run/AutoPush.run.xml | 2 +- .../ftc/teamcode/opmodes/auton/blue/CloseBasket.java | 6 +++--- .../ftc/teamcode/opmodes/auton/blue/FarBasket.java | 6 +++--- .../{BlueCloseBasket.java => CloseBasketTrajectories.java} | 4 ++-- .../{BlueFarBasket.java => FarBasketTrajectories.java} | 4 ++-- .../{BlueFarPush.java => FarPushTrajectories.java} | 4 ++-- .../ftc/teamcode/opmodes/auton/red/CloseBasket.java | 6 +++--- .../ftc/teamcode/opmodes/auton/red/FarBasket.java | 6 +++--- .../{RedCloseBasket.java => CloseBasketTrajectories.java} | 4 ++-- .../{RedFarBasket.java => FarBasketTrajectories.java} | 4 ++-- .../{RedFarPush.java => FarPushTrajectories.java} | 4 ++-- 11 files changed, 25 insertions(+), 25 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/{BlueCloseBasket.java => CloseBasketTrajectories.java} (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/{BlueFarBasket.java => FarBasketTrajectories.java} (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/trajectories/{BlueFarPush.java => FarPushTrajectories.java} (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/{RedCloseBasket.java => CloseBasketTrajectories.java} (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/{RedFarBasket.java => FarBasketTrajectories.java} (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/trajectories/{RedFarPush.java => FarPushTrajectories.java} (95%) diff --git a/.run/AutoPush.run.xml b/.run/AutoPush.run.xml index 8730f5c..0a95fad 100644 --- a/.run/AutoPush.run.xml +++ b/.run/AutoPush.run.xml @@ -1,6 +1,6 @@ -