From 27d24f0cfe42b65d339529ae34e0aced9fdaaf22 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Sun, 2 Nov 2025 22:43:06 -0500 Subject: [PATCH 01/29] New Base Structure --- TeamCode/build.gradle | 9 +- .../firstinspires/ftc/teamcode/AnalogTest.kt | 28 - .../firstinspires/ftc/teamcode/BasicDrive.kt | 136 -- .../ftc/teamcode/CommandBaseTest.kt | 19 - .../ftc/teamcode/FlyWheelTest.kt | 48 - .../firstinspires/ftc/teamcode/IntakeTest.kt | 45 - .../ftc/teamcode/Pedro/Constants.java | 58 - .../ftc/teamcode/Pedro/Tuning.java | 1327 ----------------- .../org/firstinspires/ftc/teamcode/Teleop.kt | 170 --- .../org/firstinspires/ftc/teamcode/Test.kt | 31 - .../firstinspires/ftc/teamcode/VisionTest.kt | 92 -- .../ftc/teamcode/auton/BlueFarAuton.kt | 140 -- .../ftc/teamcode/auton/BlueShortAuton.kt | 150 -- .../ftc/teamcode/auton/RedShortAuton.kt | 147 -- .../ftc/teamcode/commands/FollowPath.kt | 17 - .../common/GoBildaPinpointDriver.java | 746 --------- .../teamcode/common/commands/TeleopStopper.kt | 13 + .../ftc/teamcode/common/hardware/AxonCR.kt | 46 + .../ftc/teamcode/common/hardware/Util.kt | 12 + .../hardware/cached}/CachedAxon.kt | 4 +- .../hardware/cached}/CachedMotor.kt | 4 +- .../hardware/manual}/ManualAxon.kt | 3 +- .../hardware/manual}/ManualManager.kt | 2 +- .../hardware/manual}/ManualMotor.kt | 3 +- .../teamcode/common/subsystem/Subsystem.kt | 15 + .../common/subsystem/SubsystemManager.kt | 17 + .../ftc/teamcode/opmode/OpMode.kt | 58 + .../ftc/teamcode/opmode/auton/SampleAuton.kt | 8 + .../teamcode/opmode/teleop/SampleTeleop.kt | 10 + .../ftc/teamcode/pedro/Constants.kt | 78 + .../org/firstinspires/ftc/teamcode/readme.md | 1 - .../ftc/teamcode/subsystems/Drivetrain.kt | 44 - .../ftc/teamcode/subsystems/FlyWheel.kt | 59 - .../ftc/teamcode/subsystems/Intake.kt | 26 - .../ftc/teamcode/subsystems/Odom.kt | 44 - .../ftc/teamcode/subsystems/PedroDrive.kt | 25 - .../ftc/teamcode/subsystems/Subsystem.kt | 10 - .../ftc/teamcode/subsystems/Uppies.kt | 334 ----- .../ftc/teamcode/subsystems/Vision.kt | 70 - .../ftc/teamcode/util/APIDFController.java | 258 ---- .../ftc/teamcode/util/CommandUtil.kt | 15 - .../firstinspires/ftc/teamcode/util/Misc.kt | 13 - .../firstinspires/ftc/teamcode/util/Pose2d.kt | 84 -- .../firstinspires/ftc/teamcode/util/Vec2d.kt | 124 -- build.dependencies.gradle | 2 +- 45 files changed, 271 insertions(+), 4274 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AnalogTest.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicDrive.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandBaseTest.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyWheelTest.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IntakeTest.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Constants.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Tuning.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueFarAuton.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueShortAuton.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/RedShortAuton.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowPath.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/GoBildaPinpointDriver.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/TeleopStopper.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/AxonCR.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/Util.kt rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{util => common/hardware/cached}/CachedAxon.kt (84%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{util => common/hardware/cached}/CachedMotor.kt (90%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{util => common/hardware/manual}/ManualAxon.kt (85%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{util => common/hardware/manual}/ManualManager.kt (82%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{util => common/hardware/manual}/ManualMotor.kt (83%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/SubsystemManager.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FlyWheel.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odom.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PedroDrive.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Uppies.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Vision.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/APIDFController.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CommandUtil.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Misc.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Pose2d.kt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Vec2d.kt diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 350a5d409d72..31a8c2d05386 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -21,13 +21,12 @@ android { dependencies { implementation project(':FtcRobotController') -// implementation 'com.github.Ashondiscord:Ftc-offseason:0c0de61789' -// implementation('com.millburnx:cmdx:+') - implementation 'com.github.Ashondiscord:Ftc-offseason:CmdX-v0.1.2' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:34e2ea1904' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:34e2ea1904' +// implementation('com.millburnx:cmdx:+') +// implementation('com.millburnx:cmdxpedro:+') -// implementation 'com.acmerobotics.dashboard:dashboard:0.5.0' implementation("org.jetbrains.kotlinx:kotlinx-coroutines-core:1.10.2") -// implementation 'com.github.Thermal-Equilibrium:homeostasis-FTC:1.0.8' implementation 'org.ftclib.ftclib:core:2.1.1' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AnalogTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AnalogTest.kt deleted file mode 100644 index a1c9156fb898..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AnalogTest.kt +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp - -@TeleOp(name = "AnalogTest") -class AnalogTest : LinearOpMode() { - val scheduler = CommandScheduler() - override fun runOpMode() { - val a0 = hardwareMap.analogInput["a0"] - val a1 = hardwareMap.analogInput["a1"] - val s0 = hardwareMap.crservo["s0"] - val s1 = hardwareMap.crservo["s1"] - -// telemetry.isAutoClear = true - - waitForStart() - - while (opModeIsActive() && !isStopRequested) { - telemetry.addData("a0", a0.voltage) - telemetry.addData("a1", a1.voltage) - s0.power = 0.1 - s1.power = 0.1 - telemetry.update() - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicDrive.kt deleted file mode 100644 index 71c38bd85210..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicDrive.kt +++ /dev/null @@ -1,136 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.millburnx.cmdx.Command -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.qualcomm.robotcore.hardware.DcMotorSimple -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit -import org.firstinspires.ftc.teamcode.util.ManualManager -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.Vec2d -import kotlin.math.abs -import kotlin.math.max - - -@TeleOp(name = "BasicDrive") -class BasicDrive : LinearOpMode() { - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - timer.reset() - - telemetry.addData("hz", loopHertz) - telemetry.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - - var hubs: List = emptyList() - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) as MutableList - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.AUTO } - - val br = hardwareMap["m3"] as DcMotorEx // m3 - val fl = hardwareMap["m0"] as DcMotorEx // m0 - val bl = hardwareMap["m1"] as DcMotorEx // m1 - val fr = hardwareMap["m2"] as DcMotorEx // m2 - - val odom = hardwareMap["odom"] as GoBildaPinpointDriver - - fl.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - fr.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - br.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - bl.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - - fl.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS - fr.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS - br.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS - bl.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS - - fl.direction = DcMotorSimple.Direction.REVERSE - fr.direction = DcMotorSimple.Direction.FORWARD - br.direction = DcMotorSimple.Direction.FORWARD - bl.direction = DcMotorSimple.Direction.REVERSE - - - val diameterMM = 35.0 - val ticksPerRevolution = 8192.0 - val circumferenceMM = diameterMM * Math.PI - val ticksPerMM = ticksPerRevolution / circumferenceMM - odom.setEncoderResolution(ticksPerMM, DistanceUnit.MM) - odom.setOffsets(5.75, -0.75, DistanceUnit.INCH) - odom.setEncoderDirections( - GoBildaPinpointDriver.EncoderDirection.FORWARD, - GoBildaPinpointDriver.EncoderDirection.REVERSED, - ) - odom.resetPosAndIMU() - - telemetry.isAutoClear = true - - waitForStart() - - scheduler.schedule( - Command { - while (opModeIsActive() && !isStopRequested) { - val forward = -gamepad1.left_stick_y.toDouble() - val strafe = -gamepad1.left_stick_x.toDouble() - val rotate = gamepad1.right_stick_x.toDouble() - - - val heading = odom.getHeading(AngleUnit.RADIANS) - - val rVec = Vec2d(strafe, forward).rotate(heading - Math.toRadians(90.0)) * Vec2d(1.0, 1.1) - - val denominator = max(abs(rVec.x) + abs(rVec.y) + abs(rotate), 1.0) - -// val right_y = gamepad1.right_stick_y.toDouble() - -// fl.power = forward; -// fr.power = strafe; -// br.power = rotate; -// bl.power = right_y; - - fl.power = (rVec.x + rVec.y + rotate) / denominator - fr.power = (rVec.x - rVec.y - rotate) / denominator - br.power = (rVec.x + rVec.y - rotate) / denominator - bl.power = (rVec.x - rVec.y + rotate) / denominator - - sync() - } - }, - ) - - scheduler.schedule( - Command { - while (opModeIsActive() && !isStopRequested) { - odom.update() - - val pose = odom.position - val pose2D = Pose2d( - pose.getX(DistanceUnit.INCH), - pose.getY(DistanceUnit.INCH), - pose.getHeading(AngleUnit.DEGREES) - ) - - telemetry.addData("pose", pose2D.toString()) - - sync() - } - } - ) - - while (opModeIsActive() && !isStopRequested) { - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandBaseTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandBaseTest.kt deleted file mode 100644 index 2aedbaa8f39a..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandBaseTest.kt +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.millburnx.cmdx.Command -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp - -@TeleOp(name = "CommandBaseTest") -class CommandBaseTest : LinearOpMode() { - val scheduler = CommandScheduler() - - override fun runOpMode() { - scheduler.schedule( - Command { - println("Hello World!") - }, - ) - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyWheelTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyWheelTest.kt deleted file mode 100644 index 580907ecd9f1..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyWheelTest.kt +++ /dev/null @@ -1,48 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.subsystems.FlyWheel -import org.firstinspires.ftc.teamcode.util.ManualManager - - -@TeleOp(name = "FlyWheelTest") -class FlyWheelTest : LinearOpMode() { - - var hubs: List = emptyList() - - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - telemetry.addData("hz", loopHertz) - timer.reset() - - telemetry.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.MANUAL } - - ManualManager.init() - - val flyWheel = FlyWheel(this) - - telemetry.isAutoClear = true - - waitForStart() - - scheduler.schedule(flyWheel.command) - - while (opModeIsActive() && !isStopRequested) { - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IntakeTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IntakeTest.kt deleted file mode 100644 index a1e346a7d13d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IntakeTest.kt +++ /dev/null @@ -1,45 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.millburnx.cmdx.Command -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.qualcomm.robotcore.hardware.DcMotorSimple - - -@TeleOp(name = "IntakeTest") -class IntakeTest : LinearOpMode() { - val scheduler = CommandScheduler() - - override fun runOpMode() { - val hubs = hardwareMap.getAll(LynxModule::class.java) as MutableList - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.AUTO } - - val motor = hardwareMap["m0e"] as DcMotorEx - motor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - motor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS - motor.direction = DcMotorSimple.Direction.FORWARD - telemetry.isAutoClear = true - - waitForStart() - - scheduler.schedule( - Command { - while (opModeIsActive() && !isStopRequested) { - val power = gamepad1.left_stick_x - motor.power = power.toDouble() - telemetry.addData("Power: ", power.toString()) - telemetry.update() - -// sync() - } - }, - ) - - while (opModeIsActive() && !isStopRequested) { - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Constants.java deleted file mode 100644 index b7210f6a5470..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Constants.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.firstinspires.ftc.teamcode.Pedro; - - -import com.pedropathing.control.PIDFCoefficients; -import com.pedropathing.follower.Follower; -import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; -import com.pedropathing.ftc.drivetrains.MecanumConstants; -import com.pedropathing.ftc.localization.constants.PinpointConstants; -import com.pedropathing.paths.PathConstraints; -import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - -public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants() - .mass(12) - .forwardZeroPowerAcceleration(-154.736) - .lateralZeroPowerAcceleration(-148.621) - .translationalPIDFCoefficients(new PIDFCoefficients(0.12, 0, 0.002, 0.015)) - .headingPIDFCoefficients(new PIDFCoefficients(3.5, 0, 0.28, 0.01)); - - public static MecanumConstants driveConstants = new MecanumConstants() - .maxPower(1) - .rightFrontMotorName("m2") - .rightRearMotorName("m3") - .leftFrontMotorName("m0") - .leftRearMotorName("m1") - .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) - .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) - .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) - .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) - .xVelocity(140) // 147.58413 - .yVelocity(131.245); - - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 0.3, 1); - - public static PinpointConstants localizerConstants = new PinpointConstants() - .hardwareMapName("odom") - .distanceUnit(DistanceUnit.INCH) -// .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) - .customEncoderResolution((74.505/17.8)*1.5*0.55*0.86*0.9375) // increasing this value decreases x: reading - .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED) - .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED) - .forwardPodY(-6.375) - .strafePodX(1.44); - - - - public static Follower createFollower(HardwareMap hardwareMap) { - return new FollowerBuilder(followerConstants, hardwareMap) - .pathConstraints(pathConstraints) - .pinpointLocalizer(localizerConstants) - .mecanumDrivetrain(driveConstants) - .build(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Tuning.java deleted file mode 100644 index 5f162ae2116c..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pedro/Tuning.java +++ /dev/null @@ -1,1327 +0,0 @@ -package org.firstinspires.ftc.teamcode.Pedro; - -import static org.firstinspires.ftc.teamcode.Pedro.Tuning.changes; -import static org.firstinspires.ftc.teamcode.Pedro.Tuning.drawOnlyCurrent; -import static org.firstinspires.ftc.teamcode.Pedro.Tuning.draw; -import static org.firstinspires.ftc.teamcode.Pedro.Tuning.follower; -import static org.firstinspires.ftc.teamcode.Pedro.Tuning.stopRobot; -import static org.firstinspires.ftc.teamcode.Pedro.Tuning.telemetryM; - -import com.bylazar.configurables.PanelsConfigurables; -import com.bylazar.configurables.annotations.Configurable; -import com.bylazar.configurables.annotations.IgnoreConfigurable; -import com.bylazar.field.FieldManager; -import com.bylazar.field.PanelsField; -import com.bylazar.field.Style; -import com.bylazar.telemetry.PanelsTelemetry; -import com.bylazar.telemetry.TelemetryManager; -import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.*; -import com.pedropathing.math.*; -import com.pedropathing.paths.*; -import com.pedropathing.telemetry.SelectableOpMode; -import com.pedropathing.util.*; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.Pedro.Constants; - -import java.util.ArrayList; -import java.util.List; - -/** - * This is the Tuning class. It contains a selection menu for various tuning OpModes. - * - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 6/26/2025 - */ -@Configurable -@TeleOp(name = "Tuning", group = "Pedro Pathing") -public class Tuning extends SelectableOpMode { - public static Follower follower; - - @IgnoreConfigurable - static PoseHistory poseHistory; - - @IgnoreConfigurable - static TelemetryManager telemetryM; - - @IgnoreConfigurable - static ArrayList changes = new ArrayList<>(); - - public Tuning() { - super("Select a Tuning OpMode", s -> { - s.folder("Localization", l -> { - l.add("Localization Test", LocalizationTest::new); - l.add("Forward Tuner", ForwardTuner::new); - l.add("Lateral Tuner", LateralTuner::new); - l.add("Turn Tuner", TurnTuner::new); - }); - s.folder("Automatic", a -> { - a.add("Forward Velocity Tuner", ForwardVelocityTuner::new); - a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); - a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); - a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); - }); - s.folder("Manual", p -> { - p.add("Translational Tuner", TranslationalTuner::new); - p.add("Heading Tuner", HeadingTuner::new); - p.add("Drive Tuner", DriveTuner::new); - p.add("Centripetal Tuner", CentripetalTuner::new); - }); - s.folder("Tests", p -> { - p.add("Line", Line::new); - p.add("Triangle", Triangle::new); - p.add("Circle", Circle::new); - }); - }); - } - - @Override - public void onSelect() { - if (follower == null) { - follower = Constants.createFollower(hardwareMap); - PanelsConfigurables.INSTANCE.refreshClass(this); - } else { - follower = Constants.createFollower(hardwareMap); - } - - follower.setStartingPose(new Pose()); - - poseHistory = follower.getPoseHistory(); - - telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); - - Drawing.init(); - } - - @Override - public void onLog(List lines) {} - - public static void drawOnlyCurrent() { - try { - Drawing.drawRobot(follower.getPose()); - Drawing.sendPacket(); - } catch (Exception e) { - throw new RuntimeException("Drawing failed " + e); - } - } - - public static void draw() { - Drawing.drawDebug(follower); - } - - /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ - public static void stopRobot() { - follower.startTeleopDrive(true); - follower.setTeleOpDrive(0,0,0,true); - } -} - -/** - * 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. - * You should use this to check the robot's localization. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 5/6/2024 - */ -class LocalizationTest extends OpMode { - @Override - public void init() {} - - /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("This will print your robot's position to telemetry while " - + "allowing robot control through a basic mecanum drive on gamepad 1."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void start() { - follower.startTeleopDrive(); - follower.update(); - } - - /** - * This updates the robot's pose estimate, the simple mecanum drive, and updates the - * Panels telemetry with the robot's position as well as draws the robot's position. - */ - @Override - public void loop() { - follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); - follower.update(); - - telemetryM.debug("x:" + follower.getPose().getX()); - telemetryM.debug("y:" + follower.getPose().getY()); - telemetryM.debug("heading:" + follower.getPose().getHeading()); - telemetryM.debug("total heading:" + follower.getTotalHeading()); - telemetryM.update(telemetry); - - draw(); - } -} - -/** - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 5/6/2024 - */ -class ForwardTuner extends OpMode { - public static double DISTANCE = 48; - - @Override - public void init() { - follower.update(); - drawOnlyCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); - drawOnlyCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated multiplier and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Distance Moved: " + follower.getPose().getX()); - telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); - telemetryM.update(telemetry); - - draw(); - } -} - -/** - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 2.0, 6/26/2025 - */ -class LateralTuner extends OpMode { - public static double DISTANCE = 48; - - @Override - public void init() { - follower.update(); - drawOnlyCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); - drawOnlyCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated multiplier and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Distance Moved: " + follower.getPose().getY()); - telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); - telemetryM.update(telemetry); - - draw(); - } -} - -/** - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 5/6/2024 - */ -class TurnTuner extends OpMode { - public static double ANGLE = 2 * Math.PI; - - @Override - public void init() { - follower.update(); - drawOnlyCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); - - drawOnlyCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated multiplier and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Total Angle: " + follower.getTotalHeading()); - telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); - telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); - telemetryM.update(telemetry); - - draw(); - } -} - -/** - * 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 - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 3/13/2024 - */ -class ForwardVelocityTuner extends OpMode { - private final ArrayList velocities = new ArrayList<>(); - public static double DISTANCE = 48; - public static double RECORD_NUMBER = 10; - - private boolean end; - - @Override - public void init() {} - - /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.debug("pose", follower.getPose()); - telemetryM.update(telemetry); - - follower.update(); - drawOnlyCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run forward at full power. */ - @Override - public void start() { - for (int i = 0; i < RECORD_NUMBER; i++) { - velocities.add(0.0); - } - follower.startTeleopDrive(true); - follower.update(); - end = false; - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B 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.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - draw(); - - - if (!end) { - if (Math.abs(follower.getPose().getX()) > DISTANCE) { - end = true; - stopRobot(); - } else { - follower.setTeleOpDrive(1,0,0,true); - //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); - double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX()); - velocities.add(currentVelocity); - velocities.remove(0); - } - } else { - stopRobot(); - double average = 0; - for (double velocity : velocities) { - average += velocity; - } - average /= velocities.size(); - telemetryM.debug("Forward Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); - - for (int i = 0; i < velocities.size(); i++) { - telemetry.addData(String.valueOf(i), velocities.get(i)); - } - - telemetryM.update(telemetry); - telemetry.update(); - - if (gamepad1.aWasPressed()) { - follower.setXVelocity(average); - String message = "XMovement: " + average; - changes.add(message); - } - } - } -} - -/** - * 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 - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 3/13/2024 - */ -class LateralVelocityTuner extends OpMode { - private final ArrayList velocities = new ArrayList<>(); - - public static double DISTANCE = 48; - public static double RECORD_NUMBER = 10; - - private boolean end; - - @Override - public void init() {} - - /** - * This initializes the drive motors as well as the cache of velocities and the Panels - * telemetryM. - */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); - - follower.update(); - drawOnlyCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run right at full power. */ - @Override - public void start() { - for (int i = 0; i < RECORD_NUMBER; i++) { - velocities.add(0.0); - } - follower.startTeleopDrive(true); - follower.update(); - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B 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.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - draw(); - - if (!end) { - if (Math.abs(follower.getPose().getY()) > DISTANCE) { - end = true; - stopRobot(); - } else { - follower.setTeleOpDrive(0,1,0,true); - double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2))); - velocities.add(currentVelocity); - velocities.remove(0); - } - } else { - stopRobot(); - double average = 0; - for (double velocity : velocities) { - average += velocity; - } - average /= velocities.size(); - - telemetryM.debug("Strafe Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); - telemetryM.update(telemetry); - - if (gamepad1.aWasPressed()) { - follower.setYVelocity(average); - String message = "YMovement: " + average; - changes.add(message); - } - } - } -} - -/** - * 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 - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/13/2024 - */ -class ForwardZeroPowerAccelerationTuner extends OpMode { - private final ArrayList accelerations = new ArrayList<>(); - public static double VELOCITY = 30; - - private double previousVelocity; - private long previousTimeNano; - - private boolean stopping; - private boolean end; - - @Override - public void init() {} - - /** This initializes the drive motors as well as the Panels telemetryM. */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run forward at full power. */ - @Override - public void start() { - follower.startTeleopDrive(false); - follower.update(); - follower.setTeleOpDrive(1,0,0,true); - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B 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.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - draw(); - - Vector heading = new Vector(1.0, follower.getPose().getHeading()); - if (!end) { - if (!stopping) { - if (follower.getVelocity().dot(heading) > VELOCITY) { - previousVelocity = follower.getVelocity().dot(heading); - previousTimeNano = System.nanoTime(); - stopping = true; - follower.setTeleOpDrive(0,0,0,true); - } - } else { - double currentVelocity = follower.getVelocity().dot(heading); - accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); - previousVelocity = currentVelocity; - previousTimeNano = System.nanoTime(); - if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { - end = true; - } - } - } else { - double average = 0; - for (double acceleration : accelerations) { - average += acceleration; - } - average /= accelerations.size(); - - telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); - - if (gamepad1.aWasPressed()) { - follower.getConstants().setForwardZeroPowerAcceleration(average); - String message = "Forward Zero Power Acceleration: " + average; - changes.add(message); - } - } - } -} - -/** - * 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 - * 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. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 3/13/2024 - */ -class LateralZeroPowerAccelerationTuner extends OpMode { - private final ArrayList accelerations = new ArrayList<>(); - public static double VELOCITY = 30; - private double previousVelocity; - private long previousTimeNano; - private boolean stopping; - private boolean end; - - @Override - public void init() {} - - /** This initializes the drive motors as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run forward at full power. */ - @Override - public void start() { - follower.startTeleopDrive(false); - follower.update(); - follower.setTeleOpDrive(0,1,0,true); - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B 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.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - draw(); - - Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2); - if (!end) { - if (!stopping) { - if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) { - previousVelocity = Math.abs(follower.getVelocity().dot(heading)); - previousTimeNano = System.nanoTime(); - stopping = true; - follower.setTeleOpDrive(0,0,0,true); - } - } else { - double currentVelocity = Math.abs(follower.getVelocity().dot(heading)); - accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); - previousVelocity = currentVelocity; - previousTimeNano = System.nanoTime(); - if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { - end = true; - } - } - } else { - double average = 0; - for (double acceleration : accelerations) { - average += acceleration; - } - average /= accelerations.size(); - - telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); - - if (gamepad1.aWasPressed()) { - follower.getConstants().setLateralZeroPowerAcceleration(average); - String message = "Lateral Zero Power Acceleration: " + average; - changes.add(message); - } - } - } -} - -/** - * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. - * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. - * - * @author Baron Henderson - 20077 The Indubitables - * @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 - */ -class TranslationalTuner extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() {} - - /** This initializes the Follower and creates the forward and backward Paths. */ - @Override - public void init_loop() { - telemetryM.debug("This will activate the translational PIDF(s)"); - telemetryM.debug("The robot will try to stay in place while you push it laterally."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void start() { - follower.deactivateAllPIDFs(); - follower.activateTranslational(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); - forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); - backwards.setConstantHeadingInterpolation(0); - follower.followPath(forwards); - } - - /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ - @Override - public void loop() { - follower.update(); - draw(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); - telemetryM.update(telemetry); - } -} - -/** - * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. - * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. - * It will try to keep the robot at a constant heading while the user tries to turn it. - * - * @author Baron Henderson - 20077 The Indubitables - * @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 - */ -class HeadingTuner extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() {} - - /** - * This initializes the Follower and creates the forward and backward Paths. Additionally, this - * initializes the Panels telemetry. - */ - @Override - public void init_loop() { - telemetryM.debug("This will activate the heading PIDF(s)."); - telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void start() { - follower.deactivateAllPIDFs(); - follower.activateHeading(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); - forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); - backwards.setConstantHeadingInterpolation(0); - follower.followPath(forwards); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - draw(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); - telemetryM.update(telemetry); - } -} - -/** - * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. - * - * @author Baron Henderson - 20077 The Indubitables - * @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 - */ -class DriveTuner extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private PathChain forwards; - private PathChain backwards; - - @Override - public void init() {} - - /** - * This initializes the Follower and creates the forward and backward Paths. Additionally, this - * initializes the Panels telemetry. - */ - @Override - public void init_loop() { - telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); - telemetryM.debug("The robot will go forward and backward continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void start() { - follower.deactivateAllPIDFs(); - follower.activateDrive(); - - forwards = follower.pathBuilder() - .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))) - .setConstantHeadingInterpolation(0) - .build(); - - backwards = follower.pathBuilder() - .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))) - .setConstantHeadingInterpolation(0) - .build(); - - follower.followPath(forwards); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - draw(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Driving forward?: " + forward); - telemetryM.update(telemetry); - } -} - -/** - * This is the Line Test Tuner OpMode. It will drive the robot forward and back - * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. - * - * @author Baron Henderson - 20077 The Indubitables - * @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 - */ -class Line extends OpMode { - public static double DISTANCE = 48; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() {} - - /** This initializes the Follower and creates the forward and backward Paths. */ - @Override - public void init_loop() { - telemetryM.debug("This will activate all the PIDF(s)"); - telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void start() { - follower.activateAllPIDFs(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); - forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); - backwards.setConstantHeadingInterpolation(0); - follower.followPath(forwards); - } - - /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ - @Override - public void loop() { - follower.update(); - draw(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Driving Forward?: " + forward); - telemetryM.update(telemetry); - } -} - -/** - * This is the Centripetal Tuner 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. - * - * @author Baron Henderson - 20077 The Indubitables - * @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 - */ -class CentripetalTuner extends OpMode { - public static double DISTANCE = 24; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() {} - - /** - * This initializes the Follower and creates the forward and backward Paths. - * Additionally, this initializes the Panels telemetry. - */ - @Override - public void init_loop() { - telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); - telemetryM.debug("The robot will go continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void start() { - follower.activateAllPIDFs(); - forwards = new Path(new BezierCurve(new Pose(), new Pose(Math.abs(DISTANCE),0), new Pose(Math.abs(DISTANCE),DISTANCE))); - backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE),DISTANCE), new Pose(Math.abs(DISTANCE),0), new Pose(0,0))); - - backwards.setTangentHeadingInterpolation(); - backwards.reverseHeadingInterpolation(); - - follower.followPath(forwards); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - draw(); - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Driving away from the origin along the curve?: " + forward); - telemetryM.update(telemetry); - } -} - -/** - * This is the Triangle autonomous OpMode. - * It runs the robot in a triangle, with the starting point being the bottom-middle point. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge - * @version 1.0, 12/30/2024 - */ -class Triangle extends OpMode { - - private final Pose startPose = new Pose(0, 0, Math.toRadians(0)); - private final Pose interPose = new Pose(24, -24, Math.toRadians(90)); - private final Pose endPose = new Pose(24, 24, Math.toRadians(45)); - - private PathChain triangle; - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - draw(); - - if (follower.atParametricEnd()) { - follower.followPath(triangle, true); - } - } - - @Override - public void init() {} - - @Override - public void init_loop() { - telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); - telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - /** Creates the PathChain for the "triangle".*/ - @Override - public void start() { - follower.setStartingPose(startPose); - - triangle = follower.pathBuilder() - .addPath(new BezierLine(startPose, interPose)) - .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading()) - .addPath(new BezierLine(interPose, endPose)) - .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading()) - .addPath(new BezierLine(endPose, startPose)) - .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading()) - .build(); - - follower.followPath(triangle); - } -} - -/** - * 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 - */ -class Circle extends OpMode { - public static double RADIUS = 10; - private PathChain circle; - - public void start() { - circle = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS, 0), new Pose(RADIUS, RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(RADIUS, RADIUS), new Pose(RADIUS, 2 * RADIUS), new Pose(0, 2 * RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(0, 2 * RADIUS), new Pose(-RADIUS, 2 * RADIUS), new Pose(-RADIUS, RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(-RADIUS, RADIUS), new Pose(-RADIUS, 0), new Pose(0, 0))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .build(); - follower.followPath(circle); - } - - @Override - public void init_loop() { - telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); - telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); - telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); - telemetryM.update(telemetry); - follower.update(); - drawOnlyCurrent(); - } - - @Override - public void init() {} - - /** - * 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(); - draw(); - - if (follower.atParametricEnd()) { - follower.followPath(circle); - } - } -} - -/** - * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. - * - * @author Lazar - 19234 - * @version 1.1, 5/19/2025 - */ -class Drawing { - public static final double ROBOT_RADIUS = 9; // woah - private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); - - private static final Style robotLook = new Style( - "", "#3F51B5", 0.75 - ); - private static final Style historyLook = new Style( - "", "#4CAF50", 0.75 - ); - - /** - * This prepares Panels Field for using Pedro Offsets - */ - public static void init() { - panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); - } - - /** - * 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 Pedro Follower instance. - */ - public static void drawDebug(Follower follower) { - if (follower.getCurrentPath() != null) { - drawPath(follower.getCurrentPath(), robotLook); - Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); - drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); - } - drawPoseHistory(follower.getPoseHistory(), historyLook); - drawRobot(follower.getPose(), historyLook); - - sendPacket(); - } - - /** - * This draws a robot at a specified Pose with a specified - * look. The heading is represented as a line. - * - * @param pose the Pose to draw the robot at - * @param style the parameters used to draw the robot with - */ - public static void drawRobot(Pose pose, Style style) { - if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { - return; - } - - panelsField.setStyle(style); - panelsField.moveCursor(pose.getX(), pose.getY()); - panelsField.circle(ROBOT_RADIUS); - - Vector v = pose.getHeadingAsUnitVector(); - v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); - double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; - double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); - - panelsField.setStyle(style); - panelsField.moveCursor(x1, y1); - panelsField.line(x2, y2); - } - - /** - * This draws a robot at a specified Pose. The heading is represented as a line. - * - * @param pose the Pose to draw the robot at - */ - public static void drawRobot(Pose pose) { - drawRobot(pose, robotLook); - } - - /** - * This draws a Path with a specified look. - * - * @param path the Path to draw - * @param style the parameters used to draw the Path with - */ - public static void drawPath(Path path, Style style) { - double[][] points = path.getPanelsDrawingPoints(); - - for (int i = 0; i < points[0].length; i++) { - for (int j = 0; j < points.length; j++) { - if (Double.isNaN(points[j][i])) { - points[j][i] = 0; - } - } - } - - panelsField.setStyle(style); - panelsField.moveCursor(points[0][0], points[0][1]); - panelsField.line(points[1][0], points[1][1]); - } - - /** - * This draws all the Paths in a PathChain with a - * specified look. - * - * @param pathChain the PathChain to draw - * @param style the parameters used to draw the PathChain with - */ - public static void drawPath(PathChain pathChain, Style style) { - for (int i = 0; i < pathChain.size(); i++) { - drawPath(pathChain.getPath(i), style); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - * @param style the parameters used to draw the pose history with - */ - public static void drawPoseHistory(PoseHistory poseTracker, Style style) { - panelsField.setStyle(style); - - int size = poseTracker.getXPositionsArray().length; - for (int i = 0; i < size - 1; i++) { - - panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); - panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - */ - public static void drawPoseHistory(PoseHistory poseTracker) { - drawPoseHistory(poseTracker, historyLook); - } - - /** - * This tries to send the current packet to FTControl Panels. - */ - public static void sendPacket() { - panelsField.update(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.kt deleted file mode 100644 index 04ce8d6f6760..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.kt +++ /dev/null @@ -1,170 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.bylazar.configurables.annotations.Configurable -import com.millburnx.cmdx.Command -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.subsystems.FlyWheel -import org.firstinspires.ftc.teamcode.subsystems.Intake -import org.firstinspires.ftc.teamcode.subsystems.PedroDrive -import org.firstinspires.ftc.teamcode.subsystems.Uppies -import org.firstinspires.ftc.teamcode.util.APIDFController -import org.firstinspires.ftc.teamcode.util.ManualManager -import org.firstinspires.ftc.teamcode.util.Vec2d -import org.firstinspires.ftc.teamcode.util.deg -import org.firstinspires.ftc.teamcode.util.rad -import kotlin.math.absoluteValue - - -@TeleOp(name = "Teleop") -class Teleop : LinearOpMode() { - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - timer.reset() - - telemetry.addData("hz", loopHertz) - telemetry.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - - var hubs: List = emptyList() - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.MANUAL } - - ManualManager.init() - - val pedro = PedroDrive(this) - val intake = Intake(this) - val flyWheel = FlyWheel(this) - val uppies = Uppies(this, intake, flyWheel) - - telemetry.isAutoClear = true - - waitForStart() - - pedro.follower.startTeleopDrive(); - - val assistPID = APIDFController(Assist.kP, Assist.kI, Assist.kD, 0.0) - - var offsetHeading = 0.0 - - scheduler.schedule(pedro.command) - scheduler.schedule(Command("pedro teleop") { - while (opModeIsActive() && !isStopRequested) { - val override = (gamepad2.left_stick_x.absoluteValue > 0.3 || gamepad2.left_stick_y.absoluteValue > 0.3) - - val rx = if (gamepad2.right_stick_x.absoluteValue > 0.5 || gamepad2.right_stick_y.absoluteValue > 0.5) { - assistPID.p = Assist.kP - assistPID.i = Assist.kI - assistPID.d = Assist.kD - - val targetHeading = Vec2d().angleTo(Vec2d(gamepad2.right_stick_x, -gamepad2.right_stick_y)) - val error = assistPID.calculate((pedro.follower.pose.heading - offsetHeading).deg(), targetHeading.deg()) - telemetry.addLine("$targetHeading $error") - error - } else { - -gamepad1.right_stick_x.toDouble() - } - - telemetry.addData("override", override) - -// pedro.follower.setTeleOpDrive( -// (if (override) -gamepad2.left_stick_y else -gamepad1.left_stick_x).toDouble(), -// (if (override) -gamepad2.left_stick_x else gamepad1.left_stick_y).toDouble(), -// rx, -// !override, // Robot Centric -// offsetHeading + 90.0.rad(), -// ) - if (override) { - pedro.follower.setTeleOpDrive( - -gamepad2.left_stick_y.toDouble(), - -gamepad2.left_stick_x.toDouble(), - rx, - true, - offsetHeading + 90.0.rad() - ) - } else { - pedro.follower.setTeleOpDrive( - -gamepad1.left_stick_y.toDouble(), - -gamepad1.left_stick_x.toDouble(), - rx, - true - ) - } - sync() - } - }) - - scheduler.schedule(intake.command) - scheduler.schedule(flyWheel.command) - scheduler.schedule(uppies.command) - - var flyWheelClose = true - - scheduler.schedule(Command { - while (opModeIsActive()) { - if (gamepad1.bWasPressed()) { - println("${uppies.autoFireCommandTeleop.job} | ${uppies.autoFireCommandTeleop.job?.isActive}") - if (uppies.autoFireCommandTeleop.job?.isActive == true) { - // TODO: I don't think this canceling works? But you can ignore if its not a quick fix - // Cuz theoretically the library should be fine but who knows lol - uppies.autoFireCommandTeleop.cancel() - println("CANCELING AUTO") - } else { - scheduler.schedule(uppies.autoFireCommandTeleop) - } - } - if (gamepad2.xWasPressed()) { - offsetHeading = pedro.follower.pose.heading - } - if (gamepad2.leftBumperWasPressed()) { - if (flyWheelClose) { - FlyWheel.TeleopClosePower -= 0.05 - } else { - FlyWheel.FarPower -= 0.05 - } - } - if (gamepad2.rightBumperWasPressed()) { - if (flyWheelClose) { - FlyWheel.TeleopClosePower += 0.05 - } else { - FlyWheel.FarPower += 0.05 - } - } - flyWheel.power = if (flyWheelClose) FlyWheel.TeleopClosePower else FlyWheel.FarPower - telemetry.addData("flywheel power - $flyWheelClose", flyWheel.power) - sync() - } - }) - - if (gamepad2.dpad_right) flyWheelClose = true - if (gamepad2.dpad_left) flyWheelClose = false - - while (opModeIsActive() && !isStopRequested) { - } - scheduler.runner.cancel() - } - -} - -@Configurable -object Assist { - @JvmField - var kP = 0.05 - - @JvmField - var kI = 0.0 - - @JvmField - var kD = 0.0 -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.kt deleted file mode 100644 index 4a889f647c40..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.kt +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import org.firstinspires.ftc.teamcode.subsystems.FlyWheel - - -@TeleOp(name = "Test") -class Test : LinearOpMode() { - val scheduler = CommandScheduler() - - override fun runOpMode() { - val hubs = hardwareMap.getAll(LynxModule::class.java) as MutableList - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.AUTO } - - val flyWheel = FlyWheel(this) -// val uppies = Uppies(this, tel) - -// super.telemetry.isAutoClear = true - - waitForStart() - - scheduler.schedule(flyWheel.command) -// scheduler.schedule(uppies.command) - - while (opModeIsActive() && !isStopRequested) { - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.kt deleted file mode 100644 index cfbb214f26b1..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.kt +++ /dev/null @@ -1,92 +0,0 @@ -package org.firstinspires.ftc.teamcode - -import com.arcrobotics.ftclib.controller.PIDController -import com.bylazar.configurables.annotations.Configurable -import com.millburnx.cmdx.Command -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.subsystems.Drivetrain -import org.firstinspires.ftc.teamcode.subsystems.Odom -import org.firstinspires.ftc.teamcode.subsystems.Vision -import org.firstinspires.ftc.teamcode.util.ManualManager -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.normalizeDegrees -import kotlin.math.abs - - -@Configurable -@TeleOp(name = "VisionTest") -class VisionTest : LinearOpMode() { - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - timer.reset() - - telemetry.addData("hz", loopHertz) -// tel.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - -// val tel = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - - var hubs: List = emptyList() - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.MANUAL } - - ManualManager.init() -// telemetry.isAutoClear = true - - val vision = Vision(this) - val odom = Odom(this) - val drivetrain = Drivetrain(this) - - waitForStart() - - scheduler.schedule(vision.command) - - var prevAlignButton = gamepad1.b - while (opModeIsActive() && !isStopRequested) { - val newAlignButton = gamepad1.b - if (newAlignButton && !prevAlignButton) { - scheduler.schedule(Command("Auto Align") { - // TODO: Get this command working, didn't have time to properly test yesterday - // Possibly could have errors in the vision subsystem btw - val targetPose = vision.targetPose ?: return@Command - val anglePid = PIDController(kP, kI, kD) - val angleTo = { odom.pose.position.angleTo(targetPose.position) } - val getError = { - normalizeDegrees(odom.pose.heading - angleTo()) - } - while (abs(getError()) < angleThreshold) { - val power = anglePid.calculate(odom.pose.heading, angleTo()) - drivetrain.drive(Pose2d(0.0, 0.0, power)) - } - }) - } - prevAlignButton = newAlignButton - } - } - - companion object { - @JvmField - var angleThreshold = 10.0 - - @JvmField - var kP = 0.0 - - @JvmField - var kI = 0.0 - - @JvmField - var kD = 0.0 - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueFarAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueFarAuton.kt deleted file mode 100644 index 02444e8f20ea..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueFarAuton.kt +++ /dev/null @@ -1,140 +0,0 @@ -package org.firstinspires.ftc.teamcode.auton - -import com.millburnx.cmdx.commandGroups.Sequential -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.pedropathing.geometry.BezierCurve -import com.pedropathing.geometry.BezierLine -import com.pedropathing.geometry.Pose -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.commands.FollowPathCommand -import org.firstinspires.ftc.teamcode.subsystems.FlyWheel -import org.firstinspires.ftc.teamcode.subsystems.Intake -import org.firstinspires.ftc.teamcode.subsystems.PedroDrive -import org.firstinspires.ftc.teamcode.subsystems.Uppies -import org.firstinspires.ftc.teamcode.util.ManualManager -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.SleepFor - - -@Autonomous(name = "Blue Far Auton", preselectTeleOp = "Teleop") -class BlueFarAuton : LinearOpMode() { - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - timer.reset() - - telemetry.addData("hz", loopHertz) - telemetry.addData("cmd", this.runner.commandList.joinToString(", ")) - telemetry.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - - var hubs: List = emptyList() - - fun Path1(pedro: PedroDrive, intake: Intake) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(56.000, 8.000), Pose(56.000, 12.000)) - ) - .setLinearHeadingInterpolation(Math.toRadians(90.0), Math.toRadians(112.0)) - .build(); - - - fun Path2(pedro: PedroDrive) = - pedro.follower - .pathBuilder() - .addPath( - BezierCurve( - Pose(56.000, 12.000), - Pose(56.000, 36.000), - Pose(44.000, 36.000) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(112.0), Math.toRadians(0.0)) - .build(); - - fun Path3(pedro: PedroDrive, uppies: Uppies) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(44.000, 36.000), Pose(16.000, 36.000)) - ) - .setConstantHeadingInterpolation(Math.toRadians(Math.toRadians(0.0))) - .addParametricCallback(0.8) { - uppies.next() - } - .build(); - - fun Path4(pedro: PedroDrive, intake: Intake) = - pedro.follower - .pathBuilder() - .addPath( - BezierCurve( - Pose(16.000, 36.000), - Pose(16.000, 12.000), - Pose(56.000, 12.000) - ) - ) - .addParametricCallback(0.5) { - intake.power = -0.1 - } - .setLinearHeadingInterpolation(Math.toRadians(0.0), Math.toRadians(108.0)) - .build(); - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.MANUAL } - - ManualManager.init() - - val pedro = PedroDrive(this, Pose2d(56.0, 8.0, 90.0)) - val intake = Intake(this) - val flyWheel = FlyWheel(this) - val uppies = Uppies(this, intake, flyWheel) - - intake.locked = true // get rid of gamepad control - - val path1 = Path1(pedro, intake) - val path2 = Path2(pedro) - val path3 = Path3(pedro,uppies) - val path4 = Path4(pedro, intake) - - telemetry.isAutoClear = true - - uppies.next() - scheduler.schedule(uppies.command) - - waitForStart() - - scheduler.schedule(pedro.command) - scheduler.schedule(intake.command) - scheduler.schedule(flyWheel.command) - - scheduler.schedule(Sequential { -// Command { flyWheel.running = true } -// Command { uppies.autoFireCommand } - +FollowPathCommand(pedro.follower, path1) - +uppies.autoFireCommand - +FollowPathCommand(pedro.follower, path2) - Command { intake.locked = true; intake.power = -1.0 } - +FollowPathCommand(pedro.follower, path3) - +FollowPathCommand(pedro.follower, path4) - Command { intake.power = 0.1; SleepFor(250)} - +uppies.autoFireCommand - // you can also use integrate stuff by manually calling the stuff - // rather than pedro callbacks. the follow path command is quite simple. - }) - - while (opModeIsActive() && !isStopRequested) { - } - scheduler.runner.cancel() - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueShortAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueShortAuton.kt deleted file mode 100644 index b0a990ee8075..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/BlueShortAuton.kt +++ /dev/null @@ -1,150 +0,0 @@ -package org.firstinspires.ftc.teamcode.auton - -import com.millburnx.cmdx.commandGroups.Sequential -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.pedropathing.geometry.BezierCurve -import com.pedropathing.geometry.BezierLine -import com.pedropathing.geometry.Pose -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.commands.FollowPathCommand -import org.firstinspires.ftc.teamcode.subsystems.FlyWheel -import org.firstinspires.ftc.teamcode.subsystems.Intake -import org.firstinspires.ftc.teamcode.subsystems.PedroDrive -import org.firstinspires.ftc.teamcode.subsystems.Uppies -import org.firstinspires.ftc.teamcode.util.ManualManager -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.SleepFor - - -@Autonomous(name = "Blue Short Auton", preselectTeleOp = "Teleop") -class BlueShortAuton : LinearOpMode() { - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - timer.reset() - - telemetry.addData("hz", loopHertz) - telemetry.addData("cmd", this.runner.commandList.joinToString(", ")) - telemetry.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - - var hubs: List = emptyList() - - fun Path1(pedro: PedroDrive, intake: Intake) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(28.000, 133.000), Pose(50.000, 110.000)) - ) - .setLinearHeadingInterpolation(Math.toRadians(145.0), Math.toRadians(145.0)) - .build(); - - - fun Path2(pedro: PedroDrive, intake: Intake) = - pedro.follower - .pathBuilder() - .addPath( - BezierCurve( - Pose(50.000, 110.000), - Pose(60.000, 98.000), - Pose(45.000, 92.000) - ) - ) - .addParametricCallback(0.5) { - intake.locked = true; intake.power = -1.0 - } - .setLinearHeadingInterpolation(Math.toRadians(135.0), Math.toRadians(0.0)) - .build(); - - fun Path3(pedro: PedroDrive, uppies: Uppies) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(45.000, 92.000), Pose(20.000, 92.000)) - ) - .setConstantHeadingInterpolation(Math.toRadians(0.0)) - .addParametricCallback(1.0) { - uppies.next() - } - .build(); - - fun Path4(pedro: PedroDrive) = - pedro.follower - .pathBuilder() - .addPath( - BezierCurve( - Pose(20.000, 92.000), - Pose(40.000, 92.000), - Pose(50.000, 110.000) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0.0), Math.toRadians(135.0)) - .build(); - - fun Path5(pedro: PedroDrive) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(40.000, 110.000), Pose(60.000, 130.000)) - ) - .setLinearHeadingInterpolation(Math.toRadians(135.0), Math.toRadians(90.0)) - .build(); - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.MANUAL } - - ManualManager.init() - - val pedro = PedroDrive(this, Pose2d(28.0, 133.0, 145.0)) - val intake = Intake(this) - val flyWheel = FlyWheel(this) - val uppies = Uppies(this, intake, flyWheel) - - intake.locked = true // get rid of gamepad control - - val path1 = Path1(pedro, intake) - val path2 = Path2(pedro, intake) - val path3 = Path3(pedro,uppies) - val path4 = Path4(pedro) - val path5 = Path5(pedro) - - telemetry.isAutoClear = true - - uppies.next() - scheduler.schedule(uppies.command) - - waitForStart() - - scheduler.schedule(pedro.command) - scheduler.schedule(intake.command) - scheduler.schedule(flyWheel.command) - flyWheel.power = FlyWheel.ClosePower - - scheduler.schedule(Sequential { -// Command { flyWheel.running = true } -// Command { uppies.autoFireCommand } - +FollowPathCommand(pedro.follower, path1) - +uppies.autoFireCommand - +FollowPathCommand(pedro.follower, path2) -// Command { intake.locked = true; intake.power = -1.0 } - +FollowPathCommand(pedro.follower, path3) - Command { SleepFor (1000) } - +FollowPathCommand(pedro.follower, path4) - +uppies.autoFireCommand - +FollowPathCommand(pedro.follower, path5) - }) - - while (opModeIsActive() && !isStopRequested) { - } - scheduler.runner.cancel() - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/RedShortAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/RedShortAuton.kt deleted file mode 100644 index 3c0e5706cc67..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/RedShortAuton.kt +++ /dev/null @@ -1,147 +0,0 @@ -package org.firstinspires.ftc.teamcode.auton - -import com.millburnx.cmdx.commandGroups.Sequential -import com.millburnx.cmdx.runtimeGroups.CommandScheduler -import com.pedropathing.geometry.BezierCurve -import com.pedropathing.geometry.BezierLine -import com.pedropathing.geometry.Pose -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.commands.FollowPathCommand -import org.firstinspires.ftc.teamcode.subsystems.FlyWheel -import org.firstinspires.ftc.teamcode.subsystems.Intake -import org.firstinspires.ftc.teamcode.subsystems.PedroDrive -import org.firstinspires.ftc.teamcode.subsystems.Uppies -import org.firstinspires.ftc.teamcode.util.ManualManager -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.SleepFor - - -@Autonomous(name = "Red Short Auton", preselectTeleOp = "Teleop") -class RedShortAuton : LinearOpMode() { - val timer = ElapsedTime() - val scheduler = CommandScheduler().apply { - onSync = { - val loopHertz = 1.0 / timer.seconds() - timer.reset() - - telemetry.addData("hz", loopHertz) - telemetry.addData("cmd", this.runner.commandList.joinToString(", ")) - telemetry.update() - - hubs.forEach { it.clearBulkCache() } - ManualManager.update() - } - } - - var hubs: List = emptyList() - - fun Path1(pedro: PedroDrive, intake: Intake) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(144.0 - 28.000, 133.000), Pose(144.0 - 50.000, 110.000)) - ) - .setLinearHeadingInterpolation(Math.toRadians(35.0), Math.toRadians(35.0)) - .build(); - - - fun Path2(pedro: PedroDrive) = - pedro.follower - .pathBuilder() - .addPath( - BezierCurve( - Pose(144.0 - 50.000, 110.000), - Pose(144.0 - 60.000, 98.000), - Pose(144.0 - 45.000, 92.000) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(45.0), Math.toRadians(180.0)) - .build(); - - fun Path3(pedro: PedroDrive, uppies: Uppies) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(144.0 - 45.000, 92.000), Pose(144.0 - 20.000, 92.000)) - ) - .setConstantHeadingInterpolation(Math.toRadians(180.0)) - .addParametricCallback(1.0) { - uppies.next() - } - .build(); - - fun Path4(pedro: PedroDrive) = - pedro.follower - .pathBuilder() - .addPath( - BezierCurve( - Pose(144.0 - 20.000, 92.000), - Pose(144.0 - 40.000, 92.000), - Pose(144.0 - 50.000, 110.000) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(180.0), Math.toRadians(45.0)) - .build(); - - fun Path5(pedro: PedroDrive) = - pedro.follower - .pathBuilder() - .addPath( - BezierLine(Pose(144.0 - 40.000, 110.000), Pose(144.0 - 60.000, 130.000)) - ) - .setLinearHeadingInterpolation(Math.toRadians(45.0), Math.toRadians(90.0)) - .build(); - - override fun runOpMode() { - hubs = hardwareMap.getAll(LynxModule::class.java) - hubs.forEach { it.bulkCachingMode = LynxModule.BulkCachingMode.MANUAL } - - ManualManager.init() - - val pedro = PedroDrive(this, Pose2d(144.0-28.0, 133.0, 35.0)) - val intake = Intake(this) - val flyWheel = FlyWheel(this) - val uppies = Uppies(this, intake, flyWheel) - - intake.locked = true // get rid of gamepad control - - val path1 = Path1(pedro, intake) - val path2 = Path2(pedro) - val path3 = Path3(pedro, uppies) - val path4 = Path4(pedro) - val path5 = Path5(pedro) - - telemetry.isAutoClear = true - - uppies.next() - scheduler.schedule(uppies.command) - - waitForStart() - - scheduler.schedule(pedro.command) - scheduler.schedule(intake.command) - scheduler.schedule(flyWheel.command) - flyWheel.power = FlyWheel.ClosePower - - scheduler.schedule(Sequential { -// Command { flyWheel.running = true } -// Command { uppies.autoFireCommand } - +FollowPathCommand(pedro.follower, path1) - +uppies.autoFireCommand - +FollowPathCommand(pedro.follower, path2) - Command { intake.locked = true; intake.power = -1.0 } - +FollowPathCommand(pedro.follower, path3) - Command { SleepFor(1000) } - +FollowPathCommand(pedro.follower, path4) - +uppies.autoFireCommand - +FollowPathCommand(pedro.follower, path5) - }) - - while (opModeIsActive() && !isStopRequested) { - } - scheduler.runner.cancel() - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowPath.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowPath.kt deleted file mode 100644 index 349af03e355b..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowPath.kt +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.teamcode.commands - -import com.millburnx.cmdx.Command -import com.pedropathing.follower.Follower -import com.pedropathing.paths.PathChain -import org.firstinspires.ftc.teamcode.util.WaitFor - -fun FollowPathCommand(follower: Follower, path: PathChain, name: String = "Follow Pedro Path"): Command { - return Command(name) { - FollowPath(follower, path) - } -} - -suspend fun Command.FollowPath(follower: Follower, path: PathChain) { - follower.followPath(path) - WaitFor { !follower.isBusy } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/GoBildaPinpointDriver.java deleted file mode 100644 index ffc55736b497..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/GoBildaPinpointDriver.java +++ /dev/null @@ -1,746 +0,0 @@ -/* MIT License - * Copyright (c) [2025] [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.common; - -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 org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; - -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), - FAULT_BAD_READ (1 << 5); - - 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; - } - if ((s & DeviceStatus.FAULT_BAD_READ.status) != 0){ - return DeviceStatus.FAULT_BAD_READ; - } - else { - return DeviceStatus.NOT_READY; - } - } - - /** - * Confirm that the number received is a number, and does not include a change above the threshold - * @param oldValue the reading from the previous cycle - * @param newValue the new reading - * @param threshold the maximum change between this reading and the previous one - * @param bulkUpdate true if we are updating the loopTime variable. If not it should be false. - * @return newValue if the position is good, oldValue otherwise - */ - private Float isPositionCorrupt(float oldValue, float newValue, int threshold, boolean bulkUpdate){ - boolean noData = bulkUpdate && (loopTime < 1); - - boolean isCorrupt = noData || Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; - - if(!isCorrupt){ - return newValue; - } - - deviceStatus = DeviceStatus.FAULT_BAD_READ.status; - return oldValue; - } - - /** - * Confirm that the number received is a number, and does not include a change above the threshold - * @param oldValue the reading from the previous cycle - * @param newValue the new reading - * @param threshold the velocity allowed to be reported - * @return newValue if the velocity is good, oldValue otherwise - */ - private Float isVelocityCorrupt(float oldValue, float newValue, int threshold){ - boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue) > threshold; - boolean noData = (loopTime <= 1); - - if(!isCorrupt){ - return newValue; - } - - deviceStatus = DeviceStatus.FAULT_BAD_READ.status; - return oldValue; - } - - /** - * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. - */ - public void update(){ - final int positionThreshold = 5000; //more than one FTC field in mm - final int headingThreshold = 120; //About 20 full rotations in Radians - final int velocityThreshold = 10000; //10k mm/sec is faster than an FTC robot should be going... - final int headingVelocityThreshold = 120; //About 20 rotations per second - - float oldPosX = xPosition; - float oldPosY = yPosition; - float oldPosH = hOrientation; - float oldVelX = xVelocity; - float oldVelY = yVelocity; - float oldVelH = hVelocity; - - 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); - - /* - * Check to see if any of the floats we have received from the device are NaN or are too large - * if they are, we return the previously read value and alert the user via the DeviceStatus Enum. - */ - xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold, true); - yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold, true); - hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, true); - xVelocity = isVelocityCorrupt(oldVelX, xVelocity, velocityThreshold); - yVelocity = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold); - hVelocity = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold); - - } - - /** - * 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) { - final int headingThreshold = 120; - - float oldPosH = hOrientation; - - hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); - - hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, false); - - if (deviceStatus == DeviceStatus.FAULT_BAD_READ.status){ - deviceStatus = DeviceStatus.READY.status; - } - } - } - - /** - * 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 - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public void setOffsets(double xOffset, double yOffset){ - writeFloat(Register.X_POD_OFFSET, (float) xOffset); - writeFloat(Register.Y_POD_OFFSET, (float) yOffset); - } - - /** - * 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 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 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 - * @param distanceUnit the unit of distance used for offsets. - */ - public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit){ - writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset)); - writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(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 - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public void setEncoderResolution(double ticks_per_mm){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,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_unit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 - * @param distanceUnit unit used for distance - */ - public void setEncoderResolution(double ticks_per_unit, DistanceUnit distanceUnit){ - double resolution = distanceUnit.toMm(ticks_per_unit); - writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) resolution,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; - } - - /** - * 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. - * @param posX the new X position you'd like the Pinpoint to track your robot relive to. - * @param distanceUnit the unit for posX - */ - public void setPosX(double posX, DistanceUnit distanceUnit){ - writeByteArray(Register.X_POSITION,(floatToByteArray((float) distanceUnit.toMm(posX), 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. - * @param posY the new Y position you'd like the Pinpoint to track your robot relive to. - * @param distanceUnit the unit for posY - */ - public void setPosY(double posY, DistanceUnit distanceUnit){ - writeByteArray(Register.Y_POSITION,(floatToByteArray((float) distanceUnit.toMm(posY), ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Send a heading 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. - * @param heading the new heading you'd like the Pinpoint to track your robot relive to. - * @param angleUnit Radians or Degrees - */ - public void setHeading(double heading, AngleUnit angleUnit){ - writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) angleUnit.toRadians(heading), ByteOrder.LITTLE_ENDIAN))); - } - - /** - * 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); } - - /** - * @return a scalar that the IMU measured heading is multiplied by. This is tuned for each unit - * and should not need adjusted. - */ - 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
- * FAULT_BAD_READ - The Java code has detected a bad I²C read, the result reported is a - * duplicate of the last good read. - */ - 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 - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public double getPosX(){ - return xPosition; - } - - /** - * @return the estimated X (forward) position of the robot in specified unit - * @param distanceUnit the unit that the estimated position will return in - */ - public double getPosX(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(xPosition); - } - - /** - * @return the estimated Y (Strafe) position of the robot in mm - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public double getPosY(){ - return yPosition; - } - - /** - * @return the estimated Y (Strafe) position of the robot in specified unit - * @param distanceUnit the unit that the estimated position will return in - */ - public double getPosY(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(yPosition); - } - - /** - * @return the unnormalized estimated H (heading) position of the robot in radians - * unnormalized heading is not constrained from -180° to 180°. It will continue counting multiple rotations. - * @deprecated two overflows for this function exist with AngleUnit parameter. These minimize the possibility of unit confusion. - */ - @Deprecated - public double getHeading(){ - return hOrientation; - } - - /** - * @return the normalized estimated H (heading) position of the robot in specified unit - * normalized heading is wrapped from -180°, to 180°. - */ - public double getHeading(AngleUnit angleUnit){ - return angleUnit.fromRadians((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI; - } - - /** - * @return the unnormalized estimated H (heading) position of the robot in specified unit - * unnormalized heading is not constrained from -180° to 180°. It will continue counting - * multiple rotations. - */ - public double getHeading(UnnormalizedAngleUnit unnormalizedAngleUnit){ - return unnormalizedAngleUnit.fromRadians(hOrientation); - } - - /** - * @return the estimated X (forward) velocity of the robot in mm/sec - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public double getVelX(){ - return xVelocity; - } - - /** - * @return the estimated X (forward) velocity of the robot in specified unit/sec - */ - public double getVelX(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(xVelocity); - } - - /** - * @return the estimated Y (strafe) velocity of the robot in mm/sec - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public double getVelY(){ - return yVelocity; - } - - /** - * @return the estimated Y (strafe) velocity of the robot in specified unit/sec - */ - public double getVelY(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(yVelocity); - } - - /** - * @return the estimated H (heading) velocity of the robot in radians/sec - * @deprecated The overflow for this function has an AngleUnit, which can reduce the chance of unit confusion. - */ - @Deprecated - public double getHeadingVelocity() { - return hVelocity; - } - - /** - * @return the estimated H (heading) velocity of the robot in specified unit/sec - */ - public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit){ - return unnormalizedAngleUnit.fromRadians(hVelocity); - } - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the X (forward) pod in specified unit - */ - public float getXOffset(DistanceUnit distanceUnit){ - return (float) distanceUnit.fromMm(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(DistanceUnit distanceUnit){ - return (float) distanceUnit.fromMm(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, - //this wraps the hOrientation variable from -180° to +180° - ((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); - } - - /** - * @deprecated This function is not recommended, as velocity is wrapped from -180° to 180°. - * instead use individual getters. - * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second - */ - @Deprecated - public Pose2D getVelocity(){ - return new Pose2D(DistanceUnit.MM, - xVelocity, - yVelocity, - AngleUnit.RADIANS, - ((hVelocity + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/TeleopStopper.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/TeleopStopper.kt new file mode 100644 index 000000000000..168ac4c4d252 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/TeleopStopper.kt @@ -0,0 +1,13 @@ +package org.firstinspires.ftc.teamcode.common.commands + +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.SleepFor +import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.opmode.OpMode + +// Automatically ends teleop after 2 minutes +fun TeleOpStopper(opMode: OpMode) = Command("TeleOp Stopper") { + WaitFor { opMode.isStarted() || opMode.isStopRequested } + SleepFor(120 * 1000) + opMode.requestOpModeStop() +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/AxonCR.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/AxonCR.kt new file mode 100644 index 000000000000..b076cc05fdc5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/AxonCR.kt @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.common.hardware + +import com.qualcomm.robotcore.hardware.CRServo +import com.qualcomm.robotcore.hardware.DcMotorSimple +import com.qualcomm.robotcore.hardware.HardwareMap + +class AxonCR( + hardwareMap: HardwareMap, + name: String, + encoderName: String, + reverse: Boolean = false, + val encoderReverse: Boolean = false +) { + val servo: CRServo = hardwareMap.crservo[name].apply { + direction = if (reverse) DcMotorSimple.Direction.REVERSE else DcMotorSimple.Direction.FORWARD + } + var power + get() = servo.power + set(power) { + servo.power = power + } + + val encoder = hardwareMap.analogInput[encoderName] + var rawPosition: Double = 0.0 //kedaar wuz here + var rotations: Int = 0 //kedaar wuz also here + val position + get() = rotations + rawPosition + + fun updatePosition(): Double { + val oldPosition = rawPosition + val raw = encoder.voltage / 3.3 + if (encoderReverse) rawPosition = raw else rawPosition = 1 - raw + + val angleDifference: Double = rawPosition - oldPosition + val threshold = 0.5 + + // Handle wraparound at 0|1 boundary + if (angleDifference < -threshold) { + rotations++ // 1 to 0 + } else if (angleDifference > threshold) { + rotations-- // 0 to 1 + } + + return position + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/Util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/Util.kt new file mode 100644 index 000000000000..c26e1f5025eb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/Util.kt @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.common.hardware + +import com.qualcomm.robotcore.hardware.DcMotor +import com.qualcomm.robotcore.hardware.DcMotorEx +import com.qualcomm.robotcore.hardware.DcMotorSimple + +fun motorSetup(motor: DcMotorEx, reverse: Boolean = false, float: Boolean = false) { + motor.zeroPowerBehavior = if (float) DcMotor.ZeroPowerBehavior.FLOAT else DcMotor.ZeroPowerBehavior.BRAKE + motor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER + motor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS + motor.direction = if (reverse) DcMotorSimple.Direction.REVERSE else DcMotorSimple.Direction.FORWARD +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CachedAxon.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt similarity index 84% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CachedAxon.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt index 6166af879f3d..9a052963df94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CachedAxon.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt @@ -1,7 +1,7 @@ -package org.firstinspires.ftc.teamcode.util +package org.firstinspires.ftc.teamcode.common.hardware.cached import com.qualcomm.robotcore.hardware.HardwareMap -import org.firstinspires.ftc.teamcode.subsystems.AxonCR +import org.firstinspires.ftc.teamcode.common.hardware.AxonCR import kotlin.math.abs open class CachedAxon( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CachedMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CachedMotor.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt index 767a3bf11048..44f7ff211cf2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CachedMotor.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt @@ -1,9 +1,9 @@ -package org.firstinspires.ftc.teamcode.util +package org.firstinspires.ftc.teamcode.common.hardware.cached import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.DcMotorEx import com.qualcomm.robotcore.hardware.HardwareMap -import org.firstinspires.ftc.teamcode.subsystems.motorSetup +import org.firstinspires.ftc.teamcode.common.hardware.motorSetup import kotlin.math.abs open class CachedMotor( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualAxon.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt similarity index 85% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualAxon.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt index 93eee208d219..8efdc99dcadd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualAxon.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt @@ -1,6 +1,7 @@ -package org.firstinspires.ftc.teamcode.util +package org.firstinspires.ftc.teamcode.common.hardware.manual import com.qualcomm.robotcore.hardware.HardwareMap +import org.firstinspires.ftc.teamcode.common.hardware.cached.CachedAxon import kotlin.math.abs class ManualAxon( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualManager.kt similarity index 82% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualManager.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualManager.kt index 429ec4976fdd..55124b8762d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualManager.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util +package org.firstinspires.ftc.teamcode.common.hardware.manual object ManualManager { val motors = mutableListOf() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt similarity index 83% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualMotor.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt index 867f749ad0f1..70976e852485 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ManualMotor.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt @@ -1,6 +1,7 @@ -package org.firstinspires.ftc.teamcode.util +package org.firstinspires.ftc.teamcode.common.hardware.manual import com.qualcomm.robotcore.hardware.HardwareMap +import org.firstinspires.ftc.teamcode.common.hardware.cached.CachedMotor import kotlin.math.abs class ManualMotor( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt new file mode 100644 index 000000000000..44c562e3cc98 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt @@ -0,0 +1,15 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command + +open class Subsystem( + val name: String +) { + init { + SubsystemManager.subsystems.add(this) + } + open val run: suspend Command.() -> Unit = {} + open val cleanup: () -> Unit = {} + // by lazy to make sure it's created post-overrides + val command: Command by lazy { Command(name, cleanup, run) } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/SubsystemManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/SubsystemManager.kt new file mode 100644 index 000000000000..1389c782a207 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/SubsystemManager.kt @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.runtimeGroups.CommandScheduler + +object SubsystemManager { + val subsystems = mutableListOf() + + fun init() { + subsystems.clear() + } + + fun registerAll(scheduler: CommandScheduler) { + subsystems.forEach { subsystem -> + scheduler.schedule(subsystem.command) + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt new file mode 100644 index 000000000000..7dcae0a1af38 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.opmode + +import com.bylazar.gamepad.GamepadManager +import com.bylazar.gamepad.PanelsGamepad +import com.bylazar.telemetry.PanelsTelemetry +import com.millburnx.cmdx.runtimeGroups.CommandScheduler +import com.qualcomm.hardware.lynx.LynxModule +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +import com.qualcomm.robotcore.hardware.Gamepad +import com.qualcomm.robotcore.util.ElapsedTime +import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualManager +import org.firstinspires.ftc.teamcode.common.subsystem.SubsystemManager + +abstract class OpMode : LinearOpMode() { + val tel = PanelsTelemetry.telemetry + val gm1: GamepadManager = PanelsGamepad.firstManager + val gm2: GamepadManager = PanelsGamepad.secondManager + + val gp1: Gamepad + get() = gm1.asCombinedFTCGamepad(gamepad1) + val gp2: Gamepad + get() = gm2.asCombinedFTCGamepad(gamepad2) + + val loopTimer = ElapsedTime() + var hubs: List = emptyList() + val scheduler = CommandScheduler().apply { + onSync = { + val loopHertz = 1.0 / loopTimer.seconds() + loopTimer.reset() + + tel.addData("hz", loopHertz) + tel.update(telemetry) + + // hardware + hubs.forEach { it.clearBulkCache() } + ManualManager.update() + } + } + + // all code runs here, it is before the wait for start + // so for code that runs afterward, use a command with a WaitFor blocker + abstract fun run() + + override fun runOpMode() { + telemetry.isAutoClear = true + + SubsystemManager.init() + run() + SubsystemManager.registerAll(scheduler) + + waitForStart() + + @Suppress("ControlFlowWithEmptyBody") // Loop is to keep the active mode running + while (opModeIsActive()) { + } + scheduler.runner.cancel() // Clean up faulty commands + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt new file mode 100644 index 000000000000..f8994c14ae6b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.opmode.auton + +import org.firstinspires.ftc.teamcode.opmode.OpMode + +class SampleAuton: OpMode() { + override fun run() { + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt new file mode 100644 index 000000000000..66005b6ca8b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop + +import org.firstinspires.ftc.teamcode.common.commands.TeleOpStopper +import org.firstinspires.ftc.teamcode.opmode.OpMode + +class SampleTeleop: OpMode() { + override fun run() { + scheduler.schedule(TeleOpStopper(this)) + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt new file mode 100644 index 000000000000..14da4f034eed --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.pedro + +import com.pedropathing.follower.Follower +import com.pedropathing.follower.FollowerConstants +import com.pedropathing.ftc.FollowerBuilder +import com.pedropathing.ftc.drivetrains.MecanumConstants +import com.pedropathing.ftc.localization.constants.PinpointConstants +import com.pedropathing.paths.PathConstraints +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver.EncoderDirection +import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction +import com.qualcomm.robotcore.hardware.HardwareMap +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit +import kotlin.math.PI + +object Constants { + val followerConstants: FollowerConstants = FollowerConstants() + .mass(10.0) + .forwardZeroPowerAcceleration(-154.736) + .lateralZeroPowerAcceleration(-148.621) + .centripetalScaling(0.0) +// .translationalPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) +// .headingPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) +// .drivePIDFCoefficients(FilteredPIDFCoefficients(0.0, 0.0, 0.0, 0.6, 0.0)) +// .useSecondaryTranslationalPIDF(true) +// .useSecondaryHeadingPIDF(true) +// .useSecondaryDrivePIDF(true) +// .secondaryTranslationalPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) +// .secondaryHeadingPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) +// .secondaryDrivePIDFCoefficients(FilteredPIDFCoefficients(0.0, 0.0, 0.0, 0.0, 0.0)) + + fun MecanumConstants.setMotors() = apply { + rightFrontMotorName("m2") + rightRearMotorName("m3") + leftFrontMotorName("m0") + leftRearMotorName("m1") + leftFrontMotorDirection(Direction.REVERSE) + leftRearMotorDirection(Direction.REVERSE) + rightFrontMotorDirection(Direction.FORWARD) + rightRearMotorDirection(Direction.FORWARD) + } + + fun MecanumConstants.setPower() = apply { + maxPower(1.0) + xVelocity(147.58413) + yVelocity(131.245) + } + + val driveConstants: MecanumConstants = MecanumConstants() + .setMotors() + .setPower() + + val localizerConstants: PinpointConstants = PinpointConstants() + .hardwareMapName("pinpoint") + .distanceUnit(DistanceUnit.INCH) + .customEncoderResolution(OpenOdo.ENCODER_RESOLUTION) + .forwardPodY(-6.375) + .strafePodX(1.44) + .forwardEncoderDirection(EncoderDirection.REVERSED) + .strafeEncoderDirection(EncoderDirection.REVERSED) + + val pathConstraints: PathConstraints = PathConstraints(0.99, 100.0, 1.0, 1.0) + + fun createFollower(hardwareMap: HardwareMap): Follower { + return FollowerBuilder(followerConstants, hardwareMap) + .mecanumDrivetrain(driveConstants) + .pinpointLocalizer(localizerConstants) + .pathConstraints(pathConstraints) + .build() + } +} + + +object OpenOdo { + const val TICKS_PER_REVOLUTION = 8192.0 + const val DIAMETER_MM = 35.0 + const val ENCODER_RESOLUTION_MM = TICKS_PER_REVOLUTION / (DIAMETER_MM * PI) + const val ENCODER_RESOLUTION = ENCODER_RESOLUTION_MM / 25.4 +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md deleted file mode 100644 index 2a26e98fbd49..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ /dev/null @@ -1 +0,0 @@ -# Decode 2025-26 - MillburnX \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.kt deleted file mode 100644 index c38788a8c718..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.kt +++ /dev/null @@ -1,44 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.util.ManualMotor -import org.firstinspires.ftc.teamcode.util.Pose2d -import kotlin.math.abs -import kotlin.math.max - -class Drivetrain(opMode: LinearOpMode) : Subsystem("Intake") { - val fl = ManualMotor(opMode.hardwareMap, "m3", reverse = true) - val fr = ManualMotor(opMode.hardwareMap, "m1", reverse = false) - val br = ManualMotor(opMode.hardwareMap, "m0", reverse = false) - val bl = ManualMotor(opMode.hardwareMap, "m2", reverse = true) - - fun drive(direction: Pose2d) { - val denominator = max(abs(direction.y) + abs(direction.x) + abs(direction.heading), 1.0) - fl.power = (direction.y + direction.x + direction.heading) / denominator - fr.power = (direction.y - direction.x - direction.heading) / denominator - br.power = (direction.y + direction.x - direction.heading) / denominator - bl.power = (direction.y - direction.x + direction.heading) / denominator - } - override val run: suspend Command.() -> Unit = { - with(opMode) { - while (opModeIsActive() && !isStopRequested) { - val y = -gamepad1.left_stick_y.toDouble() - val x = gamepad1.left_stick_x * 1.1 - val rx = gamepad1.right_stick_x.toDouble() - -// val denominator = max(abs(y) + abs(x) + abs(rx), 1.0) -// fl.power = (y + x + rx) / denominator -// fr.power = (y - x - rx) / denominator -// br.power = (y + x - rx) / denominator -// bl.power = (y - x + rx) / denominator - - drive(Pose2d(x,y,rx)) - - sync() - } - } - } - - override val command = Command(this.name, cleanup, run) -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FlyWheel.kt deleted file mode 100644 index 56261dc9ae03..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FlyWheel.kt +++ /dev/null @@ -1,59 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.bylazar.configurables.annotations.Configurable -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.qualcomm.robotcore.hardware.DcMotorSimple -import org.firstinspires.ftc.teamcode.util.ManualMotor - -fun motorSetup(motor: DcMotorEx, reverse: Boolean = false, float: Boolean = false) { - motor.zeroPowerBehavior = if (float) DcMotor.ZeroPowerBehavior.FLOAT else DcMotor.ZeroPowerBehavior.BRAKE - motor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - motor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODERS - motor.direction = if (reverse) DcMotorSimple.Direction.REVERSE else DcMotorSimple.Direction.FORWARD -} - -@Configurable -class FlyWheel(opMode: LinearOpMode) : Subsystem("FlyWheel") { - val left = ManualMotor(opMode.hardwareMap, "m2e", reverse = true) - val right = ManualMotor(opMode.hardwareMap, "m1e", reverse = true) - - var running = false - var power = FarPower; - - override val run: suspend Command.() -> Unit = { - with(opMode) { - var wasDown = gamepad1.x - while (opModeIsActive() && !isStopRequested) { - val isDown = gamepad1.x - if (isDown != wasDown && isDown) { - running = !running - } - wasDown = isDown - if (running) { - left.power = power - right.power = power - } else { - left.power = 0.0 - right.power = 0.0 - } - opMode.telemetry.addData("fw running", running) - opMode.telemetry.addData("fw power", power) - sync() - } - } - } - - override val command = Command(this.name, cleanup, run) - - companion object { - @JvmField - var FarPower: Double = 1.0 - @JvmField - var TeleopClosePower: Double = 0.71 - @JvmField - var ClosePower: Double = .675 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.kt deleted file mode 100644 index c22a51810e12..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.kt +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.util.ManualMotor - -class Intake(opMode: LinearOpMode) : Subsystem("Intake") { - val motor = ManualMotor(opMode.hardwareMap, "m3e", reverse = true) - var power: Double = 0.0 - var locked: Boolean = false - - override val run: suspend Command.() -> Unit = { - with (opMode) { - while (opModeIsActive() && !isStopRequested) { - if (!locked) { - power = -(gamepad1.right_trigger - gamepad1.left_trigger).toDouble() - } - - motor.power = power - sync() - } - } - } - - override val command = Command(this.name,cleanup,run) -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odom.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odom.kt deleted file mode 100644 index e5e2ce7f077d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odom.kt +++ /dev/null @@ -1,44 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.millburnx.cmdx.Command -import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit -import org.firstinspires.ftc.teamcode.util.Pose2d - -class Odom(opMode: LinearOpMode) : Subsystem("Odom") { - val odom = opMode.hardwareMap["odom"] as GoBildaPinpointDriver - - init { - val diameterMM = 35.0 - val ticksPerRevolution = 8192.0 - val circumferenceMM = diameterMM * Math.PI - val ticksPerMM = ticksPerRevolution / circumferenceMM - odom.setEncoderResolution(ticksPerMM, DistanceUnit.MM) - odom.setOffsets(5.75, -0.75, DistanceUnit.INCH) - odom.setEncoderDirections( - GoBildaPinpointDriver.EncoderDirection.FORWARD, - GoBildaPinpointDriver.EncoderDirection.REVERSED, - ) - odom.resetPosAndIMU() - } - - val pose: Pose2d - get() = Pose2d( - odom.position.getX(DistanceUnit.INCH), - odom.position.getY(DistanceUnit.INCH), - odom.position.getHeading(AngleUnit.DEGREES) - ) - override val run: suspend Command.() -> Unit = { - with(opMode) { - while (opModeIsActive() && !isStopRequested) { - odom.update() - - telemetry.addData("Pose", pose) - } - } - } - - override val command = Command(this.name, cleanup, run) -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PedroDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PedroDrive.kt deleted file mode 100644 index aacb8d273dda..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PedroDrive.kt +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.Pedro.Constants -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.toPedro - -class PedroDrive(opMode: LinearOpMode, startingPose: Pose2d = Pose2d()) : Subsystem("Pedro Drive") { - - val follower = Constants.createFollower(opMode.hardwareMap).apply { - setStartingPose(startingPose.toPedro()) - } - - override val run: suspend Command.() -> Unit = { - with(opMode) { - while (opModeIsActive() && !isStopRequested) { - follower.update() - sync() - } - } - } - - override val command = Command(this.name, cleanup, run) -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.kt deleted file mode 100644 index 5d9ad5dc465d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.kt +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.millburnx.cmdx.Command - -abstract class Subsystem(val name: String) { - open fun init() {} - open val run: suspend Command.() -> Unit = {} - open val cleanup: () -> Unit = {} - abstract val command: Command -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Uppies.kt deleted file mode 100644 index 63d9f826f307..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Uppies.kt +++ /dev/null @@ -1,334 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import com.arcrobotics.ftclib.controller.PIDController -import com.bylazar.configurables.annotations.Configurable -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.hardware.CRServo -import com.qualcomm.robotcore.hardware.DcMotorSimple -import com.qualcomm.robotcore.hardware.HardwareMap -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.util.ManualAxon -import org.firstinspires.ftc.teamcode.util.SleepFor -import org.firstinspires.ftc.teamcode.util.WaitFor - -class AxonCR( - hardwareMap: HardwareMap, - name: String, - encoderName: String, - reverse: Boolean = false, - val encoderReverse: Boolean = false -) { - val servo: CRServo = hardwareMap.crservo[name].apply { - direction = if (reverse) DcMotorSimple.Direction.REVERSE else DcMotorSimple.Direction.FORWARD - } - var power - get() = servo.power - set(power) { - servo.power = power - } - - val encoder = hardwareMap.analogInput[encoderName] - var rawPosition: Double = 0.0 //kedaar wuz here - var rotations: Int = 0 //kedaar wuz also here - val position - get() = rotations + rawPosition - - fun updatePosition(): Double { - val oldPosition = rawPosition - val raw = encoder.voltage / 3.3 - if (encoderReverse) rawPosition = raw else rawPosition = 1 - raw - - val angleDifference: Double = rawPosition - oldPosition - val threshold = 0.5 - - // Handle wraparound at 0|1 boundary - if (angleDifference < -threshold) { - rotations++ // 1 to 0 - } else if (angleDifference > threshold) { - rotations-- // 0 to 1 - } - - return position - } -} - -@Configurable -class Uppies(opMode: LinearOpMode, intake: Intake, flyWheel: FlyWheel) : Subsystem("Uppies") { - val left = ManualAxon(opMode.hardwareMap, "s0", "a0", reverse = false, encoderReverse = false) - val right = ManualAxon(opMode.hardwareMap, "s1", "a1", reverse = true, encoderReverse = true) - - var leftState: Positions = Positions.OPEN; - var rightState: Positions = Positions.OPEN; - - var state: States = States.LEFT_SHOT - - val states = listOf( - States.LEFT_LOADED, - States.LEFT_SHOT, -// States.RIGHT_LOADED, -// States.RIGHT_SHOT, - ) - - val pidLeft = PIDController(kP, kI, kD) - val pidRight = PIDController(kP, kI, kD) - - var leftRotations = 0 - var rightRotations = 0 - - fun next() { - state = states[(states.indexOf(state) + 1) % states.size] - when (state) { - States.LEFT_LOADED -> leftState = Positions.LOADED - States.LEFT_SHOT -> { - leftRotations++ - leftState = Positions.OPEN - } - -// States.RIGHT_LOADED -> rightState = Positions.LOADED -// States.RIGHT_SHOT -> { -// rightRotations++ -// rightState = Positions.OPEN -// } - } - } - - fun prev() { - if (state == States.LEFT_LOADED) { - state = States.LEFT_SHOT - leftState = Positions.OPEN - rightState = Positions.OPEN - } -// if (state == States.RIGHT_LOADED) { -// state = States.LEFT_SHOT -// leftState = Positions.OPEN -// rightState = Positions.OPEN -// } - } - - val loadBall: suspend Command.() -> Unit = { - intake.power = -1.0 // run intake - SleepFor(intakeDuration) - next() // load 2 - SleepFor(250) - intake.power = -0.5 - WaitFor { atPosition() } - intake.power = 0.0 // stop intake - } - - val autoFireCommand = Command("auto-fire", { - println("uppies log | cancelling") - intake.power = 0.0 - intake.locked = false - flyWheel.running = false - }) { - intake.locked = true - flyWheel.running = true - intake.power = 0.0 - WaitFor { atPosition() } - SleepFor(flyWheelDuration) - next() // fire first ball - WaitFor { atPosition() } - println("uppies log | firing first ball | $state") - loadBall() - println("uppies log | load second ball | $state") - next() // fire 2 - WaitFor { atPosition() } - println("uppies log | firing second ball | $state") - loadBall() - println("uppies log | load third ball | $state") - next() // fire 3 - WaitFor { atPosition() } - println("uppies log | firing third ball | $state") - intake.locked = false - flyWheel.running = false - } - - val autoFireCommandTeleop = Command("auto-fire-teleop", { - println("uppies log | cancelling") - intake.power = 0.0 - intake.locked = false - flyWheel.running = false - }) { - intake.locked = true - flyWheel.running = true - intake.power = 0.0 - WaitFor { atPosition() } - SleepFor(flyWheelDuration) - next() // fire first ball - WaitFor { atPosition() } - println("uppies log | firing first ball | $state") - loadBall() - println("uppies log | load second ball | $state") - next() // fire 2 - WaitFor { atPosition() } - println("uppies log | firing second ball | $state") -// loadBall() -// println("uppies log | load third ball | $state") -// next() // fire 3 -// WaitFor { atPosition() } -// println("uppies log | firing third ball | $state") - intake.locked = false - flyWheel.running = false - } - - val loadBallNew: suspend Command.() -> Unit = { - intake.power = -1.0 // run intake - SleepFor(intakeDuration) - next() // load 2 - SleepFor(250) - intake.power = -0.5 - uppiesValidator() - intake.power = 0.0 // stop intake - } - - val uppiesValidator: suspend Command.() -> Unit = { - val elapsedTime = ElapsedTime() - while (!atPosition() && elapsedTime.milliseconds() < 3000) { - sync() - } - if (!atPosition()) { - // shit is stuck tspmo - prev() - intake.power = 0.5 - SleepFor(500) // reverse intake - // --- - intake.power = -1.0 // run intake - SleepFor(intakeDuration) - next() // load 2 - SleepFor(250) - intake.power = -0.5 - SleepFor(500) - intake.power = 0.0 - } - } - - val autoFireCommandNew = Command("auto-fire-new", { - println("uppies log | cancelling") - intake.power = 0.0 - intake.locked = false - flyWheel.running = false - }) { - intake.locked = true - flyWheel.running = true - intake.power = 0.0 - uppiesValidator() - SleepFor(flyWheelDuration) - next() // fire first ball - uppiesValidator() - println("uppies log | firing first ball | $state") - loadBallNew() - println("uppies log | load second ball | $state") - next() // fire 2 - uppiesValidator() - println("uppies log | firing second ball | $state") - loadBallNew() - println("uppies log | load third ball | $state") - next() // fire 3 - uppiesValidator() - println("uppies log | firing third ball | $state") - intake.locked = false - flyWheel.running = false - } - - fun atPosition(): Boolean { - val leftError = left.position - (leftRotations + leftState.getPosition(true)) - val rightError = right.position - (rightRotations + rightState.getPosition(false)) - - return leftError > threshold && rightError > threshold - } - - override val run: suspend Command.() -> Unit = { - with(opMode) { - var prevRevButton = gamepad1.left_bumper - var prevAdvButton = gamepad1.right_bumper - while (!isStopRequested) { - val newRevButton = gamepad1.left_bumper - val newAdvButton = gamepad1.right_bumper - if (!prevRevButton && newRevButton) { - prev() - } - if (!prevAdvButton && newAdvButton) { - next() - } - prevRevButton = newRevButton - prevAdvButton = newAdvButton - - val leftTarget = leftRotations + leftState.getPosition(true) - val rightTarget = rightRotations + rightState.getPosition(false) - - pidLeft.p = kP - pidLeft.i = kI - pidLeft.d = kD - - pidRight.p = kP - pidRight.i = kI - pidRight.d = kD - - left.power = pidLeft.calculate(left.position, leftTarget) - right.power = pidRight.calculate(right.position, rightTarget) - - telemetry.addData("state", state) - telemetry.addData("left state", leftState) - telemetry.addData("right state", rightState) - telemetry.addData("left target", leftTarget) - telemetry.addData("right target", rightTarget) - telemetry.addData("left raw pos", left.rawPosition) - telemetry.addData("right raw pos", right.rawPosition) - telemetry.addData("left pos", left.position) - telemetry.addData("right pos", right.position) - sync() - } - } - } - - override val command = Command(this.name, cleanup, run) - - enum class States { - LEFT_LOADED, - LEFT_SHOT, -// RIGHT_LOADED, -// RIGHT_SHOT - } - - enum class Positions { - OPEN, LOADED; - - fun getPosition(left: Boolean): Double = when (this) { - OPEN -> if (left) openLeft else openRight - LOADED -> if (left) loadedLeft else loadedRight - } - } - - companion object { - @JvmField - var kP = 2.0 - - @JvmField - var kI = 0.0 - - @JvmField - var kD = 0.05 - - @JvmField - var openLeft = 0.14 - - @JvmField - var openRight = 0.125 - - @JvmField - var loadedLeft = 0.43 - - @JvmField - var loadedRight = 0.4 - - @JvmField - var threshold = -0.05 - - @JvmField - var intakeDuration = 1500L - - @JvmField - var flyWheelDuration = 500L - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Vision.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Vision.kt deleted file mode 100644 index af66eb648b66..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Vision.kt +++ /dev/null @@ -1,70 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems - -import android.util.Size -import com.bylazar.configurables.annotations.Configurable -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName -import org.firstinspires.ftc.teamcode.util.Pose2d -import org.firstinspires.ftc.teamcode.util.Vec2d -import org.firstinspires.ftc.vision.VisionPortal -import org.firstinspires.ftc.vision.VisionProcessor -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection -import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor - -@Configurable -class Vision(opMode: LinearOpMode) : Subsystem("Vision") { - val aprilTagProcessor = AprilTagProcessor.Builder() - .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) - .build() - val processors = listOf( - aprilTagProcessor - ) - - val camera1: VisionPortal = - VisionPortal - .Builder() - .setCamera(opMode.hardwareMap["cam1"] as WebcamName) - .addProcessors(*processors.toTypedArray()) - .setCameraResolution(Size(cameraSizeX.toInt(), cameraSizeY.toInt())) - .setStreamFormat(VisionPortal.StreamFormat.MJPEG) - .enableLiveView(true) - .setAutoStopLiveView(true) - .build() - - var targetPose: Pose2d? = null - var angleError: Double = 0.0 - var aprilTags = listOf() - - override val run: suspend Command.() -> Unit = { - with(opMode) { - while (opModeIsActive() && !isStopRequested) { - aprilTags = aprilTagProcessor.detections - telemetry.addData("april tags", aprilTags.size) - val firstTag = aprilTags.firstOrNull() - if (firstTag != null) { - telemetry.addData("dist", firstTag.ftcPose.range) - telemetry.addData("heading", -firstTag.ftcPose.bearing) - - val pose = Pose2d() // replace with odom - val absoluteHeading = pose.radians + Math.toRadians(-firstTag.ftcPose.bearing) - angleError = -firstTag.ftcPose.bearing - targetPose = pose + Vec2d(firstTag.ftcPose.range, 0).rotate(absoluteHeading) - } - telemetry.update() - sync() - } - } - } - - override val command = Command(this.name, cleanup, run) - - companion object { - @JvmField - var cameraSizeX = 1280 - - @JvmField - var cameraSizeY = 720 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/APIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/APIDFController.java deleted file mode 100644 index bbc3800d3275..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/APIDFController.java +++ /dev/null @@ -1,258 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -import static org.firstinspires.ftc.teamcode.util.MiscKt.normalizeDegrees; - -public class APIDFController { - - private double kP, kI, kD, kF; - private double setPoint; - private double measuredValue; - private double minIntegral, maxIntegral; - - private double errorVal_p; - private double errorVal_v; - - private double totalError; - private double prevErrorVal; - - private double errorTolerance_p = 0.05; - private double errorTolerance_v = Double.POSITIVE_INFINITY; - - private double lastTimeStamp; - private double period; - - /** - * The base constructor for the PIDF controller - */ - public APIDFController(double kp, double ki, double kd, double kf) { - this(kp, ki, kd, kf, 0, 0); - } - - /** - * This is the full constructor for the PIDF controller. Our PIDF controller - * includes a feed-forward value which is useful for fighting friction and gravity. - * Our errorVal represents the return of e(t) and prevErrorVal is the previous error. - * - * @param sp The setpoint of the pid control loop. - * @param pv The measured value of he pid control loop. We want sp = pv, or to the degree - * such that sp - pv, or e(t) < tolerance. - */ - public APIDFController(double kp, double ki, double kd, double kf, double sp, double pv) { - kP = kp; - kI = ki; - kD = kd; - kF = kf; - - setPoint = sp; - measuredValue = pv; - - minIntegral = -1.0; - maxIntegral = 1.0; - - lastTimeStamp = 0; - period = 0; - - errorVal_p = normalizeDegrees(setPoint - measuredValue); - reset(); - } - - public void reset() { - totalError = 0; - prevErrorVal = 0; - lastTimeStamp = 0; - } - - /** - * Sets the error which is considered tolerable for use with {@link #atSetPoint()}. - * - * @param positionTolerance Position error which is tolerable. - */ - public void setTolerance(double positionTolerance) { - setTolerance(positionTolerance, Double.POSITIVE_INFINITY); - } - - /** - * Sets the error which is considered tolerable for use with {@link #atSetPoint()}. - * - * @param positionTolerance Position error which is tolerable. - * @param velocityTolerance Velocity error which is tolerable. - */ - public void setTolerance(double positionTolerance, double velocityTolerance) { - errorTolerance_p = positionTolerance; - errorTolerance_v = velocityTolerance; - } - - /** - * Returns the current setpoint of the PIDFController. - * - * @return The current setpoint. - */ - public double getSetPoint() { - return setPoint; - } - - /** - * Sets the setpoint for the PIDFController - * - * @param sp The desired setpoint. - */ - public void setSetPoint(double sp) { - setPoint = sp; - errorVal_p = normalizeDegrees(setPoint - measuredValue); - errorVal_v = (errorVal_p - prevErrorVal) / period; - } - - /** - * Returns true if the error is within the percentage of the total input range, determined by - * {@link #setTolerance}. - * - * @return Whether the error is within the acceptable bounds. - */ - public boolean atSetPoint() { - return Math.abs(errorVal_p) < errorTolerance_p - && Math.abs(errorVal_v) < errorTolerance_v; - } - - /** - * @return the PIDF coefficients - */ - public double[] getCoefficients() { - return new double[]{kP, kI, kD, kF}; - } - - /** - * @return the positional error e(t) - */ - public double getPositionError() { - return errorVal_p; - } - - /** - * @return the tolerances of the controller - */ - public double[] getTolerance() { - return new double[]{errorTolerance_p, errorTolerance_v}; - } - - /** - * @return the velocity error e'(t) - */ - public double getVelocityError() { - return errorVal_v; - } - - /** - * Calculates the next output of the PIDF controller. - * - * @return the next output using the current measured value via - * {@link #calculate(double)}. - */ - public double calculate() { - return calculate(measuredValue); - } - - /** - * Calculates the next output of the PIDF controller. - * - * @param pv The given measured value. - * @param sp The given setpoint. - * @return the next output using the given measurd value via - * {@link #calculate(double)}. - */ - public double calculate(double pv, double sp) { - // set the setpoint to the provided value - setSetPoint(sp); - return calculate(pv); - } - - /** - * Calculates the control value, u(t). - * - * @param pv The current measurement of the process variable. - * @return the value produced by u(t). - */ - public double calculate(double pv) { - prevErrorVal = errorVal_p; - - double currentTimeStamp = (double) System.nanoTime() / 1E9; - if (lastTimeStamp == 0) lastTimeStamp = currentTimeStamp; - period = currentTimeStamp - lastTimeStamp; - lastTimeStamp = currentTimeStamp; - - if (measuredValue == pv) { - errorVal_p = normalizeDegrees(setPoint - measuredValue); - } else { - errorVal_p = normalizeDegrees(setPoint - pv); - measuredValue = pv; - } - - if (Math.abs(period) > 1E-6) { - errorVal_v = (errorVal_p - prevErrorVal) / period; - } else { - errorVal_v = 0; - } - - /* - if total error is the integral from 0 to t of e(t')dt', and - e(t) = sp - pv, then the total error, E(t), equals sp*t - pv*t. - */ - totalError += period * (setPoint - measuredValue); - totalError = totalError < minIntegral ? minIntegral : Math.min(maxIntegral, totalError); - - // returns u(t) - return kP * errorVal_p + kI * totalError + kD * errorVal_v + kF * setPoint; - } - - public void setPIDF(double kp, double ki, double kd, double kf) { - kP = kp; - kI = ki; - kD = kd; - kF = kf; - } - - public void setIntegrationBounds(double integralMin, double integralMax) { - minIntegral = integralMin; - maxIntegral = integralMax; - } - - public void clearTotalError() { - totalError = 0; - } - - public void setP(double kp) { - kP = kp; - } - - public void setI(double ki) { - kI = ki; - } - - public void setD(double kd) { - kD = kd; - } - - public void setF(double kf) { - kF = kf; - } - - public double getP() { - return kP; - } - - public double getI() { - return kI; - } - - public double getD() { - return kD; - } - - public double getF() { - return kF; - } - - public double getPeriod() { - return period; - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CommandUtil.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CommandUtil.kt deleted file mode 100644 index f1f21de618c7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CommandUtil.kt +++ /dev/null @@ -1,15 +0,0 @@ -package org.firstinspires.ftc.teamcode.util - -import com.millburnx.cmdx.Command -import com.qualcomm.robotcore.util.ElapsedTime - -suspend fun Command.WaitFor(case: () -> Boolean) { - while (!case()) { - sync() - } -} - -suspend fun Command.SleepFor(ms: Long) { - val elapsedTime = ElapsedTime() - WaitFor { elapsedTime.milliseconds() >= ms } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Misc.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Misc.kt deleted file mode 100644 index 32a727f6cf7a..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Misc.kt +++ /dev/null @@ -1,13 +0,0 @@ -package org.firstinspires.ftc.teamcode.util - -import kotlin.math.floor - -fun normalizeRadians(radians: Double): Double { - val temp = (radians + Math.PI) / (2.0 * Math.PI) - return (temp - floor(temp) - 0.5) * 2.0 -} - -fun normalizeDegrees(angle: Double): Double = Math.toDegrees(normalizeRadians(Math.toRadians(angle))) - -fun Double.rad() = Math.toRadians(this) -fun Double.deg() = Math.toDegrees(this) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Pose2d.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Pose2d.kt deleted file mode 100644 index 93c3eae6093b..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Pose2d.kt +++ /dev/null @@ -1,84 +0,0 @@ -package org.firstinspires.ftc.teamcode.util - -import com.pedropathing.geometry.Pose -import kotlin.math.absoluteValue -import kotlin.math.sign - -data class Pose2d( - val position: Vec2d = Vec2d(), - val heading: Double = 0.0, -) { - constructor(x: Double, y: Double, heading: Double) : this(Vec2d(x, y), heading) - constructor(x: Double, y: Double) : this(Vec2d(x,y), 0.0) - constructor(values: Array) : this(values[0], values[1], values[2]) - - val x: Double - get() = position.x - val y: Double - get() = position.y - val degrees: Double - get() = heading - val radians: Double - get() = Math.toRadians(heading) - val rotation: Double - get() = heading - - companion object { - fun fromRadians( - position: Vec2d, - radians: Double, - ): Pose2d = Pose2d(position, Math.toDegrees(radians)) - - fun fromRadians( - x: Double, - y: Double, - radians: Double, - ): Pose2d = Pose2d(x, y, Math.toDegrees(radians)) - -// fun fromRR(pose: com.acmerobotics.roadrunner.geometry.Pose2d): Pose2d = Pose2d(pose.x, pose.y, Math.toDegrees(pose.heading)) - - fun fromRR(pose: Pose2d): Pose2d = Pose2d(pose.x, pose.y, Math.toDegrees(pose.heading)) - } - - fun toRR(): Pose2d = Pose2d(position.y, -position.x, heading) - -// fun toRR(): com.acmerobotics.roadrunner.geometry.Pose2d = -// com.acmerobotics.roadrunner.geometry -// .Pose2d(position.y, -position.x, Math.toRadians(heading)) -// -// fun toRawRR(): com.acmerobotics.roadrunner.geometry.Pose2d = -// com.acmerobotics.roadrunner.geometry -// .Pose2d(position.x, position.y, Math.toRadians(heading)) - - operator fun unaryMinus(): Pose2d = Pose2d(-position, -heading) - - fun flipPos(): Pose2d = Pose2d(-position, heading) - - fun abs(): Pose2d = Pose2d(position.abs(), heading.absoluteValue) - - operator fun plus(other: Pose2d): Pose2d = Pose2d(position + other.position, normalizeDegrees(heading + other.heading)) - - operator fun plus(other: Vec2d): Pose2d = Pose2d(position + other, heading) - - operator fun minus(other: Pose2d): Pose2d = Pose2d(position - other.position, normalizeDegrees(heading - other.heading)) - - operator fun minus(other: Vec2d): Pose2d = Pose2d(position - other, heading) - - operator fun times(other: Vec2d): Pose2d = Pose2d(position * other, heading) - - operator fun times(scalar: Double): Pose2d = Pose2d(position * scalar, heading) - - operator fun div(other: Vec2d): Pose2d = Pose2d(position / other, heading) - - operator fun div(scalar: Double): Pose2d = Pose2d(position / scalar, heading) - - fun distanceTo(pose: Pose2d) = distanceTo(pose.position) - - fun distanceTo(vec2d: Vec2d) = position.distanceTo(vec2d) - - fun sign() = Pose2d(position.sign(), heading.sign) -} - -fun Pose2d.toPedro(): Pose { - return Pose(x, y, radians) -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Vec2d.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Vec2d.kt deleted file mode 100644 index 43d1122cd631..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Vec2d.kt +++ /dev/null @@ -1,124 +0,0 @@ -package org.firstinspires.ftc.teamcode.util - -import kotlin.math.atan2 -import kotlin.math.cos -import kotlin.math.sin - -/** - * Represents a 2D vector/point - */ -data class Vec2d( - val x: Double = 0.0, - val y: Double = x, -) { - constructor(x: Number, y: Number) : this(x.toDouble(), y.toDouble()) - constructor(arr: Array) : this(arr[0].toDouble(), arr[1].toDouble()) - constructor(arr: Array) : this(arr[0], arr[1]) - - constructor(v: Double) : this(v, v) - constructor(v: Float) : this(v.toDouble()) - constructor(v: Int) : this(v.toDouble()) - - operator fun plus(other: Vec2d) = Vec2d(x + other.x, y + other.y) - - operator fun plus(other: Double) = Vec2d(x + other, y + other) - - operator fun plus(other: Float) = this + other.toDouble() - - operator fun plus(other: Int) = this + other.toDouble() - - operator fun minus(other: Vec2d) = Vec2d(x - other.x, y - other.y) - - operator fun minus(other: Double) = Vec2d(x - other, y - other) - - operator fun minus(other: Float) = this - other.toDouble() - - operator fun minus(other: Int) = this - other.toDouble() - - operator fun times(other: Vec2d) = Vec2d(x * other.x, y * other.y) - - operator fun times(other: Double) = Vec2d(x * other, y * other) - - operator fun times(other: Float) = this * other.toDouble() - - operator fun times(other: Int) = this * other.toDouble() - - operator fun div(other: Vec2d) = Vec2d(x / other.x, y / other.y) - - operator fun div(other: Double) = Vec2d(x / other, y / other) - - operator fun div(other: Float) = this / other.toDouble() - - operator fun div(other: Int) = this / other.toDouble() - - operator fun unaryMinus() = Vec2d(-x, -y) - - /** - * Returns the Euclidean distance to another point - */ - fun distanceTo(other: Vec2d): Double { - val xDiff = x - other.x - val yDiff = y - other.y - return kotlin.math.sqrt(xDiff * xDiff + yDiff * yDiff) - } - - fun magnituide(): Double = kotlin.math.sqrt(x * x + y * y) - - fun normalize(): Double = kotlin.math.sqrt(this.dot(this)) - - fun dot(other: Vec2d): Double = x * other.x + y * other.y - - /** - * Returns the angle to another point - */ - fun angleTo(other: Vec2d): Double = atan2(other.y - y, other.x - x) - - override fun toString(): String = "Point($x, $y)" - - /** - * Rotates the vector by an angle - */ - fun rotate(angle: Double): Vec2d { - val cos = cos(angle) - val sin = sin(angle) - return Vec2d(x * cos - y * sin, x * sin + y * cos) - } - - /** - * Linearly interpolates between two vectors/points - */ - fun lerp( - other: Vec2d, - t: Double, - ) = this + (other - this) * t - - fun lerp( - other: Vec2d, - t: Float, - ) = this.lerp(other, t.toDouble()) - - /** - * Returns a copy of the vector with the absolute value of each component - */ - fun abs() = Vec2d(kotlin.math.abs(x), kotlin.math.abs(y)) - - fun sqrt() = Vec2d(kotlin.math.sqrt(x), kotlin.math.sqrt(y)) - - fun sign() = Vec2d(kotlin.math.sign(x), kotlin.math.sign(y)) - - fun coerceIn( - min: Vec2d, - max: Vec2d, - ) = Vec2d(x.coerceIn(min.x, max.x), y.coerceIn(min.y, max.y)) - - fun coerceIn( - min: Double, - max: Double, - ) = coerceIn(Vec2d(min), Vec2d(max)) - - fun flip() = Vec2d(y, x) - - companion object { - fun fromAngle(radians: Double) = Vec2d(cos(radians), sin(radians)) - } -} \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index c24e30f80e20..a24668708e1e 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -16,6 +16,6 @@ dependencies { implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.pedropathing:ftc:2.0.3' implementation 'com.pedropathing:telemetry:1.0.0' - implementation 'com.bylazar:fullpanels:1.0.6' + implementation 'com.bylazar:fullpanels:1.0.9' } From 3b5e72a86939c17c836f4894f744b2d1d447442d Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Tue, 4 Nov 2025 10:27:36 -0500 Subject: [PATCH 02/29] Add Pedro Tuner --- .../ftc/teamcode/pedro/Tuning.kt | 1312 +++++++++++++++++ 1 file changed, 1312 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.kt new file mode 100644 index 000000000000..f12b1337160f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.kt @@ -0,0 +1,1312 @@ +package org.firstinspires.ftc.teamcode.pedro + +import com.bylazar.configurables.PanelsConfigurables.refreshClass +import com.bylazar.configurables.annotations.Configurable +import com.bylazar.configurables.annotations.IgnoreConfigurable +import com.bylazar.field.PanelsField.field +import com.bylazar.field.PanelsField.presets +import com.bylazar.field.Style +import com.bylazar.telemetry.PanelsTelemetry +import com.bylazar.telemetry.TelemetryManager +import com.pedropathing.follower.Follower +import com.pedropathing.geometry.BezierCurve +import com.pedropathing.geometry.BezierLine +import com.pedropathing.geometry.Pose +import com.pedropathing.math.Vector +import com.pedropathing.paths.HeadingInterpolator +import com.pedropathing.paths.Path +import com.pedropathing.paths.PathChain +import com.pedropathing.telemetry.SelectScope +import com.pedropathing.telemetry.SelectableOpMode +import com.pedropathing.util.PoseHistory +import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import org.firstinspires.ftc.teamcode.pedro.Constants.createFollower +import org.firstinspires.ftc.teamcode.pedro.Drawing.drawRobot +import java.util.function.Consumer +import java.util.function.Supplier +import kotlin.math.abs +import kotlin.math.pow + +/** + * This is the Tuning class. It contains a selection menu for various tuning OpModes. + * + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 6/26/2025 + */ +@Configurable +@TeleOp(name = "Tuning", group = "Pedro Pathing") +class Tuning : SelectableOpMode("Select a Tuning OpMode", Consumer { s: SelectScope?>? -> + s!!.folder("Localization") { l: SelectScope?>? -> + l!!.add("Localization Test", Supplier { LocalizationTest() }) + l.add("Forward Tuner", Supplier { ForwardTuner() }) + l.add("Lateral Tuner", Supplier { LateralTuner() }) + l.add("Turn Tuner", Supplier { TurnTuner() }) + } + s.folder("Automatic") { a: SelectScope?>? -> + a!!.add("Forward Velocity Tuner", Supplier { ForwardVelocityTuner() }) + a.add("Lateral Velocity Tuner", Supplier { LateralVelocityTuner() }) + a.add("Forward Zero Power Acceleration Tuner", Supplier { ForwardZeroPowerAccelerationTuner() }) + a.add("Lateral Zero Power Acceleration Tuner", Supplier { LateralZeroPowerAccelerationTuner() }) + } + s.folder("Manual") { p: SelectScope?>? -> + p!!.add("Translational Tuner", Supplier { TranslationalTuner() }) + p.add("Heading Tuner", Supplier { HeadingTuner() }) + p.add("Drive Tuner", Supplier { DriveTuner() }) + p.add("Centripetal Tuner", Supplier { CentripetalTuner() }) + } + s.folder("Tests") { p: SelectScope?>? -> + p!!.add("Line", Supplier { Line() }) + p.add("Triangle", Supplier { Triangle() }) + p.add("Circle", Supplier { Circle() }) + } +}) { + public override fun onSelect() { + if (follower == null) { + follower = createFollower(hardwareMap) + refreshClass(this) + } else { + follower = createFollower(hardwareMap) + } + + follower!!.setStartingPose(Pose()) + + poseHistory = follower!!.poseHistory + + telemetryM = PanelsTelemetry.telemetry + + Drawing.init() + } + + public override fun onLog(lines: MutableList?) {} + + companion object { + var follower: Follower? = null + + @IgnoreConfigurable + var poseHistory: PoseHistory? = null + + @IgnoreConfigurable + var telemetryM: TelemetryManager? = null + + @IgnoreConfigurable + var changes: ArrayList = ArrayList() + + fun drawOnlyCurrent() { + try { + drawRobot(follower!!.pose) + Drawing.sendPacket() + } catch (e: Exception) { + throw RuntimeException("Drawing failed $e") + } + } + + fun draw() { + Drawing.drawDebug(follower!!) + } + + /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ + fun stopRobot() { + follower!!.startTeleopDrive(true) + follower!!.setTeleOpDrive(0.0, 0.0, 0.0, true) + } + } +} + +/** + * 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. + * You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +internal class LocalizationTest : OpMode() { + override fun init() {} + + /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + override fun init_loop() { + Tuning.telemetryM!!.debug( + "This will print your robot's position to telemetry while " + "allowing robot control through a basic mecanum drive on gamepad 1." + ) + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun start() { + Tuning.follower!!.startTeleopDrive() + Tuning.follower!!.update() + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + override fun loop() { + Tuning.follower!!.setTeleOpDrive( + -gamepad1.left_stick_y.toDouble(), + -gamepad1.left_stick_x.toDouble(), + -gamepad1.right_stick_x.toDouble(), + true + ) + Tuning.follower!!.update() + + Tuning.telemetryM!!.debug("x:" + Tuning.follower!!.pose.x) + Tuning.telemetryM!!.debug("y:" + Tuning.follower!!.pose.y) + Tuning.telemetryM!!.debug("heading:" + Tuning.follower!!.pose.heading) + Tuning.telemetryM!!.debug("total heading:" + Tuning.follower!!.totalHeading) + Tuning.telemetryM!!.update(telemetry) + + Tuning.draw() + } +} + +/** + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +internal class ForwardTuner : OpMode() { + override fun init() { + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("Pull your robot forward $DISTANCE inches. Your forward ticks to inches will be shown on the telemetry.") + Tuning.telemetryM!!.update(telemetry) + Tuning.drawOnlyCurrent() + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + override fun loop() { + Tuning.follower!!.update() + + Tuning.telemetryM!!.debug("Distance Moved: " + Tuning.follower!!.pose.x) + Tuning.telemetryM!!.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to $DISTANCE inches.") + Tuning.telemetryM!!.debug( + "Multiplier: " + (DISTANCE / (Tuning.follower!!.pose.x / Tuning.follower!!.getPoseTracker().localizer.forwardMultiplier)) + ) + Tuning.telemetryM!!.update(telemetry) + + Tuning.draw() + } + + companion object { + var DISTANCE: Double = 48.0 + } +} + +/** + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 2.0, 6/26/2025 + */ +internal class LateralTuner : OpMode() { + override fun init() { + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("Pull your robot to the right $DISTANCE inches. Your strafe ticks to inches will be shown on the telemetry.") + Tuning.telemetryM!!.update(telemetry) + Tuning.drawOnlyCurrent() + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + override fun loop() { + Tuning.follower!!.update() + + Tuning.telemetryM!!.debug("Distance Moved: " + Tuning.follower!!.pose.y) + Tuning.telemetryM!!.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to $DISTANCE inches.") + Tuning.telemetryM!!.debug( + "Multiplier: " + (DISTANCE / (Tuning.follower!!.pose.y / Tuning.follower!!.getPoseTracker().localizer.lateralMultiplier)) + ) + Tuning.telemetryM!!.update(telemetry) + + Tuning.draw() + } + + companion object { + var DISTANCE: Double = 48.0 + } +} + +/** + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +internal class TurnTuner : OpMode() { + override fun init() { + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("Turn your robot $ANGLE radians. Your turn ticks to inches will be shown on the telemetry.") + Tuning.telemetryM!!.update(telemetry) + + Tuning.drawOnlyCurrent() + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + override fun loop() { + Tuning.follower!!.update() + + Tuning.telemetryM!!.debug("Total Angle: " + Tuning.follower!!.totalHeading) + Tuning.telemetryM!!.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to $ANGLE radians.") + Tuning.telemetryM!!.debug( + "Multiplier: " + (ANGLE / (Tuning.follower!!.totalHeading / Tuning.follower!!.getPoseTracker().localizer.turningMultiplier)) + ) + Tuning.telemetryM!!.update(telemetry) + + Tuning.draw() + } + + companion object { + var ANGLE: Double = 2 * Math.PI + } +} + +/** + * 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 + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +internal class ForwardVelocityTuner : OpMode() { + private val velocities = ArrayList() + private var end = false + + override fun init() {} + + /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("The robot will run at 1 power until it reaches $DISTANCE inches forward.") + Tuning.telemetryM!!.debug("Make sure you have enough room, since the robot has inertia after cutting power.") + Tuning.telemetryM!!.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.") + Tuning.telemetryM!!.debug("Press B on game pad 1 to stop.") + Tuning.telemetryM!!.debug("pose", Tuning.follower!!.pose) + Tuning.telemetryM!!.update(telemetry) + + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + override fun start() { + var i = 0 + while (i < RECORD_NUMBER) { + velocities.add(0.0) + i++ + } + Tuning.follower!!.startTeleopDrive(true) + Tuning.follower!!.update() + end = false + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B 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 fun loop() { + if (gamepad1.bWasPressed()) { + Tuning.stopRobot() + requestOpModeStop() + } + + Tuning.follower!!.update() + Tuning.draw() + + + if (!end) { + if (abs(Tuning.follower!!.pose.x) > DISTANCE) { + end = true + Tuning.stopRobot() + } else { + Tuning.follower!!.setTeleOpDrive(1.0, 0.0, 0.0, true) + //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); + val currentVelocity: Double = abs(Tuning.follower!!.poseTracker.localizer.velocity.x) + velocities.add(currentVelocity) + velocities.removeAt(0) + } + } else { + Tuning.stopRobot() + var average = 0.0 + for (velocity in velocities) { + average += velocity!! + } + average /= velocities.size.toDouble() + Tuning.telemetryM!!.debug("Forward Velocity: $average") + Tuning.telemetryM!!.debug("\n") + Tuning.telemetryM!!.debug("Press A to set the Forward Velocity temporarily (while robot remains on).") + + for (i in velocities.indices) { + telemetry.addData(i.toString(), velocities[i]) + } + + Tuning.telemetryM!!.update(telemetry) + telemetry.update() + + if (gamepad1.aWasPressed()) { + Tuning.follower!!.setXVelocity(average) + val message = "XMovement: $average" + Tuning.changes.add(message) + } + } + } + + companion object { + var DISTANCE: Double = 48.0 + var RECORD_NUMBER: Double = 10.0 + } +} + +/** + * 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 + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +internal class LateralVelocityTuner : OpMode() { + private val velocities = ArrayList() + + private var end = false + + override fun init() {} + + /** + * This initializes the drive motors as well as the cache of velocities and the Panels + * telemetryM. + */ + override fun init_loop() { + Tuning.telemetryM!!.debug("The robot will run at 1 power until it reaches $DISTANCE inches to the right.") + Tuning.telemetryM!!.debug("Make sure you have enough room, since the robot has inertia after cutting power.") + Tuning.telemetryM!!.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity.") + Tuning.telemetryM!!.debug("Press B on Gamepad 1 to stop.") + Tuning.telemetryM!!.update(telemetry) + + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This starts the OpMode by setting the drive motors to run right at full power. */ + override fun start() { + var i = 0 + while (i < RECORD_NUMBER) { + velocities.add(0.0) + i++ + } + Tuning.follower!!.startTeleopDrive(true) + Tuning.follower!!.update() + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B 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 fun loop() { + if (gamepad1.bWasPressed()) { + Tuning.stopRobot() + requestOpModeStop() + } + + Tuning.follower!!.update() + Tuning.draw() + + if (!end) { + if (abs(Tuning.follower!!.pose.y) > DISTANCE) { + end = true + Tuning.stopRobot() + } else { + Tuning.follower!!.setTeleOpDrive(0.0, 1.0, 0.0, true) + val currentVelocity: Double = abs(Tuning.follower!!.velocity.dot(Vector(1.0, Math.PI / 2))) + velocities.add(currentVelocity) + velocities.removeAt(0) + } + } else { + Tuning.stopRobot() + var average = 0.0 + for (velocity in velocities) { + average += velocity!! + } + average /= velocities.size.toDouble() + + Tuning.telemetryM!!.debug("Strafe Velocity: $average") + Tuning.telemetryM!!.debug("\n") + Tuning.telemetryM!!.debug("Press A to set the Lateral Velocity temporarily (while robot remains on).") + Tuning.telemetryM!!.update(telemetry) + + if (gamepad1.aWasPressed()) { + Tuning.follower!!.setYVelocity(average) + val message = "YMovement: $average" + Tuning.changes.add(message) + } + } + } + + companion object { + var DISTANCE: Double = 48.0 + var RECORD_NUMBER: Double = 10.0 + } +} + +/** + * 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 + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +internal class ForwardZeroPowerAccelerationTuner : OpMode() { + private val accelerations = ArrayList() + private var previousVelocity = 0.0 + private var previousTimeNano: Long = 0 + + private var stopping = false + private var end = false + + override fun init() {} + + /** This initializes the drive motors as well as the Panels telemetryM. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("The robot will run forward until it reaches $VELOCITY inches per second.") + Tuning.telemetryM!!.debug("Then, it will cut power from the drivetrain and roll to a stop.") + Tuning.telemetryM!!.debug("Make sure you have enough room.") + Tuning.telemetryM!!.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed.") + Tuning.telemetryM!!.debug("Press B on Gamepad 1 to stop.") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + override fun start() { + Tuning.follower!!.startTeleopDrive(false) + Tuning.follower!!.update() + Tuning.follower!!.setTeleOpDrive(1.0, 0.0, 0.0, true) + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B 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 fun loop() { + if (gamepad1.bWasPressed()) { + Tuning.stopRobot() + requestOpModeStop() + } + + Tuning.follower!!.update() + Tuning.draw() + + val heading = Vector(1.0, Tuning.follower!!.pose.heading) + if (!end) { + if (!stopping) { + if (Tuning.follower!!.velocity.dot(heading) > VELOCITY) { + previousVelocity = Tuning.follower!!.velocity.dot(heading) + previousTimeNano = System.nanoTime() + stopping = true + Tuning.follower!!.setTeleOpDrive(0.0, 0.0, 0.0, true) + } + } else { + val currentVelocity: Double = Tuning.follower!!.velocity.dot(heading) + accelerations.add( + (currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / 10.0.pow( + 9.0 + )) + ) + previousVelocity = currentVelocity + previousTimeNano = System.nanoTime() + if (currentVelocity < Tuning.follower!!.constraints.velocityConstraint) { + end = true + } + } + } else { + var average = 0.0 + for (acceleration in accelerations) { + average += acceleration!! + } + average /= accelerations.size.toDouble() + + Tuning.telemetryM!!.debug("Forward Zero Power Acceleration (Deceleration): $average") + Tuning.telemetryM!!.debug("\n") + Tuning.telemetryM!!.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on).") + Tuning.telemetryM!!.update(telemetry) + + if (gamepad1.aWasPressed()) { + Tuning.follower!!.getConstants().setForwardZeroPowerAcceleration(average) + val message = "Forward Zero Power Acceleration: $average" + Tuning.changes.add(message) + } + } + } + + companion object { + var VELOCITY: Double = 30.0 + } +} + +/** + * 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 + * 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. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +internal class LateralZeroPowerAccelerationTuner : OpMode() { + private val accelerations = ArrayList() + private var previousVelocity = 0.0 + private var previousTimeNano: Long = 0 + private var stopping = false + private var end = false + + override fun init() {} + + /** This initializes the drive motors as well as the Panels telemetry. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("The robot will run to the right until it reaches $VELOCITY inches per second.") + Tuning.telemetryM!!.debug("Then, it will cut power from the drivetrain and roll to a stop.") + Tuning.telemetryM!!.debug("Make sure you have enough room.") + Tuning.telemetryM!!.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed.") + Tuning.telemetryM!!.debug("Press B on game pad 1 to stop.") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + override fun start() { + Tuning.follower!!.startTeleopDrive(false) + Tuning.follower!!.update() + Tuning.follower!!.setTeleOpDrive(0.0, 1.0, 0.0, true) + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B 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 fun loop() { + if (gamepad1.bWasPressed()) { + Tuning.stopRobot() + requestOpModeStop() + } + + Tuning.follower!!.update() + Tuning.draw() + + val heading = Vector(1.0, Tuning.follower!!.pose.heading - Math.PI / 2) + if (!end) { + if (!stopping) { + if (abs(Tuning.follower!!.velocity.dot(heading)) > VELOCITY) { + previousVelocity = abs(Tuning.follower!!.velocity.dot(heading)) + previousTimeNano = System.nanoTime() + stopping = true + Tuning.follower!!.setTeleOpDrive(0.0, 0.0, 0.0, true) + } + } else { + val currentVelocity: Double = abs(Tuning.follower!!.velocity.dot(heading)) + accelerations.add( + (currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / 10.0.pow( + 9.0 + )) + ) + previousVelocity = currentVelocity + previousTimeNano = System.nanoTime() + if (currentVelocity < Tuning.follower!!.constraints.velocityConstraint) { + end = true + } + } + } else { + var average = 0.0 + for (acceleration in accelerations) { + average += acceleration!! + } + average /= accelerations.size.toDouble() + + Tuning.telemetryM!!.debug("Lateral Zero Power Acceleration (Deceleration): $average") + Tuning.telemetryM!!.debug("\n") + Tuning.telemetryM!!.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on).") + Tuning.telemetryM!!.update(telemetry) + + if (gamepad1.aWasPressed()) { + Tuning.follower!!.getConstants().setLateralZeroPowerAcceleration(average) + val message = "Lateral Zero Power Acceleration: $average" + Tuning.changes.add(message) + } + } + } + + companion object { + var VELOCITY: Double = 30.0 + } +} + +/** + * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. + * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. + * + * @author Baron Henderson - 20077 The Indubitables + * @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 + */ +internal class TranslationalTuner : OpMode() { + private var forward = true + + private var forwards: Path? = null + private var backwards: Path? = null + + override fun init() {} + + /** This initializes the Follower and creates the forward and backward Paths. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("This will activate the translational PIDF(s)") + Tuning.telemetryM!!.debug("The robot will try to stay in place while you push it laterally.") + Tuning.telemetryM!!.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s).") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun start() { + Tuning.follower!!.deactivateAllPIDFs() + Tuning.follower!!.activateTranslational() + forwards = Path(BezierLine(Pose(0.0, 0.0), Pose(DISTANCE, 0.0))) + forwards!!.setConstantHeadingInterpolation(0.0) + backwards = Path(BezierLine(Pose(DISTANCE, 0.0), Pose(0.0, 0.0))) + backwards!!.setConstantHeadingInterpolation(0.0) + Tuning.follower!!.followPath(forwards) + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + override fun loop() { + Tuning.follower!!.update() + Tuning.draw() + + if (!Tuning.follower!!.isBusy) { + if (forward) { + forward = false + Tuning.follower!!.followPath(backwards) + } else { + forward = true + Tuning.follower!!.followPath(forwards) + } + } + + Tuning.telemetryM!!.debug("Push the robot laterally to test the Translational PIDF(s).") + Tuning.telemetryM!!.update(telemetry) + } + + companion object { + var DISTANCE: Double = 40.0 + } +} + +/** + * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. + * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. + * It will try to keep the robot at a constant heading while the user tries to turn it. + * + * @author Baron Henderson - 20077 The Indubitables + * @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 + */ +internal class HeadingTuner : OpMode() { + private var forward = true + + private var forwards: Path? = null + private var backwards: Path? = null + + override fun init() {} + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + override fun init_loop() { + Tuning.telemetryM!!.debug("This will activate the heading PIDF(s).") + Tuning.telemetryM!!.debug("The robot will try to stay at a constant heading while you try to turn it.") + Tuning.telemetryM!!.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s).") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun start() { + Tuning.follower!!.deactivateAllPIDFs() + Tuning.follower!!.activateHeading() + forwards = Path(BezierLine(Pose(0.0, 0.0), Pose(DISTANCE, 0.0))) + forwards!!.setConstantHeadingInterpolation(0.0) + backwards = Path(BezierLine(Pose(DISTANCE, 0.0), Pose(0.0, 0.0))) + backwards!!.setConstantHeadingInterpolation(0.0) + Tuning.follower!!.followPath(forwards) + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + override fun loop() { + Tuning.follower!!.update() + Tuning.draw() + + if (!Tuning.follower!!.isBusy) { + if (forward) { + forward = false + Tuning.follower!!.followPath(backwards) + } else { + forward = true + Tuning.follower!!.followPath(forwards) + } + } + + Tuning.telemetryM!!.debug("Turn the robot manually to test the Heading PIDF(s).") + Tuning.telemetryM!!.update(telemetry) + } + + companion object { + var DISTANCE: Double = 40.0 + } +} + +/** + * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. + * + * @author Baron Henderson - 20077 The Indubitables + * @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 + */ +internal class DriveTuner : OpMode() { + private var forward = true + + private var forwards: PathChain? = null + private var backwards: PathChain? = null + + override fun init() {} + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + override fun init_loop() { + Tuning.telemetryM!!.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward.") + Tuning.telemetryM!!.debug("The robot will go forward and backward continuously along the path.") + Tuning.telemetryM!!.debug("Make sure you have enough room.") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun start() { + Tuning.follower!!.deactivateAllPIDFs() + Tuning.follower!!.activateDrive() + + forwards = Tuning.follower!!.pathBuilder().setGlobalDeceleration() + .addPath(BezierLine(Pose(0.0, 0.0), Pose(DISTANCE, 0.0))).setConstantHeadingInterpolation(0.0).build() + + backwards = Tuning.follower!!.pathBuilder().setGlobalDeceleration() + .addPath(BezierLine(Pose(DISTANCE, 0.0), Pose(0.0, 0.0))).setConstantHeadingInterpolation(0.0).build() + + Tuning.follower!!.followPath(forwards) + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + override fun loop() { + Tuning.follower!!.update() + Tuning.draw() + + if (!Tuning.follower!!.isBusy) { + if (forward) { + forward = false + Tuning.follower!!.followPath(backwards) + } else { + forward = true + Tuning.follower!!.followPath(forwards) + } + } + + Tuning.telemetryM!!.debug("Driving forward?: $forward") + Tuning.telemetryM!!.update(telemetry) + } + + companion object { + var DISTANCE: Double = 40.0 + } +} + +/** + * This is the Line Test Tuner OpMode. It will drive the robot forward and back + * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. + * + * @author Baron Henderson - 20077 The Indubitables + * @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 + */ +internal class Line : OpMode() { + private var forward = true + + private var forwards: Path? = null + private var backwards: Path? = null + + override fun init() {} + + /** This initializes the Follower and creates the forward and backward Paths. */ + override fun init_loop() { + Tuning.telemetryM!!.debug("This will activate all the PIDF(s)") + Tuning.telemetryM!!.debug("The robot will go forward and backward continuously along the path while correcting.") + Tuning.telemetryM!!.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s).") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun start() { + Tuning.follower!!.activateAllPIDFs() + forwards = Path(BezierLine(Pose(0.0, 0.0), Pose(DISTANCE, 0.0))) + forwards!!.setConstantHeadingInterpolation(0.0) + backwards = Path(BezierLine(Pose(DISTANCE, 0.0), Pose(0.0, 0.0))) + backwards!!.setConstantHeadingInterpolation(0.0) + Tuning.follower!!.followPath(forwards) + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + override fun loop() { + Tuning.follower!!.update() + Tuning.draw() + + if (!Tuning.follower!!.isBusy) { + if (forward) { + forward = false + Tuning.follower!!.followPath(backwards) + } else { + forward = true + Tuning.follower!!.followPath(forwards) + } + } + + Tuning.telemetryM!!.debug("Driving Forward?: $forward") + Tuning.telemetryM!!.update(telemetry) + } + + companion object { + var DISTANCE: Double = 48.0 + } +} + +/** + * This is the Centripetal Tuner 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. + * + * @author Baron Henderson - 20077 The Indubitables + * @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 + */ +internal class CentripetalTuner : OpMode() { + private var forward = true + + private var forwards: Path? = null + private var backwards: Path? = null + + override fun init() {} + + /** + * This initializes the Follower and creates the forward and backward Paths. + * Additionally, this initializes the Panels telemetry. + */ + override fun init_loop() { + Tuning.telemetryM!!.debug("This will run the robot in a curve going $DISTANCE inches to the left and the same number of inches forward.") + Tuning.telemetryM!!.debug("The robot will go continuously along the path.") + Tuning.telemetryM!!.debug("Make sure you have enough room.") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun start() { + Tuning.follower!!.activateAllPIDFs() + forwards = Path(BezierCurve(Pose(), Pose(abs(DISTANCE), 0.0), Pose(abs(DISTANCE), DISTANCE))) + backwards = Path(BezierCurve(Pose(abs(DISTANCE), DISTANCE), Pose(abs(DISTANCE), 0.0), Pose(0.0, 0.0))) + + backwards!!.setTangentHeadingInterpolation() + backwards!!.reverseHeadingInterpolation() + + Tuning.follower!!.followPath(forwards) + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + override fun loop() { + Tuning.follower!!.update() + Tuning.draw() + if (!Tuning.follower!!.isBusy) { + if (forward) { + forward = false + Tuning.follower!!.followPath(backwards) + } else { + forward = true + Tuning.follower!!.followPath(forwards) + } + } + + Tuning.telemetryM!!.debug("Driving away from the origin along the curve?: $forward") + Tuning.telemetryM!!.update(telemetry) + } + + companion object { + var DISTANCE: Double = 24.0 + } +} + +/** + * This is the Triangle autonomous OpMode. + * It runs the robot in a triangle, with the starting point being the bottom-middle point. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge + * @version 1.0, 12/30/2024 + */ +internal class Triangle : OpMode() { + private val startPose = Pose(0.0, 0.0, Math.toRadians(0.0)) + private val interPose = Pose(24.0, -24.0, Math.toRadians(90.0)) + private val endPose = Pose(24.0, 24.0, Math.toRadians(45.0)) + + private var triangle: PathChain? = null + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + override fun loop() { + Tuning.follower!!.update() + Tuning.draw() + + if (Tuning.follower!!.atParametricEnd()) { + Tuning.follower!!.followPath(triangle, true) + } + } + + override fun init() {} + + override fun init_loop() { + Tuning.telemetryM!!.debug("This will run in a roughly triangular shape, starting on the bottom-middle point.") + Tuning.telemetryM!!.debug("So, make sure you have enough space to the left, front, and right to run the OpMode.") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + /** Creates the PathChain for the "triangle". */ + override fun start() { + Tuning.follower!!.setStartingPose(startPose) + + triangle = Tuning.follower!!.pathBuilder().addPath(BezierLine(startPose, interPose)) + .setLinearHeadingInterpolation(startPose.heading, interPose.heading).addPath(BezierLine(interPose, endPose)) + .setLinearHeadingInterpolation(interPose.heading, endPose.heading).addPath(BezierLine(endPose, startPose)) + .setLinearHeadingInterpolation(endPose.heading, startPose.heading).build() + + Tuning.follower!!.followPath(triangle) + } +} + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bézier 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 + */ +internal class Circle : OpMode() { + private var circle: PathChain? = null + + override fun start() { + circle = Tuning.follower!!.pathBuilder() + .addPath(BezierCurve(Pose(0.0, 0.0), Pose(RADIUS, 0.0), Pose(RADIUS, RADIUS))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0.0, RADIUS)) + .addPath(BezierCurve(Pose(RADIUS, RADIUS), Pose(RADIUS, 2 * RADIUS), Pose(0.0, 2 * RADIUS))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0.0, RADIUS)) + .addPath(BezierCurve(Pose(0.0, 2 * RADIUS), Pose(-RADIUS, 2 * RADIUS), Pose(-RADIUS, RADIUS))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0.0, RADIUS)) + .addPath(BezierCurve(Pose(-RADIUS, RADIUS), Pose(-RADIUS, 0.0), Pose(0.0, 0.0))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0.0, RADIUS)).build() + Tuning.follower!!.followPath(circle) + } + + override fun init_loop() { + Tuning.telemetryM!!.debug("This will run in a roughly circular shape of radius $RADIUS, starting on the right-most edge. ") + Tuning.telemetryM!!.debug("So, make sure you have enough space to the left, front, and back to run the OpMode.") + Tuning.telemetryM!!.debug("It will also continuously face the center of the circle to test your heading and centripetal correction.") + Tuning.telemetryM!!.update(telemetry) + Tuning.follower!!.update() + Tuning.drawOnlyCurrent() + } + + override fun init() {} + + /** + * 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 fun loop() { + Tuning.follower!!.update() + Tuning.draw() + + if (Tuning.follower!!.atParametricEnd()) { + Tuning.follower!!.followPath(circle) + } + } + + companion object { + var RADIUS: Double = 10.0 + } +} + +/** + * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. + * + * @author Lazar - 19234 + * @version 1.1, 5/19/2025 + */ +internal object Drawing { + const val ROBOT_RADIUS: Double = 9.0 // woah + private val panelsField = field + + private val robotLook = Style( + "", "#3F51B5", 0.75 + ) + private val historyLook = Style( + "", "#4CAF50", 0.75 + ) + + /** + * This prepares Panels Field for using Pedro Offsets + */ + fun init() { + panelsField.setOffsets(presets.PEDRO_PATHING) + } + + /** + * 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 Pedro Follower instance. + */ + fun drawDebug(follower: Follower) { + if (follower.currentPath != null) { + drawPath(follower.currentPath, robotLook) + val closestPoint = follower.getPointFromPath(follower.currentPath.closestPointTValue) + drawRobot( + Pose( + closestPoint.x, + closestPoint.y, + follower.currentPath.getHeadingGoal(follower.currentPath.closestPointTValue) + ), robotLook + ) + } + drawPoseHistory(follower.poseHistory, historyLook) + drawRobot(follower.pose, historyLook) + + sendPacket() + } + + /** + * This draws a robot at a specified Pose with a specified + * look. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + * @param style the parameters used to draw the robot with + */ + /** + * This draws a robot at a specified Pose. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + */ + @JvmOverloads + fun drawRobot(pose: Pose?, style: Style = robotLook) { + if (pose == null || pose.x.isNaN() || pose.y.isNaN() || pose.heading.isNaN()) { + return + } + + panelsField.setStyle(style) + panelsField.moveCursor(pose.x, pose.y) + panelsField.circle(ROBOT_RADIUS) + + val v = pose.headingAsUnitVector + v.magnitude *= ROBOT_RADIUS + val x1 = pose.x + v.xComponent / 2 + val y1 = pose.y + v.yComponent / 2 + val x2 = pose.x + v.xComponent + val y2 = pose.y + v.yComponent + + panelsField.setStyle(style) + panelsField.moveCursor(x1, y1) + panelsField.line(x2, y2) + } + + /** + * This draws a Path with a specified look. + * + * @param path the Path to draw + * @param style the parameters used to draw the Path with + */ + fun drawPath(path: Path, style: Style) { + val points = path.panelsDrawingPoints + + for (i in points[0]!!.indices) { + for (j in points.indices) { + if (points[j]!![i].isNaN()) { + points[j]!![i] = 0.0 + } + } + } + + panelsField.setStyle(style) + panelsField.moveCursor(points[0]!![0], points[0]!![1]) + panelsField.line(points[1]!![0], points[1]!![1]) + } + + /** + * This draws all the Paths in a PathChain with a + * specified look. + * + * @param pathChain the PathChain to draw + * @param style the parameters used to draw the PathChain with + */ + fun drawPath(pathChain: PathChain, style: Style) { + for (i in 0.. Date: Wed, 5 Nov 2025 23:42:45 -0500 Subject: [PATCH 03/29] 11/05/25 --- TeamCode/build.gradle | 1 + .../ftc/teamcode/common/commands/AutoFire.kt | 33 +++++ .../common/hardware/cached/CachedMotor.kt | 3 + .../ftc/teamcode/common/subsystem/FlyWheel.kt | 70 ++++++++++ .../ftc/teamcode/common/subsystem/Intake.kt | 24 ++++ .../ftc/teamcode/common/subsystem/Pedro.kt | 32 +++++ .../teamcode/common/subsystem/Subsystem.kt | 6 +- .../ftc/teamcode/common/subsystem/Uppies.kt | 122 ++++++++++++++++++ .../ftc/teamcode/opmode/auton/SampleAuton.kt | 2 + .../ftc/teamcode/opmode/teleop/MotorTuner.kt | 33 +++++ .../teamcode/opmode/teleop/SampleTeleop.kt | 2 + .../ftc/teamcode/opmode/teleop/ServoTuner.kt | 31 +++++ .../ftc/teamcode/opmode/teleop/Teleop.kt | 21 +++ .../ftc/teamcode/pedro/Constants.kt | 22 ++-- 14 files changed, 390 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MotorTuner.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ServoTuner.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 31a8c2d05386..d1ab0621ecf8 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -29,4 +29,5 @@ dependencies { implementation("org.jetbrains.kotlinx:kotlinx-coroutines-core:1.10.2") implementation 'org.ftclib.ftclib:core:2.1.1' + implementation 'com.google.firebase:firebase-crashlytics-buildtools:3.0.6' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt new file mode 100644 index 000000000000..6bac3664f821 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.common.commands + +import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.SleepFor +import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel +import org.firstinspires.ftc.teamcode.common.subsystem.Intake +import org.firstinspires.ftc.teamcode.common.subsystem.Uppies +import org.firstinspires.ftc.teamcode.opmode.OpMode + +fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) = Command("AutoFire") { + with(opMode) { + require(isStarted) + lock(intake, flyWheel, uppies) + intake.power = 0.0 + flyWheel.state = FlyWheel.State.SHOOTING + WaitFor { flyWheel.atVelocity == true } + unlock(intake, flyWheel, uppies) + } +} + +private fun lock(intake: Intake, flyWheel: FlyWheel, uppies: Uppies) { + intake.isTeleop = false + flyWheel.isTeleop = false + uppies.isTeleop = false +} + +private fun unlock(intake: Intake, flyWheel: FlyWheel, uppies: Uppies) { + intake.isTeleop = true + flyWheel.isTeleop = true + uppies.isTeleop = true +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt index 44f7ff211cf2..ca431f217b3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt @@ -47,4 +47,7 @@ open class CachedMotor( } } } + + val velocity + get() = motor.velocity } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt new file mode 100644 index 000000000000..ae23330db670 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualMotor +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@Configurable +class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWheel") { + val leftMotor = ManualMotor(opMode.hardwareMap, "m2e", reverse = true) + val rightMotor = ManualMotor(opMode.hardwareMap, "m1e", reverse = true) + var state = State.IDLE + var power = 0.0 + + val atVelocity: Boolean + get() { + return leftMotor.velocity >= targetVelocity && -rightMotor.velocity >= targetVelocity + } + + val teleopState = TeleopData() + + override val run: suspend Command.() -> Unit = { + with (opMode) { + WaitFor { isStarted || !isStopRequested } + while (!isStopRequested) { + if (isTeleop) { + val currentX = gamepad1.x + val currentB = gamepad1.b + if (currentX && !teleopState.prevX) { + state = if (state == State.SHOOTING) State.IDLE else State.SHOOTING + } + if (currentB && !teleopState.prevB) { + state = if (state == State.INTAKING) State.IDLE else State.INTAKING + } + teleopState.prevX = currentX + teleopState.prevB = currentB + } + power = when (state) { + State.IDLE -> 0.0 + State.SHOOTING -> ShootingVelocity + State.INTAKING -> IntakingVelocity + } + leftMotor.power = power + rightMotor.power = power + tel.addData("fw state", state) + tel.addData("fw-l velocity", leftMotor.velocity) + tel.addData("fw-r velocity", -rightMotor.velocity) + sync() + } + } + } + + enum class State { + IDLE, + SHOOTING, + INTAKING + } + + companion object { + @JvmField + var ShootingVelocity = 0.7 + @JvmField + var IntakingVelocity = -1.0 + @JvmField + var targetVelocity = 100.0 + } + + data class TeleopData(var prevX: Boolean = false, var prevB: Boolean = false) +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt new file mode 100644 index 000000000000..cbc858be82e3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualMotor +import org.firstinspires.ftc.teamcode.opmode.OpMode + +class Intake(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Intake") { + val motor = ManualMotor(opMode.hardwareMap, "m3e") + var power = 0.0 + + override val run: suspend Command.() -> Unit = { + with (opMode) { + WaitFor { isStarted || !isStopRequested } + while (!isStopRequested) { + if (isTeleop) { + power = (gp1.right_trigger - gp1.left_trigger).toDouble(); + } + motor.power = power + sync() + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt new file mode 100644 index 000000000000..7bb8795f3dac --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.WaitFor +import com.pedropathing.geometry.Pose +import org.firstinspires.ftc.teamcode.opmode.OpMode +import org.firstinspires.ftc.teamcode.pedro.Constants + +class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") { + val follower = Constants.createFollower(opMode.hardwareMap).apply { + setStartingPose(Pose(0.0, 0.0, 0.0)) + } + + override val run: suspend Command.() -> Unit = { + with(opMode) { + follower.update() + if (isTeleop) follower.startTeleopDrive() + WaitFor { isStarted || !isStopRequested } + while (!isStopRequested) { + follower.update() + if (isTeleop) { + follower.setTeleOpDrive( + -gp1.left_stick_y.toDouble(), + -gp1.left_stick_x.toDouble(), + -gp1.right_stick_x.toDouble() + ) + } + sync() + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt index 44c562e3cc98..a7a852465451 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt @@ -2,14 +2,16 @@ package org.firstinspires.ftc.teamcode.common.subsystem import com.millburnx.cmdx.Command -open class Subsystem( +abstract class Subsystem( val name: String ) { init { SubsystemManager.subsystems.add(this) } - open val run: suspend Command.() -> Unit = {} + + abstract val run: suspend Command.() -> Unit open val cleanup: () -> Unit = {} + // by lazy to make sure it's created post-overrides val command: Command by lazy { Command(name, cleanup, run) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt new file mode 100644 index 000000000000..f5c1cdac9314 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -0,0 +1,122 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.arcrobotics.ftclib.controller.PIDController +import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualAxon +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@Configurable +class Uppies( + opMode: OpMode, + val flyWheelState: () -> FlyWheel.State, + var isTeleop: Boolean = false +) : Subsystem("Uppies") { + val leftServo = ManualAxon(opMode.hardwareMap, "s0", "a0", threshold = 0.01) + var leftState = LeftState.INTAKE_READY + var leftRotations = 0; + val leftTarget: Double + get() = leftRotations + when (leftState) { + LeftState.INTAKE_READY -> IntakeReadyPosition + LeftState.LOADER_READY -> LoaderReadyPosition + LeftState.LOADED -> LoadedPosition + } + val leftPID = PIDController(kP, kI, kD) + + val teleopState = TeleopData() + + fun prevState() { + leftState = if (leftState == LeftState.LOADED) { + if (flyWheelState() == FlyWheel.State.INTAKING) { + LeftState.LOADER_READY + } else { + LeftState.INTAKE_READY + } + } else { + if (leftState == LeftState.INTAKE_READY) { + leftRotations-- + } + LeftState.LOADED + } + } + + fun nextState() { + leftState = if (leftState == LeftState.LOADED) { + if (flyWheelState() == FlyWheel.State.INTAKING) { + leftRotations-- + LeftState.LOADER_READY + } else { + leftRotations++ + LeftState.INTAKE_READY + } + } else { + LeftState.LOADED + } + } + + fun flywheelSync() { + if (flyWheelState() == FlyWheel.State.INTAKING && leftState == LeftState.INTAKE_READY) { + leftRotations-- + leftState = LeftState.LOADER_READY + } + if (flyWheelState() != FlyWheel.State.INTAKING && leftState == LeftState.LOADER_READY) { + leftRotations++ + leftState = LeftState.INTAKE_READY + } + } + + override val run: suspend Command.() -> Unit = { + with(opMode) { + WaitFor { isStarted || !isStopRequested } + while (!isStopRequested) { + if (isTeleop) { + val currentLeftBumper = gp1.left_bumper + val currentRightBumper = gp1.right_bumper + if (currentLeftBumper && !teleopState.prevLeftBumper) { + prevState() + } + if (currentRightBumper && !teleopState.prevRightBumper) { + nextState() + } + teleopState.prevLeftBumper = currentLeftBumper + teleopState.prevRightBumper = currentRightBumper + } + flywheelSync() + leftPID.p = kP + leftPID.i = kI + leftPID.d = kD + leftServo.power = leftPID.calculate(leftServo.position, leftTarget) + tel.addData("left state", leftState) + tel.addData("left position", leftServo.position) + sync() + } + } + } + + enum class LeftState { + INTAKE_READY, LOADER_READY, LOADED, + } + + data class TeleopData(var prevLeftBumper: Boolean = false, var prevRightBumper: Boolean = false) + + companion object { + @JvmField + var kP = 2.0 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.06 + + @JvmField + var IntakeReadyPosition = 0.125 + + @JvmField + var LoaderReadyPosition = 0.68 + + @JvmField + var LoadedPosition = 0.4 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt index f8994c14ae6b..8a37eb7237cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.opmode.auton +import com.qualcomm.robotcore.eventloop.opmode.Autonomous import org.firstinspires.ftc.teamcode.opmode.OpMode +@Autonomous class SampleAuton: OpMode() { override fun run() { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MotorTuner.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MotorTuner.kt new file mode 100644 index 000000000000..49a32a205439 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MotorTuner.kt @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop + +import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.WaitFor +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@Configurable +@TeleOp +class MotorTuner + : OpMode() { + override fun run() { +// scheduler.schedule(TeleOpStopper(this)) + val motor = hardwareMap.dcMotor[motorName] + scheduler.schedule(Command { + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + println("setting $motorName to $power - ee") + motor.power = power + sync() + } + }) + } + + companion object { + @JvmField + var motorName = "m0" + + @JvmField + var power = 0.0 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt index 66005b6ca8b8..65218f73a56d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt @@ -1,8 +1,10 @@ package org.firstinspires.ftc.teamcode.opmode.teleop +import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.common.commands.TeleOpStopper import org.firstinspires.ftc.teamcode.opmode.OpMode +@TeleOp class SampleTeleop: OpMode() { override fun run() { scheduler.schedule(TeleOpStopper(this)) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ServoTuner.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ServoTuner.kt new file mode 100644 index 000000000000..878547f1bd35 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ServoTuner.kt @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop + +import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.WaitFor +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@Configurable +@TeleOp +class ServoTuner : OpMode() { + override fun run() { +// scheduler.schedule(TeleOpStopper(this)) + val servo = hardwareMap.servo[servoName] + scheduler.schedule(Command { + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + servo.position = position + sync() + } + }) + } + + companion object { + @JvmField + var servoName = "s0" + + @JvmField + var position = 0.0 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt new file mode 100644 index 000000000000..8afcaecdc9fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import org.firstinspires.ftc.teamcode.common.commands.AutoFire +import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel +import org.firstinspires.ftc.teamcode.common.subsystem.Intake +import org.firstinspires.ftc.teamcode.common.subsystem.Pedro +import org.firstinspires.ftc.teamcode.common.subsystem.Uppies +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@TeleOp +class Teleop : OpMode() { + override fun run() { + val intake = Intake(this, isTeleop = true) + val flyWheel = FlyWheel(this, isTeleop = true) + val uppies = Uppies(this, { flyWheel.state }, isTeleop = true) + val pedro = Pedro(this, isTeleop = true) + val autoFire = AutoFire(this, intake, flyWheel, uppies) +// scheduler.schedule(TeleOpStopper(this)) + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt index 14da4f034eed..0519c041670f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedro +import com.pedropathing.control.FilteredPIDFCoefficients +import com.pedropathing.control.PIDFCoefficients import com.pedropathing.follower.Follower import com.pedropathing.follower.FollowerConstants import com.pedropathing.ftc.FollowerBuilder @@ -18,15 +20,15 @@ object Constants { .forwardZeroPowerAcceleration(-154.736) .lateralZeroPowerAcceleration(-148.621) .centripetalScaling(0.0) -// .translationalPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) -// .headingPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) -// .drivePIDFCoefficients(FilteredPIDFCoefficients(0.0, 0.0, 0.0, 0.6, 0.0)) -// .useSecondaryTranslationalPIDF(true) -// .useSecondaryHeadingPIDF(true) -// .useSecondaryDrivePIDF(true) -// .secondaryTranslationalPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) -// .secondaryHeadingPIDFCoefficients(PIDFCoefficients(0.0, 0.0, 0.0, 0.0)) -// .secondaryDrivePIDFCoefficients(FilteredPIDFCoefficients(0.0, 0.0, 0.0, 0.0, 0.0)) + .translationalPIDFCoefficients(PIDFCoefficients(0.35, 0.0, 0.01, 0.03)) + .headingPIDFCoefficients(PIDFCoefficients(1.5, 0.0, 0.1, 0.03)) + .drivePIDFCoefficients(FilteredPIDFCoefficients(0.01, 0.0, 0.0, 0.6, 0.3)) + .useSecondaryTranslationalPIDF(true) + .useSecondaryHeadingPIDF(true) + .useSecondaryDrivePIDF(true) + .secondaryTranslationalPIDFCoefficients(PIDFCoefficients(0.4, 0.0, 0.03, 0.03)) + .secondaryHeadingPIDFCoefficients(PIDFCoefficients(2.0, 0.0, 0.2, 0.03)) + .secondaryDrivePIDFCoefficients(FilteredPIDFCoefficients(0.015, 0.0, 0.0, 0.6, 0.3)) fun MecanumConstants.setMotors() = apply { rightFrontMotorName("m2") @@ -58,7 +60,7 @@ object Constants { .forwardEncoderDirection(EncoderDirection.REVERSED) .strafeEncoderDirection(EncoderDirection.REVERSED) - val pathConstraints: PathConstraints = PathConstraints(0.99, 100.0, 1.0, 1.0) + val pathConstraints: PathConstraints = PathConstraints(0.99, 100.0, .25, 2.0) fun createFollower(hardwareMap: HardwareMap): Follower { return FollowerBuilder(followerConstants, hardwareMap) From 8e4df2a712d964ad377ed6533486a8d145bcc830 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Thu, 6 Nov 2025 14:26:46 -0500 Subject: [PATCH 04/29] Add motor power clamping (should help with looptimes). Create Velocity PIDF for flywheel. --- .../common/hardware/cached/CachedAxon.kt | 7 +++-- .../common/hardware/cached/CachedMotor.kt | 7 +++-- .../common/hardware/manual/ManualAxon.kt | 5 ++-- .../common/hardware/manual/ManualMotor.kt | 5 ++-- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 30 ++++++++++++++++++- .../ftc/teamcode/common/subsystem/Uppies.kt | 4 +-- 6 files changed, 44 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt index 9a052963df94..30bd25d81b9f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedAxon.kt @@ -16,9 +16,10 @@ open class CachedAxon( open var power = 0.0 set(value) { - if (abs(value - field) > threshold) { - field = value - axon.power = value + val clamped = value.coerceIn(-1.0, 1.0) + if (abs(clamped - field) > threshold) { + field = clamped + axon.power = clamped } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt index ca431f217b3e..35d48dbf9c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt @@ -19,9 +19,10 @@ open class CachedMotor( open var power = 0.0 set(value) { - if (abs(value - field) > threshold) { - field = value - motor.power = value + val clamped = value.coerceIn(-1.0, 1.0) + if (abs(clamped - field) > threshold) { + field = clamped + motor.power = clamped } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt index 8efdc99dcadd..f07ab827b551 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt @@ -18,8 +18,9 @@ class ManualAxon( override var power = 0.0 set(value) { - if (abs(value - field) > threshold) { - field = value + val clamped = value.coerceIn(-1.0, 1.0) + if (abs(clamped - field) > threshold) { + field = clamped pending = true } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt index 70976e852485..e6aa17fb7f77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualMotor.kt @@ -17,8 +17,9 @@ class ManualMotor( override var power = 0.0 set(value) { - if (abs(value - field) > threshold) { - field = value + val clamped = value.coerceIn(-1.0, 1.0) + if (abs(clamped - field) > threshold) { + field = clamped pending = true } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index ae23330db670..b28cccf5740d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.common.subsystem +import com.arcrobotics.ftclib.controller.PIDController +import com.arcrobotics.ftclib.controller.wpilibcontroller.SimpleMotorFeedforward import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.WaitFor @@ -21,7 +23,7 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh val teleopState = TeleopData() override val run: suspend Command.() -> Unit = { - with (opMode) { + with(opMode) { WaitFor { isStarted || !isStopRequested } while (!isStopRequested) { if (isTeleop) { @@ -60,11 +62,37 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh companion object { @JvmField var ShootingVelocity = 0.7 + @JvmField var IntakingVelocity = -1.0 + @JvmField var targetVelocity = 100.0 } data class TeleopData(var prevX: Boolean = false, var prevB: Boolean = false) +} + +@Configurable +class FlyWheelController() { + val PID = PIDController(kP, kI, kD) + val FF = SimpleMotorFeedforward(kS, kV, 0.0) + + fun calculate(currentVelocity: Double, targetVelocity: Double, voltage: Double): Double { + val pid = PID.calculate(currentVelocity, targetVelocity) + val ff = FF.calculate(targetVelocity) + val voltageCompensation = (12.0 / voltage) + val weightedVoltageCompensation = 1 + (1 - voltageCompensation) * voltageWeight + + return (pid + ff) * weightedVoltageCompensation + } + + companion object { + @JvmField var kP = 0.0 + @JvmField var kI = 0.0 + @JvmField var kD = 0.0 + @JvmField var kS = 0.0 + @JvmField var kV = 0.0 + @JvmField var voltageWeight = 0.0 + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index f5c1cdac9314..9375497d0973 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -83,9 +83,7 @@ class Uppies( teleopState.prevRightBumper = currentRightBumper } flywheelSync() - leftPID.p = kP - leftPID.i = kI - leftPID.d = kD + leftPID.setPID(kP, kI, kD) leftServo.power = leftPID.calculate(leftServo.position, leftTarget) tel.addData("left state", leftState) tel.addData("left position", leftServo.position) From 90af1b40187f6ec38208854ab37ffac5c6ba8c15 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Thu, 6 Nov 2025 14:32:24 -0500 Subject: [PATCH 05/29] FlyWheel Velocity Control & Subsystem Cleanup --- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 50 +++++++++++-------- .../ftc/teamcode/common/subsystem/Uppies.kt | 30 ++++++----- 2 files changed, 46 insertions(+), 34 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index b28cccf5740d..f3b5ed889673 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -12,8 +12,10 @@ import org.firstinspires.ftc.teamcode.opmode.OpMode class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWheel") { val leftMotor = ManualMotor(opMode.hardwareMap, "m2e", reverse = true) val rightMotor = ManualMotor(opMode.hardwareMap, "m1e", reverse = true) + val leftController = FlyWheelController() + val rightController = FlyWheelController() var state = State.IDLE - var power = 0.0 + var targetVelocity = 0.0 val atVelocity: Boolean get() { @@ -26,25 +28,17 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh with(opMode) { WaitFor { isStarted || !isStopRequested } while (!isStopRequested) { - if (isTeleop) { - val currentX = gamepad1.x - val currentB = gamepad1.b - if (currentX && !teleopState.prevX) { - state = if (state == State.SHOOTING) State.IDLE else State.SHOOTING - } - if (currentB && !teleopState.prevB) { - state = if (state == State.INTAKING) State.IDLE else State.INTAKING - } - teleopState.prevX = currentX - teleopState.prevB = currentB - } - power = when (state) { + teleOpControls() + + targetVelocity = when (state) { State.IDLE -> 0.0 - State.SHOOTING -> ShootingVelocity + State.SHOOTING -> ShootingVelocity // replace with distance based State.INTAKING -> IntakingVelocity } - leftMotor.power = power - rightMotor.power = power + + leftMotor.power = leftController.calculate(leftMotor.velocity, targetVelocity, 0.0) + rightMotor.power = rightController.calculate(-rightMotor.velocity, targetVelocity, 0.0) + tel.addData("fw state", state) tel.addData("fw-l velocity", leftMotor.velocity) tel.addData("fw-r velocity", -rightMotor.velocity) @@ -53,6 +47,21 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh } } + private fun OpMode.teleOpControls() { + if (isTeleop) { + val currentX = gamepad1.x + val currentB = gamepad1.b + if (currentX && !teleopState.prevX) { + state = if (state == State.SHOOTING) State.IDLE else State.SHOOTING + } + if (currentB && !teleopState.prevB) { + state = if (state == State.INTAKING) State.IDLE else State.INTAKING + } + teleopState.prevX = currentX + teleopState.prevB = currentB + } + } + enum class State { IDLE, SHOOTING, @@ -61,13 +70,10 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh companion object { @JvmField - var ShootingVelocity = 0.7 - - @JvmField - var IntakingVelocity = -1.0 + var ShootingVelocity = 1600.0 @JvmField - var targetVelocity = 100.0 + var IntakingVelocity = -1600.0 } data class TeleopData(var prevX: Boolean = false, var prevB: Boolean = false) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index 9375497d0973..74201146df5a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -70,21 +70,12 @@ class Uppies( with(opMode) { WaitFor { isStarted || !isStopRequested } while (!isStopRequested) { - if (isTeleop) { - val currentLeftBumper = gp1.left_bumper - val currentRightBumper = gp1.right_bumper - if (currentLeftBumper && !teleopState.prevLeftBumper) { - prevState() - } - if (currentRightBumper && !teleopState.prevRightBumper) { - nextState() - } - teleopState.prevLeftBumper = currentLeftBumper - teleopState.prevRightBumper = currentRightBumper - } + teleOpControls() + flywheelSync() leftPID.setPID(kP, kI, kD) leftServo.power = leftPID.calculate(leftServo.position, leftTarget) + tel.addData("left state", leftState) tel.addData("left position", leftServo.position) sync() @@ -92,6 +83,21 @@ class Uppies( } } + private fun OpMode.teleOpControls() { + if (isTeleop) { + val currentLeftBumper = gp1.left_bumper + val currentRightBumper = gp1.right_bumper + if (currentLeftBumper && !teleopState.prevLeftBumper) { + prevState() + } + if (currentRightBumper && !teleopState.prevRightBumper) { + nextState() + } + teleopState.prevLeftBumper = currentLeftBumper + teleopState.prevRightBumper = currentRightBumper + } + } + enum class LeftState { INTAKE_READY, LOADER_READY, LOADED, } From 49b54e7605574ba2ef3732f31b6b9e21d589d36b Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Thu, 6 Nov 2025 15:39:46 -0500 Subject: [PATCH 06/29] Autofire --- TeamCode/build.gradle | 4 +- .../ftc/teamcode/common/commands/AutoFire.kt | 63 ++++++++++++++-- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 31 ++++++-- .../ftc/teamcode/common/subsystem/Uppies.kt | 72 ++++++++++--------- 4 files changed, 122 insertions(+), 48 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index d1ab0621ecf8..e8f22f74252e 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -21,8 +21,8 @@ android { dependencies { implementation project(':FtcRobotController') - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:34e2ea1904' - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:34e2ea1904' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:e3af407403' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:e3af407403' // implementation('com.millburnx:cmdx:+') // implementation('com.millburnx:cmdxpedro:+') diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index 6bac3664f821..8dae07a4601b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -2,24 +2,73 @@ package org.firstinspires.ftc.teamcode.common.commands import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command +import com.millburnx.cmdx.ICommand +import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.util.SleepFor import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakeDuration import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode -fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) = Command("AutoFire") { +fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): ICommand { with(opMode) { - require(isStarted) - lock(intake, flyWheel, uppies) - intake.power = 0.0 - flyWheel.state = FlyWheel.State.SHOOTING - WaitFor { flyWheel.atVelocity == true } - unlock(intake, flyWheel, uppies) + suspend fun Command.shootBall() { + WaitFor { (flyWheel.atVelocity && uppies.atTarget) || isStopRequested } + uppies.nextState() + } + + suspend fun Command.loadBall() { + intake.power = 0.5 + SleepFor({ isStopRequested }) { intakeDuration } + uppies.nextState() + } + + return Sequential("Auto Fire") { + Command("Prepare Subsystems") { + if (!isStarted) parentGroup?.cancel() + lock(intake, flyWheel, uppies) + intake.power = 0.0 + flyWheel.state = FlyWheel.State.SHOOTING + } + Command("Verify First Ball") { + if (uppies.state != Uppies.State.LOADED) { + loadBall() + } + } + Command("Shoot First Ball") { shootBall() } + Command("Load Second Ball") { loadBall() } + Command("Shoot Second Ball") { shootBall() } + Command("Load Third Ball") { loadBall() } + Command("Shoot Third Ball") { shootBall() } + Command("Reset Subsystems") { + intake.power = 0.0 + flyWheel.state = FlyWheel.State.IDLE + unlock(intake, flyWheel, uppies) + } + } } } +@Configurable +object AutoFireSettings { + @JvmField + var intakeDuration = 500L +} + +//fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) = Command("AutoFire") { +// with(opMode) { +// require(isStarted) +// lock(intake, flyWheel, uppies) +// intake.power = 0.0 +// flyWheel.state = FlyWheel.State.SHOOTING +// WaitFor { flyWheel.atVelocity } +// uppies.nextState() +// unlock(intake, flyWheel, uppies) +// } +//} + private fun lock(intake: Intake, flyWheel: FlyWheel, uppies: Uppies) { intake.isTeleop = false flyWheel.isTeleop = false diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index f3b5ed889673..15f50e321cc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -7,6 +7,7 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualMotor import org.firstinspires.ftc.teamcode.opmode.OpMode +import kotlin.math.abs @Configurable class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWheel") { @@ -19,7 +20,9 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh val atVelocity: Boolean get() { - return leftMotor.velocity >= targetVelocity && -rightMotor.velocity >= targetVelocity + val leftAtVelocity = abs(leftMotor.velocity - targetVelocity) <= velocityThreshold + val rightAtVelocity = abs(-rightMotor.velocity - targetVelocity) <= velocityThreshold + return leftAtVelocity && rightAtVelocity } val teleopState = TeleopData() @@ -74,6 +77,9 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh @JvmField var IntakingVelocity = -1600.0 + + @JvmField + var velocityThreshold = 200.0 } data class TeleopData(var prevX: Boolean = false, var prevB: Boolean = false) @@ -94,11 +100,22 @@ class FlyWheelController() { } companion object { - @JvmField var kP = 0.0 - @JvmField var kI = 0.0 - @JvmField var kD = 0.0 - @JvmField var kS = 0.0 - @JvmField var kV = 0.0 - @JvmField var voltageWeight = 0.0 + @JvmField + var kP = 0.0 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.0 + + @JvmField + var kS = 0.0 + + @JvmField + var kV = 0.0 + + @JvmField + var voltageWeight = 0.0 } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index 74201146df5a..198bca5cab17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -6,6 +6,7 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualAxon import org.firstinspires.ftc.teamcode.opmode.OpMode +import kotlin.math.abs @Configurable class Uppies( @@ -13,56 +14,60 @@ class Uppies( val flyWheelState: () -> FlyWheel.State, var isTeleop: Boolean = false ) : Subsystem("Uppies") { - val leftServo = ManualAxon(opMode.hardwareMap, "s0", "a0", threshold = 0.01) - var leftState = LeftState.INTAKE_READY - var leftRotations = 0; - val leftTarget: Double - get() = leftRotations + when (leftState) { - LeftState.INTAKE_READY -> IntakeReadyPosition - LeftState.LOADER_READY -> LoaderReadyPosition - LeftState.LOADED -> LoadedPosition + val servo = ManualAxon(opMode.hardwareMap, "s0", "a0", threshold = 0.001) + var state = State.INTAKE_READY + var rotations = 0; + val target: Double + get() = rotations + when (state) { + State.INTAKE_READY -> IntakeReadyPosition + State.LOADER_READY -> LoaderReadyPosition + State.LOADED -> LoadedPosition } - val leftPID = PIDController(kP, kI, kD) + + val atTarget: Boolean + get() = abs(servo.position - target) <= threshold + + val pid = PIDController(kP, kI, kD) val teleopState = TeleopData() fun prevState() { - leftState = if (leftState == LeftState.LOADED) { + state = if (state == State.LOADED) { if (flyWheelState() == FlyWheel.State.INTAKING) { - LeftState.LOADER_READY + State.LOADER_READY } else { - LeftState.INTAKE_READY + State.INTAKE_READY } } else { - if (leftState == LeftState.INTAKE_READY) { - leftRotations-- + if (state == State.INTAKE_READY) { + rotations-- } - LeftState.LOADED + State.LOADED } } fun nextState() { - leftState = if (leftState == LeftState.LOADED) { + state = if (state == State.LOADED) { if (flyWheelState() == FlyWheel.State.INTAKING) { - leftRotations-- - LeftState.LOADER_READY + rotations-- + State.LOADER_READY } else { - leftRotations++ - LeftState.INTAKE_READY + rotations++ + State.INTAKE_READY } } else { - LeftState.LOADED + State.LOADED } } fun flywheelSync() { - if (flyWheelState() == FlyWheel.State.INTAKING && leftState == LeftState.INTAKE_READY) { - leftRotations-- - leftState = LeftState.LOADER_READY + if (flyWheelState() == FlyWheel.State.INTAKING && state == State.INTAKE_READY) { + rotations-- + state = State.LOADER_READY } - if (flyWheelState() != FlyWheel.State.INTAKING && leftState == LeftState.LOADER_READY) { - leftRotations++ - leftState = LeftState.INTAKE_READY + if (flyWheelState() != FlyWheel.State.INTAKING && state == State.LOADER_READY) { + rotations++ + state = State.INTAKE_READY } } @@ -73,11 +78,11 @@ class Uppies( teleOpControls() flywheelSync() - leftPID.setPID(kP, kI, kD) - leftServo.power = leftPID.calculate(leftServo.position, leftTarget) + pid.setPID(kP, kI, kD) + servo.power = pid.calculate(servo.position, target) - tel.addData("left state", leftState) - tel.addData("left position", leftServo.position) + tel.addData("uppies state", state) + tel.addData("uppies position", servo.position) sync() } } @@ -98,7 +103,7 @@ class Uppies( } } - enum class LeftState { + enum class State { INTAKE_READY, LOADER_READY, LOADED, } @@ -114,6 +119,9 @@ class Uppies( @JvmField var kD = 0.06 + @JvmField + var threshold = 0.05 + @JvmField var IntakeReadyPosition = 0.125 From d0193011d69b18bc24556bc164092089e8ba2f59 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 7 Nov 2025 02:51:43 -0500 Subject: [PATCH 07/29] Jam Protection --- .../ftc/teamcode/common/commands/AutoFire.kt | 36 ++++++++++++++++++- .../ftc/teamcode/opmode/OpMode.kt | 2 ++ 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index 8dae07a4601b..31fe5a5cac49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -7,6 +7,8 @@ import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.util.SleepFor import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakeDuration +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.jamDuration +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.unjamDuration import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies @@ -19,10 +21,36 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) uppies.nextState() } - suspend fun Command.loadBall() { + suspend fun Command.attemptUnjam() { + uppies.prevState() + intake.power = -0.25 + SleepFor({ isStopRequested }) { unjamDuration } + } + + suspend fun Command.attemptLoad() { intake.power = 0.5 SleepFor({ isStopRequested }) { intakeDuration } uppies.nextState() + // check for a jam + } + + suspend fun Command.isJammed(): Boolean { + SleepFor({ isStopRequested || uppies.atTarget }) { jamDuration } + return !uppies.atTarget + } + + suspend fun Command.loadBall(jamProtection: Boolean = false) { + if (!jamProtection) { + intake.power = 0.5 + SleepFor({ isStopRequested }) { intakeDuration } + uppies.nextState() + } else { + attemptLoad() + while (isJammed()) { + attemptUnjam() + attemptLoad() + } + } } return Sequential("Auto Fire") { @@ -55,6 +83,12 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) object AutoFireSettings { @JvmField var intakeDuration = 500L + + @JvmField + var jamDuration = 500L + + @JvmField + var unjamDuration = 500L } //fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) = Command("AutoFire") { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt index 7dcae0a1af38..6b54a1056020 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -25,10 +25,12 @@ abstract class OpMode : LinearOpMode() { var hubs: List = emptyList() val scheduler = CommandScheduler().apply { onSync = { + val ms = loopTimer.milliseconds() val loopHertz = 1.0 / loopTimer.seconds() loopTimer.reset() tel.addData("hz", loopHertz) + tel.addData("ms", ms) tel.update(telemetry) // hardware From 8d7abd1eb32e5639321f4a4cb9e29f2f50180d09 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 7 Nov 2025 03:35:41 -0500 Subject: [PATCH 08/29] Add autofire in teleop --- TeamCode/build.gradle | 4 ++-- .../ftc/teamcode/common/commands/AutoFire.kt | 24 +++++++++++++++++-- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 4 ++-- .../ftc/teamcode/opmode/teleop/Teleop.kt | 5 ++-- 4 files changed, 29 insertions(+), 8 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index e8f22f74252e..eaa46c5253e2 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -21,8 +21,8 @@ android { dependencies { implementation project(':FtcRobotController') - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:e3af407403' - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:e3af407403' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:01bc6d6334' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:01bc6d6334' // implementation('com.millburnx:cmdx:+') // implementation('com.millburnx:cmdxpedro:+') diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index 31fe5a5cac49..d78fa2f02ccc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -2,10 +2,10 @@ package org.firstinspires.ftc.teamcode.common.commands import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command -import com.millburnx.cmdx.ICommand import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.util.SleepFor import com.millburnx.cmdxpedro.util.WaitFor +import kotlinx.coroutines.isActive import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakeDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.jamDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.unjamDuration @@ -14,7 +14,27 @@ import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode -fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): ICommand { +fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = Command("Auto Fire Triggerer") { + with (opMode) { + val autoFire = AutoFire(this, intake, flyWheel, uppies) + WaitFor { isStarted } + var prevButton = gp1.a + while (!isStopRequested) { + val currentButton = gp1.a + if (currentButton && !prevButton) { + if (autoFire.currentScope?.isActive ?: false) { + // already scheduled, cancel + autoFire.cancel() + } else { + scheduler.schedule(autoFire) + } + } + prevButton = currentButton + } + } +} + +fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Sequential { with(opMode) { suspend fun Command.shootBall() { WaitFor { (flyWheel.atVelocity && uppies.atTarget) || isStopRequested } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index 15f50e321cc7..a9fcb9919496 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -52,8 +52,8 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh private fun OpMode.teleOpControls() { if (isTeleop) { - val currentX = gamepad1.x - val currentB = gamepad1.b + val currentX = gp1.x + val currentB = gp1.b if (currentX && !teleopState.prevX) { state = if (state == State.SHOOTING) State.IDLE else State.SHOOTING } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index 8afcaecdc9fe..b48d575b37e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.opmode.teleop import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import org.firstinspires.ftc.teamcode.common.commands.AutoFire +import org.firstinspires.ftc.teamcode.common.commands.TeleopAutoFire import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Pedro @@ -15,7 +15,8 @@ class Teleop : OpMode() { val flyWheel = FlyWheel(this, isTeleop = true) val uppies = Uppies(this, { flyWheel.state }, isTeleop = true) val pedro = Pedro(this, isTeleop = true) - val autoFire = AutoFire(this, intake, flyWheel, uppies) + + scheduler.schedule(TeleopAutoFire(this, intake, flyWheel, uppies)) // scheduler.schedule(TeleOpStopper(this)) } } \ No newline at end of file From 1c3c80a69f3cc714e61ff6f3fd618e0b1e94da5d Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 7 Nov 2025 20:39:10 -0500 Subject: [PATCH 09/29] Vision Manager & Apriltag setup --- TeamCode/detekt.yml | 3 ++ .../teamcode/common/subsystem/Apriltags.kt | 40 +++++++++++++++++++ .../teamcode/common/vision/VisionManager.kt | 35 ++++++++++++++++ .../ftc/teamcode/opmode/teleop/Teleop.kt | 10 +++++ 4 files changed, 88 insertions(+) create mode 100644 TeamCode/detekt.yml create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt diff --git a/TeamCode/detekt.yml b/TeamCode/detekt.yml new file mode 100644 index 000000000000..56c80546113e --- /dev/null +++ b/TeamCode/detekt.yml @@ -0,0 +1,3 @@ +performance: + SpreadOperator: + active: false \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt new file mode 100644 index 000000000000..25943c262a2c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.millburnx.cmdxpedro.util.toRadians +import org.firstinspires.ftc.teamcode.common.vision.VisionManager +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection +import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor.PoseSolver + +class Apriltags : Subsystem("Apriltags") { + val processor: AprilTagProcessor = AprilTagProcessor.Builder() + .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) + .setDrawAxes(true) + .setDrawCubeProjection(true) + .build().apply { + setDecimation(2F) + setPoseSolver(PoseSolver.OPENCV_IPPE) + } + + init { + VisionManager.processors.add(processor) + } + + val apriltags: List + get() = processor.detections.toList() + + override val run: suspend Command.() -> Unit = { + // we don't actually have to do anything here + // vision portal handles running the pipeline + // and calculations are handled by the user + } +} + +fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d { + val offset = Vec2d(ftcPose.range).rotate(currentPose.radians - ftcPose.bearing.toRadians()) + return Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw) +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt new file mode 100644 index 000000000000..d6a7e64f865b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.common.vision + +import android.util.Size +import com.bylazar.configurables.annotations.Configurable +import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName +import org.firstinspires.ftc.vision.VisionPortal +import org.firstinspires.ftc.vision.VisionProcessor + +@Suppress("SpreadOperator") +@Configurable +object VisionManager { + val processors = mutableListOf() + + fun init() { + processors.clear() + } + + fun build() = + VisionPortal + .Builder() + .setCamera(opMode.hardwareMap["cam1"] as WebcamName) + .addProcessors(*processors.toTypedArray()) + .setCameraResolution(Size(cameraSizeX, cameraSizeY)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + .enableLiveView(true) + .setAutoStopLiveView(true) + .build() + + @JvmField + var cameraSizeX = 1280 + + @JvmField + var cameraSizeY = 720 +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index b48d575b37e7..bed8a83f3d65 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -2,10 +2,12 @@ package org.firstinspires.ftc.teamcode.opmode.teleop import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.common.commands.TeleopAutoFire +import org.firstinspires.ftc.teamcode.common.subsystem.Apriltags import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Pedro import org.firstinspires.ftc.teamcode.common.subsystem.Uppies +import org.firstinspires.ftc.teamcode.common.vision.VisionManager import org.firstinspires.ftc.teamcode.opmode.OpMode @TeleOp @@ -16,6 +18,14 @@ class Teleop : OpMode() { val uppies = Uppies(this, { flyWheel.state }, isTeleop = true) val pedro = Pedro(this, isTeleop = true) + // we can modify this to auto init probably + // can set a flag on VisionManager.build + // so subsequent attempts to add to the list will clear it first + // effectively auto calling VisionManager.init + VisionManager.init() + val apriltags = Apriltags() + VisionManager.build() + scheduler.schedule(TeleopAutoFire(this, intake, flyWheel, uppies)) // scheduler.schedule(TeleOpStopper(this)) } From 992bc38c0b5a5d4f425768e7813e1cc86444690e Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 7 Nov 2025 22:32:26 -0500 Subject: [PATCH 10/29] static auto aim --- .../ftc/teamcode/common/commands/AutoFire.kt | 2 +- .../teamcode/common/subsystem/Apriltags.kt | 4 +-- .../common/subsystem/AutoTargetting.kt | 35 +++++++++++++++++++ .../ftc/teamcode/common/subsystem/FlyWheel.kt | 2 +- .../ftc/teamcode/common/subsystem/Intake.kt | 2 +- .../ftc/teamcode/common/subsystem/Pedro.kt | 9 +++-- .../ftc/teamcode/common/subsystem/Uppies.kt | 2 +- .../ftc/teamcode/opmode/teleop/Teleop.kt | 3 ++ 8 files changed, 51 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index d78fa2f02ccc..84ee8e739f89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -17,7 +17,7 @@ import org.firstinspires.ftc.teamcode.opmode.OpMode fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = Command("Auto Fire Triggerer") { with (opMode) { val autoFire = AutoFire(this, intake, flyWheel, uppies) - WaitFor { isStarted } + WaitFor { isStarted || isStopRequested } var prevButton = gp1.a while (!isStopRequested) { val currentButton = gp1.a diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt index 25943c262a2c..89c52c0899af 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -24,8 +24,8 @@ class Apriltags : Subsystem("Apriltags") { VisionManager.processors.add(processor) } - val apriltags: List - get() = processor.detections.toList() + val apriltags: Map + get() = processor.detections.associateBy { it.id } override val run: suspend Command.() -> Unit = { // we don't actually have to do anything here diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt new file mode 100644 index 000000000000..bfe2c334e885 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.WaitFor +import org.firstinspires.ftc.teamcode.opmode.OpMode + +class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") { + override val run: suspend Command.() -> Unit = { + with (opMode) { + WaitFor { isStarted || isStopRequested } + var enabled = false + var target: Pose2d? = null + var prevButton = gp1.y + while (!isStopRequested) { + val apriltag = apriltags.apriltags[20] ?: apriltags.apriltags[24] + if (apriltag != null) target = apriltag.toPose(pedro.pose) + + val currentButton = gp1.y + if (currentButton && !prevButton) { + enabled = !enabled + } + prevButton = currentButton + + if (!enabled || target == null) { + pedro.isTeleop = true + if (!pedro.follower.isTeleopDrive) pedro.follower.startTeleopDrive(true) + continue + } + pedro.isTeleop = false + pedro.follower.turnTo(pedro.pose.position.angle(target.position)) + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index a9fcb9919496..7e0ea9291c4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -29,7 +29,7 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh override val run: suspend Command.() -> Unit = { with(opMode) { - WaitFor { isStarted || !isStopRequested } + WaitFor { isStarted || isStopRequested } while (!isStopRequested) { teleOpControls() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt index cbc858be82e3..4067f9ee094c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt @@ -11,7 +11,7 @@ class Intake(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Intake" override val run: suspend Command.() -> Unit = { with (opMode) { - WaitFor { isStarted || !isStopRequested } + WaitFor { isStarted || isStopRequested } while (!isStopRequested) { if (isTeleop) { power = (gp1.right_trigger - gp1.left_trigger).toDouble(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 7bb8795f3dac..18267b8c3458 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.common.subsystem import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor +import com.millburnx.cmdxpedro.util.toDegrees import com.pedropathing.geometry.Pose import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.teamcode.pedro.Constants @@ -11,11 +13,14 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") setStartingPose(Pose(0.0, 0.0, 0.0)) } + val pose: Pose2d + get() = Pose2d(follower.pose.x, follower.pose.y, follower.pose.heading.toDegrees()) + override val run: suspend Command.() -> Unit = { with(opMode) { follower.update() - if (isTeleop) follower.startTeleopDrive() - WaitFor { isStarted || !isStopRequested } + if (isTeleop) follower.startTeleopDrive(true) + WaitFor { isStarted || isStopRequested } while (!isStopRequested) { follower.update() if (isTeleop) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index 198bca5cab17..863869604dc6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -73,7 +73,7 @@ class Uppies( override val run: suspend Command.() -> Unit = { with(opMode) { - WaitFor { isStarted || !isStopRequested } + WaitFor { isStarted || isStopRequested } while (!isStopRequested) { teleOpControls() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index bed8a83f3d65..908bfb62a3e6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.opmode.teleop import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.common.commands.TeleopAutoFire import org.firstinspires.ftc.teamcode.common.subsystem.Apriltags +import org.firstinspires.ftc.teamcode.common.subsystem.AutoTargetting import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Pedro @@ -26,6 +27,8 @@ class Teleop : OpMode() { val apriltags = Apriltags() VisionManager.build() + val autoTargetting = AutoTargetting(this, pedro, apriltags) + scheduler.schedule(TeleopAutoFire(this, intake, flyWheel, uppies)) // scheduler.schedule(TeleOpStopper(this)) } From 3fa159f687a38ba1f1518748567ba80e552e84b1 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 7 Nov 2025 23:11:33 -0500 Subject: [PATCH 11/29] orbit lock --- .../ftc/teamcode/common/normalizeRadians.kt | 10 +++ .../common/subsystem/AutoTargetting.kt | 9 +-- .../ftc/teamcode/common/subsystem/Pedro.kt | 69 +++++++++++++++++-- 3 files changed, 79 insertions(+), 9 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt new file mode 100644 index 000000000000..12b75ced2c89 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.common + +import kotlin.math.floor + +fun normalizeRadians(radians: Double): Double { + val temp = (radians + Math.PI) / (2.0 * Math.PI) + return (temp - floor(temp) - 0.5) * 2.0 +} + +fun normalizeDegrees(angle: Double): Double = Math.toDegrees(normalizeRadians(Math.toRadians(angle))) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt index bfe2c334e885..e327b0b5382c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.common.subsystem import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor +import com.millburnx.cmdxpedro.util.toDegrees import org.firstinspires.ftc.teamcode.opmode.OpMode class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") { @@ -23,12 +24,12 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy prevButton = currentButton if (!enabled || target == null) { - pedro.isTeleop = true - if (!pedro.follower.isTeleopDrive) pedro.follower.startTeleopDrive(true) + pedro.isLocked = false continue } - pedro.isTeleop = false - pedro.follower.turnTo(pedro.pose.position.angle(target.position)) + pedro.isLocked = true + val targetHeading = pedro.pose.position.angle(target.position) + pedro.headingLock.targetHeading = targetHeading.toDegrees() } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 18267b8c3458..9380ac8b7238 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -4,15 +4,25 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.toDegrees +import com.millburnx.cmdxpedro.util.toRadians +import com.pedropathing.control.PIDFController import com.pedropathing.geometry.Pose +import com.pedropathing.math.MathFunctions +import com.pedropathing.math.Vector +import org.firstinspires.ftc.teamcode.common.normalizeDegrees import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.teamcode.pedro.Constants +import kotlin.math.abs + class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") { val follower = Constants.createFollower(opMode.hardwareMap).apply { setStartingPose(Pose(0.0, 0.0, 0.0)) } + val headingLock = HeadingLock(this) + var isLocked = false; + val pose: Pose2d get() = Pose2d(follower.pose.x, follower.pose.y, follower.pose.heading.toDegrees()) @@ -24,14 +34,63 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") while (!isStopRequested) { follower.update() if (isTeleop) { - follower.setTeleOpDrive( - -gp1.left_stick_y.toDouble(), - -gp1.left_stick_x.toDouble(), - -gp1.right_stick_x.toDouble() - ) + if (isLocked) { + follower.setTeleOpDrive( + -gp1.left_stick_y.toDouble(), + -gp1.left_stick_x.toDouble(), + headingLock.calc().magnitude, + false + ) + } else { + follower.setTeleOpDrive( + -gp1.left_stick_y.toDouble(), + -gp1.left_stick_x.toDouble(), + -gp1.right_stick_x.toDouble() + ) + } } sync() } } } +} + +class HeadingLock(val pedro: Pedro) { + private val constants = pedro.follower.constants + + private val headingPIDF = PIDFController(constants.coefficientsHeadingPIDF) + private val secondaryHeadingPIDF = PIDFController(constants.coefficientsSecondaryHeadingPIDF) + private val headingPIDFSwitch = constants.headingPIDFSwitch + var targetHeading = 0.0 + + fun calc(): Vector { + val headingError = normalizeDegrees(targetHeading - pedro.pose.heading).toRadians() + if (abs(headingError) < headingPIDFSwitch && + constants.useSecondaryHeadingPIDF + ) { + secondaryHeadingPIDF.updateFeedForwardInput( + MathFunctions.getTurnDirection( + pedro.pose.radians, + targetHeading.toRadians() + ) + ) + secondaryHeadingPIDF.updateError(headingError) + val headingVector = Vector( + secondaryHeadingPIDF.run().coerceIn(-1.0, 1.0), + pedro.pose.radians + ) + return headingVector.copy() + } + + headingPIDF.updateFeedForwardInput( + MathFunctions.getTurnDirection( + pedro.pose.radians, + targetHeading.toRadians() + ) + ) + headingPIDF.updateError(headingError) + val headingVector = + Vector(headingPIDF.run().coerceIn(-1.0, 1.0), pedro.pose.radians) + return headingVector.copy() + } } \ No newline at end of file From 87729d8f557112c498c0873e6b7c3e1bdc8cbe41 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Mon, 10 Nov 2025 22:42:06 -0500 Subject: [PATCH 12/29] 11/10/25 --- TeamCode/build.gradle | 1 + .../ftc/teamcode/common/commands/AutoFire.kt | 1 + .../ftc/teamcode/common/normalizeRadians.kt | 10 ------- .../teamcode/common/subsystem/Apriltags.kt | 2 +- .../common/subsystem/AutoTargetting.kt | 20 +++++++++++--- .../firstinspires/ftc/teamcode/common/util.kt | 24 +++++++++++++++++ .../teamcode/common/vision/VisionManager.kt | 4 +-- .../ftc/teamcode/opmode/teleop/Teleop.kt | 27 ++++++++++++++++++- build.dependencies.gradle | 1 + 9 files changed, 72 insertions(+), 18 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index eaa46c5253e2..e043c4d02603 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -24,6 +24,7 @@ dependencies { implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:01bc6d6334' implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:01bc6d6334' +// maven local // implementation('com.millburnx:cmdx:+') // implementation('com.millburnx:cmdxpedro:+') diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index 84ee8e739f89..862231e03e8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -30,6 +30,7 @@ fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: U } } prevButton = currentButton + sync() } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt deleted file mode 100644 index 12b75ced2c89..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/normalizeRadians.kt +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.teamcode.common - -import kotlin.math.floor - -fun normalizeRadians(radians: Double): Double { - val temp = (radians + Math.PI) / (2.0 * Math.PI) - return (temp - floor(temp) - 0.5) * 2.0 -} - -fun normalizeDegrees(angle: Double): Double = Math.toDegrees(normalizeRadians(Math.toRadians(angle))) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt index 89c52c0899af..c56c9a7854aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -35,6 +35,6 @@ class Apriltags : Subsystem("Apriltags") { } fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d { - val offset = Vec2d(ftcPose.range).rotate(currentPose.radians - ftcPose.bearing.toRadians()) + val offset = Vec2d(ftcPose.range, 0).rotate(currentPose.radians + ftcPose.bearing.toRadians()) return Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw) } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt index e327b0b5382c..dd808406f6dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt @@ -4,18 +4,28 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.toDegrees +import org.firstinspires.ftc.teamcode.common.roundTo import org.firstinspires.ftc.teamcode.opmode.OpMode class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") { + + var enabled = false + var target: Pose2d? = null + override val run: suspend Command.() -> Unit = { with (opMode) { WaitFor { isStarted || isStopRequested } - var enabled = false - var target: Pose2d? = null var prevButton = gp1.y while (!isStopRequested) { val apriltag = apriltags.apriltags[20] ?: apriltags.apriltags[24] - if (apriltag != null) target = apriltag.toPose(pedro.pose) + if (apriltag != null && !enabled) target = apriltag.toPose(pedro.pose) + + tel.addData("target x", target?.x?.roundTo(2) ?: -1.0) + tel.addData("target y", target?.y?.roundTo(2) ?: -1.0) + tel.addData("target heading", target?.heading?.roundTo(2) ?: -1.0) + + tel.addData("tag range", apriltag?.ftcPose?.range?.roundTo(2) ?: -1.0) + tel.addData("tag bearing", apriltag?.ftcPose?.bearing?.roundTo(2) ?: -1.0) val currentButton = gp1.y if (currentButton && !prevButton) { @@ -25,11 +35,13 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy if (!enabled || target == null) { pedro.isLocked = false + sync() continue } pedro.isLocked = true - val targetHeading = pedro.pose.position.angle(target.position) + val targetHeading = pedro.pose.position.angle(target!!.position) pedro.headingLock.targetHeading = targetHeading.toDegrees() + sync() } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt new file mode 100644 index 000000000000..13c87a75ee71 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.common + +import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import java.util.* +import kotlin.math.floor +import kotlin.math.pow +import kotlin.math.round + +fun normalizeRadians(radians: Double): Double { + val temp = (radians + Math.PI) / (2.0 * Math.PI) + return (temp - floor(temp) - 0.5) * 2.0 +} + +fun normalizeDegrees(angle: Double): Double = Math.toDegrees(normalizeRadians(Math.toRadians(angle))) + +fun Pose2d.simplifiedString(): String = String.format(Locale.US ,"(%.2f, %.2f, %.0f)", x, y, heading) + +fun Vec2d.simplifiedString(): String = String.format(Locale.US ,"(%.2f, %.2f)", x, y) + +fun Double.roundTo(n: Int): Double { + val factor = 10.0.pow(n) + return round(this * factor) / factor +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt index d6a7e64f865b..0b5ee97faad1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -2,8 +2,8 @@ package org.firstinspires.ftc.teamcode.common.vision import android.util.Size import com.bylazar.configurables.annotations.Configurable -import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName +import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.vision.VisionPortal import org.firstinspires.ftc.vision.VisionProcessor @@ -16,7 +16,7 @@ object VisionManager { processors.clear() } - fun build() = + fun build(opMode: OpMode): VisionPortal = VisionPortal .Builder() .setCamera(opMode.hardwareMap["cam1"] as WebcamName) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index 908bfb62a3e6..803eb4f4a993 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.opmode.teleop +import com.bylazar.field.PanelsField +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.common.commands.TeleopAutoFire import org.firstinspires.ftc.teamcode.common.subsystem.Apriltags @@ -25,10 +28,32 @@ class Teleop : OpMode() { // effectively auto calling VisionManager.init VisionManager.init() val apriltags = Apriltags() - VisionManager.build() + VisionManager.build(this) val autoTargetting = AutoTargetting(this, pedro, apriltags) + scheduler.schedule(Command("drawer") { + while (!isStopRequested) { + val field = PanelsField.field + field.setOffsets(PanelsField.presets.PEDRO_PATHING) + field.setStyle("transparent", "red", 2.0) + field.moveCursor(pedro.pose.x, pedro.pose.y) + field.circle(7.0) + field.setFill("red") + val lookVector = pedro.pose.position + Vec2d(7.0).rotate(pedro.pose.radians) + field.line(lookVector.x, lookVector.y) + + val target = autoTargetting.target + if (target != null) { + field.setStyle("blue", "blue", 2.0) + field.moveCursor(target.x, target.y) + field.circle(2.0) + } + field.update() + sync() + } + }) + scheduler.schedule(TeleopAutoFire(this, intake, flyWheel, uppies)) // scheduler.schedule(TeleOpStopper(this)) } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index a24668708e1e..6c3f16adbefb 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -14,6 +14,7 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'com.pedropathing:ftc:2.0.3' implementation 'com.pedropathing:telemetry:1.0.0' implementation 'com.bylazar:fullpanels:1.0.9' From 61e4b2328aade40bf1542497bf991f4e4a3ac45f Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Mon, 10 Nov 2025 22:47:10 -0500 Subject: [PATCH 13/29] Panels Camera Integration --- .../teamcode/common/subsystem/Apriltags.kt | 5 +- .../common/vision/PanelsIntegration.kt | 46 +++++++++++++++++++ .../teamcode/common/vision/VisionManager.kt | 2 +- 3 files changed, 49 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/PanelsIntegration.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt index c56c9a7854aa..a17be1d23544 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -8,7 +8,6 @@ import org.firstinspires.ftc.teamcode.common.vision.VisionManager import org.firstinspires.ftc.vision.apriltag.AprilTagDetection import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor.PoseSolver class Apriltags : Subsystem("Apriltags") { val processor: AprilTagProcessor = AprilTagProcessor.Builder() @@ -16,8 +15,8 @@ class Apriltags : Subsystem("Apriltags") { .setDrawAxes(true) .setDrawCubeProjection(true) .build().apply { - setDecimation(2F) - setPoseSolver(PoseSolver.OPENCV_IPPE) + setDecimation(3F) +// setPoseSolver(PoseSolver.OPENCV_IPPE) } init { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/PanelsIntegration.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/PanelsIntegration.kt new file mode 100644 index 000000000000..b472b6f2e422 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/PanelsIntegration.kt @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.common.vision + +import android.graphics.Bitmap +import android.graphics.Bitmap.createBitmap +import android.graphics.Canvas +import org.firstinspires.ftc.robotcore.external.function.Consumer +import org.firstinspires.ftc.robotcore.external.function.Continuation +import org.firstinspires.ftc.robotcore.external.stream.CameraStreamSource +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration +import org.firstinspires.ftc.vision.VisionProcessor +import org.opencv.android.Utils +import org.opencv.core.Mat +import java.util.concurrent.atomic.AtomicReference + +class PanelsIntegration : VisionProcessor, CameraStreamSource { + private val lastFrame = AtomicReference(createBitmap(1, 1, Bitmap.Config.RGB_565)) + + override fun init( + width: Int, height: Int, calibration: CameraCalibration + ) { + lastFrame.set(createBitmap(width, height, Bitmap.Config.RGB_565)) + } + + override fun processFrame(frame: Mat, captureTimeNanos: Long): Any? { + val b = createBitmap(frame.width(), frame.height(), Bitmap.Config.RGB_565) + Utils.matToBitmap(frame, b) + lastFrame.set(b) + return null + } + + override fun onDrawFrame( + canvas: Canvas, + onscreenWidth: Int, + onscreenHeight: Int, + scaleBmpPxToCanvasPx: Float, + scaleCanvasDensity: Float, + userContext: Any? + ) { + } + + override fun getFrameBitmap(continuation: Continuation>) { + continuation.dispatch { bitmapConsumer -> + bitmapConsumer.accept(lastFrame.get()) + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt index 0b5ee97faad1..f05a76ec8e52 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -20,7 +20,7 @@ object VisionManager { VisionPortal .Builder() .setCamera(opMode.hardwareMap["cam1"] as WebcamName) - .addProcessors(*processors.toTypedArray()) + .addProcessors(PanelsIntegration(), *processors.toTypedArray()) .setCameraResolution(Size(cameraSizeX, cameraSizeY)) .setStreamFormat(VisionPortal.StreamFormat.MJPEG) .enableLiveView(true) From 7d7972129e98548a1355b438f07951c317188c90 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Tue, 11 Nov 2025 02:58:22 -0500 Subject: [PATCH 14/29] Gamepad State Manager --- .../ftc/teamcode/common/commands/AutoFire.kt | 30 +++---- .../common/hardware/GamepadManager.kt | 87 +++++++++++++++++++ .../common/subsystem/AutoTargetting.kt | 5 +- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 12 +-- .../ftc/teamcode/common/subsystem/Intake.kt | 2 +- .../ftc/teamcode/common/subsystem/Pedro.kt | 10 +-- .../ftc/teamcode/common/subsystem/Uppies.kt | 19 +--- .../teamcode/common/vision/VisionManager.kt | 11 ++- .../ftc/teamcode/opmode/OpMode.kt | 18 ++-- 9 files changed, 130 insertions(+), 64 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index 862231e03e8b..d7d5d7b97aa2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -14,26 +14,24 @@ import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode -fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = Command("Auto Fire Triggerer") { - with (opMode) { - val autoFire = AutoFire(this, intake, flyWheel, uppies) - WaitFor { isStarted || isStopRequested } - var prevButton = gp1.a - while (!isStopRequested) { - val currentButton = gp1.a - if (currentButton && !prevButton) { - if (autoFire.currentScope?.isActive ?: false) { - // already scheduled, cancel - autoFire.cancel() - } else { - scheduler.schedule(autoFire) +fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = + Command("Auto Fire Triggerer") { + with(opMode) { + val autoFire = AutoFire(this, intake, flyWheel, uppies) + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + if (gp1.current.a && !gp1.prev.a) { + if (autoFire.currentScope?.isActive ?: false) { + // already scheduled, cancel + autoFire.cancel() + } else { + scheduler.schedule(autoFire) + } } + sync() } - prevButton = currentButton - sync() } } -} fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Sequential { with(opMode) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt new file mode 100644 index 000000000000..ff351252156e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.common.hardware + +import com.bylazar.gamepad.PanelsGamepad +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode +import org.firstinspires.ftc.teamcode.opmode.OpMode + +class GamepadManager(opMode: OpMode) { + private val _panelsGamepad1: com.bylazar.gamepad.GamepadManager = PanelsGamepad.firstManager + private val _panelsGamepad2: com.bylazar.gamepad.GamepadManager = PanelsGamepad.secondManager + + private val _gamepad1: com.qualcomm.robotcore.hardware.Gamepad + get() = _panelsGamepad1.asCombinedFTCGamepad(opMode.gamepad1) + private val _gamepad2: com.qualcomm.robotcore.hardware.Gamepad + get() = _panelsGamepad2.asCombinedFTCGamepad(opMode.gamepad2) + + val gamepad1: Gamepad = Gamepad(_gamepad1) + val gamepad2: Gamepad = Gamepad(_gamepad2) + + fun update() { + val oldState1 = gamepad1.current + val oldState2 = gamepad2.current + + val newState1 = GamepadState(_gamepad1) + val newState2 = GamepadState(_gamepad2) + gamepad1.current = newState1 + gamepad2.current = newState2 + + gamepad1.prev = oldState1 + gamepad2.prev = oldState2 + } +} + +class Gamepad(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) { + var current: GamepadState = GamepadState(gamepad) + var prev: GamepadState = GamepadState(gamepad) +} + +class GamepadState(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) { + val leftJoyStick = JoyStick( + gamepad.left_stick_x.toDouble(), + gamepad.left_stick_y.toDouble(), + gamepad.left_stick_button + ) + + val rightJoyStick = JoyStick( + gamepad.right_stick_x.toDouble(), + gamepad.right_stick_y.toDouble(), + gamepad.right_stick_button + ) + + val dPad = DPad( + gamepad.dpad_left, + gamepad.dpad_up, + gamepad.dpad_right, + gamepad.dpad_down + ) + + val a: Boolean = gamepad.a + val b: Boolean = gamepad.b + val x: Boolean = gamepad.x + val y: Boolean = gamepad.y + + val guide: Boolean = gamepad.guide + val start: Boolean = gamepad.start + val back = gamepad.back + + val leftBumper: Boolean = gamepad.left_bumper + val rightBumper: Boolean = gamepad.right_bumper + + val leftTrigger: Double = gamepad.left_trigger.toDouble() + val rightTrigger: Double = gamepad.right_trigger.toDouble() +} + +class JoyStick(var x: Double, var y: Double, var down: Boolean) { + val vector: Vec2d + get() = Vec2d(x, y) +} + +class DPad(var left: Boolean, var up: Boolean, var right: Boolean, var down: Boolean) { + val vector: Vec2d + get() { + val x = (if (right) 1.0 else 0.0) - (if (left) 1.0 else 0.0) + val y = (if (up) 1.0 else 0.0) - (if (down) 1.0 else 0.0) + return Vec2d(x, y) + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt index dd808406f6dd..e93cffa3eb44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt @@ -15,7 +15,6 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy override val run: suspend Command.() -> Unit = { with (opMode) { WaitFor { isStarted || isStopRequested } - var prevButton = gp1.y while (!isStopRequested) { val apriltag = apriltags.apriltags[20] ?: apriltags.apriltags[24] if (apriltag != null && !enabled) target = apriltag.toPose(pedro.pose) @@ -27,11 +26,9 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy tel.addData("tag range", apriltag?.ftcPose?.range?.roundTo(2) ?: -1.0) tel.addData("tag bearing", apriltag?.ftcPose?.bearing?.roundTo(2) ?: -1.0) - val currentButton = gp1.y - if (currentButton && !prevButton) { + if (gp1.current.y && !gp1.prev.y) { enabled = !enabled } - prevButton = currentButton if (!enabled || target == null) { pedro.isLocked = false diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index 7e0ea9291c4c..181aefa2d08b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -25,8 +25,6 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh return leftAtVelocity && rightAtVelocity } - val teleopState = TeleopData() - override val run: suspend Command.() -> Unit = { with(opMode) { WaitFor { isStarted || isStopRequested } @@ -52,16 +50,12 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh private fun OpMode.teleOpControls() { if (isTeleop) { - val currentX = gp1.x - val currentB = gp1.b - if (currentX && !teleopState.prevX) { + if (gp1.current.x && gp1.current.x) { state = if (state == State.SHOOTING) State.IDLE else State.SHOOTING } - if (currentB && !teleopState.prevB) { + if (gp1.current.b && gp1.current.b) { state = if (state == State.INTAKING) State.IDLE else State.INTAKING } - teleopState.prevX = currentX - teleopState.prevB = currentB } } @@ -81,8 +75,6 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh @JvmField var velocityThreshold = 200.0 } - - data class TeleopData(var prevX: Boolean = false, var prevB: Boolean = false) } @Configurable diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt index 4067f9ee094c..56fe44f5f2e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt @@ -14,7 +14,7 @@ class Intake(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Intake" WaitFor { isStarted || isStopRequested } while (!isStopRequested) { if (isTeleop) { - power = (gp1.right_trigger - gp1.left_trigger).toDouble(); + power = gp1.current.rightTrigger - gp1.current.leftTrigger; } motor.power = power sync() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 9380ac8b7238..25f7da107ef3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -36,16 +36,16 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") if (isTeleop) { if (isLocked) { follower.setTeleOpDrive( - -gp1.left_stick_y.toDouble(), - -gp1.left_stick_x.toDouble(), + -gp1.current.leftJoyStick.y, + -gp1.current.leftJoyStick.x, headingLock.calc().magnitude, false ) } else { follower.setTeleOpDrive( - -gp1.left_stick_y.toDouble(), - -gp1.left_stick_x.toDouble(), - -gp1.right_stick_x.toDouble() + -gp1.current.leftJoyStick.y, + -gp1.current.leftJoyStick.x, + -gp1.current.rightJoyStick.x ) } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index 863869604dc6..900e3fad22d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -29,8 +29,6 @@ class Uppies( val pid = PIDController(kP, kI, kD) - val teleopState = TeleopData() - fun prevState() { state = if (state == State.LOADED) { if (flyWheelState() == FlyWheel.State.INTAKING) { @@ -89,26 +87,15 @@ class Uppies( } private fun OpMode.teleOpControls() { - if (isTeleop) { - val currentLeftBumper = gp1.left_bumper - val currentRightBumper = gp1.right_bumper - if (currentLeftBumper && !teleopState.prevLeftBumper) { - prevState() - } - if (currentRightBumper && !teleopState.prevRightBumper) { - nextState() - } - teleopState.prevLeftBumper = currentLeftBumper - teleopState.prevRightBumper = currentRightBumper - } + if (isTeleop) return + if (gp1.current.leftBumper && !gp1.prev.leftBumper) prevState() + if (gp1.current.rightBumper && !gp1.prev.rightBumper) nextState() } enum class State { INTAKE_READY, LOADER_READY, LOADED, } - data class TeleopData(var prevLeftBumper: Boolean = false, var prevRightBumper: Boolean = false) - companion object { @JvmField var kP = 2.0 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt index f05a76ec8e52..442f3780f12c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.common.vision import android.util.Size +import com.bylazar.camerastream.PanelsCameraStream import com.bylazar.configurables.annotations.Configurable import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName import org.firstinspires.ftc.teamcode.opmode.OpMode @@ -16,16 +17,20 @@ object VisionManager { processors.clear() } - fun build(opMode: OpMode): VisionPortal = - VisionPortal + fun build(opMode: OpMode): VisionPortal { + val panelsIntegration = PanelsIntegration() + val portal = VisionPortal .Builder() .setCamera(opMode.hardwareMap["cam1"] as WebcamName) - .addProcessors(PanelsIntegration(), *processors.toTypedArray()) + .addProcessors(panelsIntegration, *processors.toTypedArray()) .setCameraResolution(Size(cameraSizeX, cameraSizeY)) .setStreamFormat(VisionPortal.StreamFormat.MJPEG) .enableLiveView(true) .setAutoStopLiveView(true) .build() + PanelsCameraStream.startStream(panelsIntegration) + return portal + } @JvmField var cameraSizeX = 1280 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt index 6b54a1056020..99602ebc0015 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -1,25 +1,23 @@ package org.firstinspires.ftc.teamcode.opmode -import com.bylazar.gamepad.GamepadManager -import com.bylazar.gamepad.PanelsGamepad +import com.bylazar.camerastream.PanelsCameraStream import com.bylazar.telemetry.PanelsTelemetry import com.millburnx.cmdx.runtimeGroups.CommandScheduler import com.qualcomm.hardware.lynx.LynxModule import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.hardware.Gamepad import com.qualcomm.robotcore.util.ElapsedTime +import org.firstinspires.ftc.teamcode.common.hardware.Gamepad +import org.firstinspires.ftc.teamcode.common.hardware.GamepadManager import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualManager import org.firstinspires.ftc.teamcode.common.subsystem.SubsystemManager abstract class OpMode : LinearOpMode() { val tel = PanelsTelemetry.telemetry - val gm1: GamepadManager = PanelsGamepad.firstManager - val gm2: GamepadManager = PanelsGamepad.secondManager - val gp1: Gamepad - get() = gm1.asCombinedFTCGamepad(gamepad1) - val gp2: Gamepad - get() = gm2.asCombinedFTCGamepad(gamepad2) + val gamepadManager: GamepadManager = GamepadManager(this) + + val gp1: Gamepad = gamepadManager.gamepad1 + val gp2: Gamepad = gamepadManager.gamepad2 val loopTimer = ElapsedTime() var hubs: List = emptyList() @@ -36,6 +34,7 @@ abstract class OpMode : LinearOpMode() { // hardware hubs.forEach { it.clearBulkCache() } ManualManager.update() + gamepadManager.update() } } @@ -56,5 +55,6 @@ abstract class OpMode : LinearOpMode() { while (opModeIsActive()) { } scheduler.runner.cancel() // Clean up faulty commands + PanelsCameraStream.stopStream() } } \ No newline at end of file From f442cb7aa329599a0833ee3e368e177497fe5f7f Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Tue, 11 Nov 2025 03:06:51 -0500 Subject: [PATCH 15/29] Revert to static lock --- .../common/subsystem/AutoTargetting.kt | 19 +++-- .../ftc/teamcode/common/subsystem/Pedro.kt | 69 ++----------------- 2 files changed, 15 insertions(+), 73 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt index e93cffa3eb44..d4a48c23542d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.common.subsystem import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor -import com.millburnx.cmdxpedro.util.toDegrees import org.firstinspires.ftc.teamcode.common.roundTo import org.firstinspires.ftc.teamcode.opmode.OpMode @@ -13,7 +12,7 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy var target: Pose2d? = null override val run: suspend Command.() -> Unit = { - with (opMode) { + with(opMode) { WaitFor { isStarted || isStopRequested } while (!isStopRequested) { val apriltag = apriltags.apriltags[20] ?: apriltags.apriltags[24] @@ -29,15 +28,15 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy if (gp1.current.y && !gp1.prev.y) { enabled = !enabled } - - if (!enabled || target == null) { - pedro.isLocked = false - sync() - continue + val newLocked = enabled && target != null + if (newLocked != pedro.isLocked) { + pedro.isLocked = newLocked + if (newLocked) { + pedro.follower.turnTo(pedro.pose.position.angle(target!!.position)) + } else { + pedro.follower.startTeleopDrive(true) + } } - pedro.isLocked = true - val targetHeading = pedro.pose.position.angle(target!!.position) - pedro.headingLock.targetHeading = targetHeading.toDegrees() sync() } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 25f7da107ef3..b3ab00fae986 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -4,23 +4,15 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.toDegrees -import com.millburnx.cmdxpedro.util.toRadians -import com.pedropathing.control.PIDFController import com.pedropathing.geometry.Pose -import com.pedropathing.math.MathFunctions -import com.pedropathing.math.Vector -import org.firstinspires.ftc.teamcode.common.normalizeDegrees import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.teamcode.pedro.Constants -import kotlin.math.abs class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") { val follower = Constants.createFollower(opMode.hardwareMap).apply { setStartingPose(Pose(0.0, 0.0, 0.0)) } - - val headingLock = HeadingLock(this) var isLocked = false; val pose: Pose2d @@ -33,64 +25,15 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") WaitFor { isStarted || isStopRequested } while (!isStopRequested) { follower.update() - if (isTeleop) { - if (isLocked) { - follower.setTeleOpDrive( - -gp1.current.leftJoyStick.y, - -gp1.current.leftJoyStick.x, - headingLock.calc().magnitude, - false - ) - } else { - follower.setTeleOpDrive( - -gp1.current.leftJoyStick.y, - -gp1.current.leftJoyStick.x, - -gp1.current.rightJoyStick.x - ) - } + if (isTeleop && !isLocked) { + follower.setTeleOpDrive( + -gp1.current.leftJoyStick.y, + -gp1.current.leftJoyStick.x, + -gp1.current.rightJoyStick.x + ) } sync() } } } -} - -class HeadingLock(val pedro: Pedro) { - private val constants = pedro.follower.constants - - private val headingPIDF = PIDFController(constants.coefficientsHeadingPIDF) - private val secondaryHeadingPIDF = PIDFController(constants.coefficientsSecondaryHeadingPIDF) - private val headingPIDFSwitch = constants.headingPIDFSwitch - var targetHeading = 0.0 - - fun calc(): Vector { - val headingError = normalizeDegrees(targetHeading - pedro.pose.heading).toRadians() - if (abs(headingError) < headingPIDFSwitch && - constants.useSecondaryHeadingPIDF - ) { - secondaryHeadingPIDF.updateFeedForwardInput( - MathFunctions.getTurnDirection( - pedro.pose.radians, - targetHeading.toRadians() - ) - ) - secondaryHeadingPIDF.updateError(headingError) - val headingVector = Vector( - secondaryHeadingPIDF.run().coerceIn(-1.0, 1.0), - pedro.pose.radians - ) - return headingVector.copy() - } - - headingPIDF.updateFeedForwardInput( - MathFunctions.getTurnDirection( - pedro.pose.radians, - targetHeading.toRadians() - ) - ) - headingPIDF.updateError(headingError) - val headingVector = - Vector(headingPIDF.run().coerceIn(-1.0, 1.0), pedro.pose.radians) - return headingVector.copy() - } } \ No newline at end of file From 7eb5f0d31948ca10385512ec3c7313df755806b8 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Tue, 11 Nov 2025 03:16:02 -0500 Subject: [PATCH 16/29] Axis Snapping --- .../ftc/teamcode/common/subsystem/Pedro.kt | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index b3ab00fae986..31245c088474 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.common.subsystem +import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor @@ -7,8 +8,10 @@ import com.millburnx.cmdxpedro.util.toDegrees import com.pedropathing.geometry.Pose import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.teamcode.pedro.Constants +import kotlin.math.abs +import kotlin.math.pow - +@Configurable class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") { val follower = Constants.createFollower(opMode.hardwareMap).apply { setStartingPose(Pose(0.0, 0.0, 0.0)) @@ -26,14 +29,28 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") while (!isStopRequested) { follower.update() if (isTeleop && !isLocked) { + // axis snapping + val snappedX = snapTo(gp1.current.leftJoyStick.x, 0.0, snapThreshold) + val snappedY = snapTo(gp1.current.leftJoyStick.y, 0.0, snapThreshold) + follower.setTeleOpDrive( - -gp1.current.leftJoyStick.y, - -gp1.current.leftJoyStick.x, - -gp1.current.rightJoyStick.x + -snappedX.pow(3), + -snappedY.pow(3), + -gp1.current.rightJoyStick.x.pow(3) ) } sync() } } } + + private fun snapTo(raw: Double, target: Double, threshold: Double): Double { + if (abs(raw - target) < threshold) return target + return raw + } + + companion object { + @JvmField + var snapThreshold = 0.1; + } } \ No newline at end of file From ccf59846b2ef43dbb84397d45afa3d160a533e80 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Tue, 11 Nov 2025 13:55:00 -0500 Subject: [PATCH 17/29] 11/11/25 --- TeamCode/build.gradle | 4 +- .../common/hardware/GamepadManager.kt | 87 ------------------- .../teamcode/common/hardware/gamepad/DPad.kt | 12 +++ .../common/hardware/gamepad/Gamepad.kt | 6 ++ .../common/hardware/gamepad/GamepadManager.kt | 31 +++++++ .../common/hardware/gamepad/GamepadState.kt | 37 ++++++++ .../common/hardware/gamepad/JoyStick.kt | 8 ++ .../hardware/gamepad/SlewRateLimiter.kt | 26 ++++++ .../teamcode/common/subsystem/Apriltags.kt | 2 +- .../{AutoTargetting.kt => AutoTargeting.kt} | 4 +- .../ftc/teamcode/common/subsystem/Pedro.kt | 47 ++++++++-- .../firstinspires/ftc/teamcode/common/util.kt | 5 ++ .../teamcode/common/vision/VisionManager.kt | 7 +- .../ftc/teamcode/opmode/OpMode.kt | 23 +++-- .../ftc/teamcode/opmode/teleop/Teleop.kt | 9 +- 15 files changed, 191 insertions(+), 117 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/DPad.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/Gamepad.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadManager.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadState.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/JoyStick.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/SlewRateLimiter.kt rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/{AutoTargetting.kt => AutoTargeting.kt} (88%) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index e043c4d02603..49cf28d9ca51 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -21,8 +21,8 @@ android { dependencies { implementation project(':FtcRobotController') - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:01bc6d6334' - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:01bc6d6334' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:67adbc2b80' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:67adbc2b80' // maven local // implementation('com.millburnx:cmdx:+') diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt deleted file mode 100644 index ff351252156e..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt +++ /dev/null @@ -1,87 +0,0 @@ -package org.firstinspires.ftc.teamcode.common.hardware - -import com.bylazar.gamepad.PanelsGamepad -import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d -import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode -import org.firstinspires.ftc.teamcode.opmode.OpMode - -class GamepadManager(opMode: OpMode) { - private val _panelsGamepad1: com.bylazar.gamepad.GamepadManager = PanelsGamepad.firstManager - private val _panelsGamepad2: com.bylazar.gamepad.GamepadManager = PanelsGamepad.secondManager - - private val _gamepad1: com.qualcomm.robotcore.hardware.Gamepad - get() = _panelsGamepad1.asCombinedFTCGamepad(opMode.gamepad1) - private val _gamepad2: com.qualcomm.robotcore.hardware.Gamepad - get() = _panelsGamepad2.asCombinedFTCGamepad(opMode.gamepad2) - - val gamepad1: Gamepad = Gamepad(_gamepad1) - val gamepad2: Gamepad = Gamepad(_gamepad2) - - fun update() { - val oldState1 = gamepad1.current - val oldState2 = gamepad2.current - - val newState1 = GamepadState(_gamepad1) - val newState2 = GamepadState(_gamepad2) - gamepad1.current = newState1 - gamepad2.current = newState2 - - gamepad1.prev = oldState1 - gamepad2.prev = oldState2 - } -} - -class Gamepad(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) { - var current: GamepadState = GamepadState(gamepad) - var prev: GamepadState = GamepadState(gamepad) -} - -class GamepadState(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) { - val leftJoyStick = JoyStick( - gamepad.left_stick_x.toDouble(), - gamepad.left_stick_y.toDouble(), - gamepad.left_stick_button - ) - - val rightJoyStick = JoyStick( - gamepad.right_stick_x.toDouble(), - gamepad.right_stick_y.toDouble(), - gamepad.right_stick_button - ) - - val dPad = DPad( - gamepad.dpad_left, - gamepad.dpad_up, - gamepad.dpad_right, - gamepad.dpad_down - ) - - val a: Boolean = gamepad.a - val b: Boolean = gamepad.b - val x: Boolean = gamepad.x - val y: Boolean = gamepad.y - - val guide: Boolean = gamepad.guide - val start: Boolean = gamepad.start - val back = gamepad.back - - val leftBumper: Boolean = gamepad.left_bumper - val rightBumper: Boolean = gamepad.right_bumper - - val leftTrigger: Double = gamepad.left_trigger.toDouble() - val rightTrigger: Double = gamepad.right_trigger.toDouble() -} - -class JoyStick(var x: Double, var y: Double, var down: Boolean) { - val vector: Vec2d - get() = Vec2d(x, y) -} - -class DPad(var left: Boolean, var up: Boolean, var right: Boolean, var down: Boolean) { - val vector: Vec2d - get() { - val x = (if (right) 1.0 else 0.0) - (if (left) 1.0 else 0.0) - val y = (if (up) 1.0 else 0.0) - (if (down) 1.0 else 0.0) - return Vec2d(x, y) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/DPad.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/DPad.kt new file mode 100644 index 000000000000..d613dcb67402 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/DPad.kt @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.common.hardware.gamepad + +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d + +class DPad(var left: Boolean, var up: Boolean, var right: Boolean, var down: Boolean) { + val vector: Vec2d + get() { + val x = (if (right) 1.0 else 0.0) - (if (left) 1.0 else 0.0) + val y = (if (up) 1.0 else 0.0) - (if (down) 1.0 else 0.0) + return Vec2d(x, y) + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/Gamepad.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/Gamepad.kt new file mode 100644 index 000000000000..6b89fc532346 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/Gamepad.kt @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode.common.hardware.gamepad + +class Gamepad(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) { + var current: GamepadState = GamepadState(gamepad) + var prev: GamepadState = GamepadState(gamepad) +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadManager.kt new file mode 100644 index 000000000000..fac837fe9b9a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadManager.kt @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.common.hardware.gamepad + +import com.bylazar.gamepad.GamepadManager +import com.bylazar.gamepad.PanelsGamepad +import org.firstinspires.ftc.teamcode.opmode.OpMode + +class GamepadManager(private val opMode: OpMode) { + private val _panelsGamepad1: GamepadManager = PanelsGamepad.firstManager + private val _panelsGamepad2: GamepadManager = PanelsGamepad.secondManager + + private val _gamepad1: com.qualcomm.robotcore.hardware.Gamepad + get() = _panelsGamepad1.asCombinedFTCGamepad(opMode.gamepad1) + private val _gamepad2: com.qualcomm.robotcore.hardware.Gamepad + get() = _panelsGamepad2.asCombinedFTCGamepad(opMode.gamepad2) + + val gamepad1: Gamepad = Gamepad(_gamepad1) + val gamepad2: Gamepad = Gamepad(_gamepad2) + + fun update() { + val oldState1 = gamepad1.current + val oldState2 = gamepad2.current + + val newState1 = GamepadState(_gamepad1) + val newState2 = GamepadState(_gamepad2) + gamepad1.current = newState1 + gamepad2.current = newState2 + + gamepad1.prev = oldState1 + gamepad2.prev = oldState2 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadState.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadState.kt new file mode 100644 index 000000000000..179e0a77a395 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadState.kt @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode.common.hardware.gamepad + +class GamepadState(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) { + val leftJoyStick = JoyStick( + gamepad.left_stick_x.toDouble(), + gamepad.left_stick_y.toDouble(), + gamepad.left_stick_button + ) + + val rightJoyStick = JoyStick( + gamepad.right_stick_x.toDouble(), + gamepad.right_stick_y.toDouble(), + gamepad.right_stick_button + ) + + val dPad = DPad( + gamepad.dpad_left, + gamepad.dpad_up, + gamepad.dpad_right, + gamepad.dpad_down + ) + + val a: Boolean = gamepad.a + val b: Boolean = gamepad.b + val x: Boolean = gamepad.x + val y: Boolean = gamepad.y + + val guide: Boolean = gamepad.guide + val start: Boolean = gamepad.start + val back = gamepad.back + + val leftBumper: Boolean = gamepad.left_bumper + val rightBumper: Boolean = gamepad.right_bumper + + val leftTrigger: Double = gamepad.left_trigger.toDouble() + val rightTrigger: Double = gamepad.right_trigger.toDouble() +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/JoyStick.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/JoyStick.kt new file mode 100644 index 000000000000..e8ff4a53810c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/JoyStick.kt @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.common.hardware.gamepad + +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d + +class JoyStick(var x: Double, var y: Double, var down: Boolean) { + val vector: Vec2d + get() = Vec2d(x, y) +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/SlewRateLimiter.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/SlewRateLimiter.kt new file mode 100644 index 000000000000..820455db5fba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/SlewRateLimiter.kt @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.common.hardware.gamepad + +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.millburnx.cmdxpedro.util.toDegrees +import org.firstinspires.ftc.teamcode.common.normalizeDegrees +import kotlin.math.abs + +object SlewRateLimiter { + fun limit(current: Double, target: Double, maxRate: Double): Double { + val rate = target - current + return current + rate.coerceIn(-maxRate, maxRate) + } + + fun limit(current: Vec2d, target: Vec2d, maxMagnitudeRate: Double, maxRotationRate: Double): Vec2d { + // if angle diff < 90, slew angle and magnitude, if angle > thresh, focus on slewing magnitude (cross-over!) + val targetAngle = Vec2d().angleTo(target).toDegrees() + val currentAngle = Vec2d().angleTo(current).toDegrees() + val angleDiff = normalizeDegrees(targetAngle - currentAngle) + + if (abs(angleDiff) <= 135) { + // TODO: Implement this stuff + } + + return Vec2d() + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt index a17be1d23544..4618d15124cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -34,6 +34,6 @@ class Apriltags : Subsystem("Apriltags") { } fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d { - val offset = Vec2d(ftcPose.range, 0).rotate(currentPose.radians + ftcPose.bearing.toRadians()) + val offset = Vec2d(ftcPose.range).rotate(currentPose.radians + ftcPose.bearing.toRadians()) return Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw) } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt index d4a48c23542d..a9a3cad3095b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargetting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt @@ -6,7 +6,7 @@ import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.roundTo import org.firstinspires.ftc.teamcode.opmode.OpMode -class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") { +class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") { var enabled = false var target: Pose2d? = null @@ -32,7 +32,7 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy if (newLocked != pedro.isLocked) { pedro.isLocked = newLocked if (newLocked) { - pedro.follower.turnTo(pedro.pose.position.angle(target!!.position)) + pedro.follower.turnTo(pedro.pose.angleTo(target!!)) } else { pedro.follower.startTeleopDrive(true) } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 31245c088474..43f0e8cb6383 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -5,16 +5,19 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.toDegrees -import com.pedropathing.geometry.Pose +import org.firstinspires.ftc.teamcode.common.hardware.gamepad.SlewRateLimiter +import org.firstinspires.ftc.teamcode.common.simplifiedString +import org.firstinspires.ftc.teamcode.common.toPedro import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.teamcode.pedro.Constants import kotlin.math.abs import kotlin.math.pow +import kotlin.math.sign @Configurable -class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") { +class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleop: Boolean = false) : Subsystem("Pedro") { val follower = Constants.createFollower(opMode.hardwareMap).apply { - setStartingPose(Pose(0.0, 0.0, 0.0)) + setStartingPose(startingPose.toPedro()) } var isLocked = false; @@ -22,6 +25,9 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") get() = Pose2d(follower.pose.x, follower.pose.y, follower.pose.heading.toDegrees()) override val run: suspend Command.() -> Unit = { + var prevX = 0.0 + var prevY = 0.0 + with(opMode) { follower.update() if (isTeleop) follower.startTeleopDrive(true) @@ -30,15 +36,21 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") follower.update() if (isTeleop && !isLocked) { // axis snapping - val snappedX = snapTo(gp1.current.leftJoyStick.x, 0.0, snapThreshold) - val snappedY = snapTo(gp1.current.leftJoyStick.y, 0.0, snapThreshold) - + val processedX = processInput(prevX, gp1.current.leftJoyStick.x) + val processedY = processInput(prevY, gp1.current.leftJoyStick.y) + val rx = gp1.current.rightJoyStick.x follower.setTeleOpDrive( - -snappedX.pow(3), - -snappedY.pow(3), - -gp1.current.rightJoyStick.x.pow(3) + -processedY, + -processedX, + -(abs(rx.pow(2)) * sign(rx)) ) + tel.addData("px", processedX) + tel.addData("py", processedY) + + prevX = processedX + prevY = processedY } + tel.addData("pose", pose.simplifiedString()) sync() } } @@ -49,8 +61,25 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") return raw } + private fun processInput(prevValue: Double, newValue: Double): Double { + val prevSnapped = snapTo(prevValue, 0.0, snapThreshold) + val newSnapped = snapTo(newValue, 0.0, snapThreshold) + + val dt = opMode.deltaTime + val slewedResult = SlewRateLimiter.limit( + prevSnapped, + newSnapped, + maxRate * dt + ) + + return abs(slewedResult.pow(2)) * sign(slewedResult) + } + companion object { @JvmField var snapThreshold = 0.1; + + @JvmField + var maxRate = 0.0; } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt index 13c87a75ee71..9d02e0de2cfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.common import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.pedropathing.geometry.Pose import java.util.* import kotlin.math.floor import kotlin.math.pow @@ -21,4 +22,8 @@ fun Vec2d.simplifiedString(): String = String.format(Locale.US ,"(%.2f, %.2f)", fun Double.roundTo(n: Int): Double { val factor = 10.0.pow(n) return round(this * factor) / factor +} + +fun Pose2d.toPedro(): Pose { + return Pose(this.x, this.y, this.radians) } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt index 442f3780f12c..ef8290d0f20b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.common.vision import android.util.Size -import com.bylazar.camerastream.PanelsCameraStream import com.bylazar.configurables.annotations.Configurable import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName import org.firstinspires.ftc.teamcode.opmode.OpMode @@ -18,17 +17,17 @@ object VisionManager { } fun build(opMode: OpMode): VisionPortal { - val panelsIntegration = PanelsIntegration() +// val panelsIntegration = PanelsIntegration() val portal = VisionPortal .Builder() .setCamera(opMode.hardwareMap["cam1"] as WebcamName) - .addProcessors(panelsIntegration, *processors.toTypedArray()) + .addProcessors(*processors.toTypedArray()) .setCameraResolution(Size(cameraSizeX, cameraSizeY)) .setStreamFormat(VisionPortal.StreamFormat.MJPEG) .enableLiveView(true) .setAutoStopLiveView(true) .build() - PanelsCameraStream.startStream(panelsIntegration) +// PanelsCameraStream.startStream(panelsIntegration) return portal } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt index 99602ebc0015..26f5887e8fe3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -1,30 +1,33 @@ package org.firstinspires.ftc.teamcode.opmode -import com.bylazar.camerastream.PanelsCameraStream import com.bylazar.telemetry.PanelsTelemetry import com.millburnx.cmdx.runtimeGroups.CommandScheduler import com.qualcomm.hardware.lynx.LynxModule import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.common.hardware.Gamepad -import org.firstinspires.ftc.teamcode.common.hardware.GamepadManager +import org.firstinspires.ftc.teamcode.common.hardware.gamepad.Gamepad +import org.firstinspires.ftc.teamcode.common.hardware.gamepad.GamepadManager import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualManager import org.firstinspires.ftc.teamcode.common.subsystem.SubsystemManager abstract class OpMode : LinearOpMode() { val tel = PanelsTelemetry.telemetry - val gamepadManager: GamepadManager = GamepadManager(this) + lateinit var gamepadManager: GamepadManager - val gp1: Gamepad = gamepadManager.gamepad1 - val gp2: Gamepad = gamepadManager.gamepad2 + val gp1: Gamepad + get() = gamepadManager.gamepad1 + val gp2: Gamepad + get() = gamepadManager.gamepad2 val loopTimer = ElapsedTime() + var deltaTime = 0.0 var hubs: List = emptyList() val scheduler = CommandScheduler().apply { onSync = { val ms = loopTimer.milliseconds() val loopHertz = 1.0 / loopTimer.seconds() + deltaTime = loopHertz loopTimer.reset() tel.addData("hz", loopHertz) @@ -34,7 +37,9 @@ abstract class OpMode : LinearOpMode() { // hardware hubs.forEach { it.clearBulkCache() } ManualManager.update() - gamepadManager.update() + if (::gamepadManager.isInitialized) { + gamepadManager.update() + } } } @@ -45,6 +50,8 @@ abstract class OpMode : LinearOpMode() { override fun runOpMode() { telemetry.isAutoClear = true + gamepadManager = GamepadManager(this) + SubsystemManager.init() run() SubsystemManager.registerAll(scheduler) @@ -55,6 +62,6 @@ abstract class OpMode : LinearOpMode() { while (opModeIsActive()) { } scheduler.runner.cancel() // Clean up faulty commands - PanelsCameraStream.stopStream() +// PanelsCameraStream.stopStream() } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index 803eb4f4a993..75160c8e1a97 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -2,11 +2,12 @@ package org.firstinspires.ftc.teamcode.opmode.teleop import com.bylazar.field.PanelsField import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.common.commands.TeleopAutoFire import org.firstinspires.ftc.teamcode.common.subsystem.Apriltags -import org.firstinspires.ftc.teamcode.common.subsystem.AutoTargetting +import org.firstinspires.ftc.teamcode.common.subsystem.AutoTargeting import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Pedro @@ -20,7 +21,7 @@ class Teleop : OpMode() { val intake = Intake(this, isTeleop = true) val flyWheel = FlyWheel(this, isTeleop = true) val uppies = Uppies(this, { flyWheel.state }, isTeleop = true) - val pedro = Pedro(this, isTeleop = true) + val pedro = Pedro(this, Pose2d(72.0, 72.0, 90.0), isTeleop = true) // we can modify this to auto init probably // can set a flag on VisionManager.build @@ -30,7 +31,7 @@ class Teleop : OpMode() { val apriltags = Apriltags() VisionManager.build(this) - val autoTargetting = AutoTargetting(this, pedro, apriltags) + val autoTargeting = AutoTargeting(this, pedro, apriltags) scheduler.schedule(Command("drawer") { while (!isStopRequested) { @@ -43,7 +44,7 @@ class Teleop : OpMode() { val lookVector = pedro.pose.position + Vec2d(7.0).rotate(pedro.pose.radians) field.line(lookVector.x, lookVector.y) - val target = autoTargetting.target + val target = autoTargeting.target if (target != null) { field.setStyle("blue", "blue", 2.0) field.moveCursor(target.x, target.y) From a6f2de6e835920115c308578e32db751dac2bc11 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Tue, 11 Nov 2025 21:08:58 -0500 Subject: [PATCH 18/29] 11/11/25 - 21 --- TeamCode/build.gradle | 4 +- .../ftc/teamcode/common/commands/AutoFire.kt | 31 +++++++++++++- .../common/subsystem/AutoTargeting.kt | 2 +- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 41 +++++++++++-------- .../ftc/teamcode/common/subsystem/Pedro.kt | 4 +- .../ftc/teamcode/common/subsystem/Uppies.kt | 4 +- .../ftc/teamcode/opmode/OpMode.kt | 4 ++ 7 files changed, 64 insertions(+), 26 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 49cf28d9ca51..a957a74587f3 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -21,8 +21,8 @@ android { dependencies { implementation project(':FtcRobotController') - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:67adbc2b80' - implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:67adbc2b80' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:aef9d51a78' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:aef9d51a78' // maven local // implementation('com.millburnx:cmdx:+') diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index d7d5d7b97aa2..db14afe8d55c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -6,9 +6,13 @@ import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.util.SleepFor import com.millburnx.cmdxpedro.util.WaitFor import kotlinx.coroutines.isActive +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.firstBallDelay import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakeDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.jamDuration +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.postFinishDuration +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.postLoadDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.unjamDuration +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.unjamPower import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies @@ -24,6 +28,9 @@ fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: U if (autoFire.currentScope?.isActive ?: false) { // already scheduled, cancel autoFire.cancel() + unlock(intake, flyWheel, uppies) + intake.power = 0.0 + flyWheel.state = FlyWheel.State.IDLE } else { scheduler.schedule(autoFire) } @@ -42,7 +49,7 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) suspend fun Command.attemptUnjam() { uppies.prevState() - intake.power = -0.25 + intake.power = -unjamPower SleepFor({ isStopRequested }) { unjamDuration } } @@ -58,7 +65,7 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) return !uppies.atTarget } - suspend fun Command.loadBall(jamProtection: Boolean = false) { + suspend fun Command.loadBall(jamProtection: Boolean = AutoFireSettings.jamProtection) { if (!jamProtection) { intake.power = 0.5 SleepFor({ isStopRequested }) { intakeDuration } @@ -70,6 +77,7 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) attemptLoad() } } + SleepFor { postLoadDuration } } return Sequential("Auto Fire") { @@ -84,12 +92,16 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) loadBall() } } + Command("First Ball Wait") { + SleepFor { firstBallDelay } + } Command("Shoot First Ball") { shootBall() } Command("Load Second Ball") { loadBall() } Command("Shoot Second Ball") { shootBall() } Command("Load Third Ball") { loadBall() } Command("Shoot Third Ball") { shootBall() } Command("Reset Subsystems") { + SleepFor { postFinishDuration } intake.power = 0.0 flyWheel.state = FlyWheel.State.IDLE unlock(intake, flyWheel, uppies) @@ -108,6 +120,21 @@ object AutoFireSettings { @JvmField var unjamDuration = 500L + + @JvmField + var unjamPower = -0.1 + + @JvmField + var firstBallDelay = 1000L + + @JvmField + var postLoadDuration = 500L + + @JvmField + var jamProtection = true + + @JvmField + var postFinishDuration = 1000L } //fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) = Command("AutoFire") { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt index a9a3cad3095b..9c626c375bd6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt @@ -34,7 +34,7 @@ class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsys if (newLocked) { pedro.follower.turnTo(pedro.pose.angleTo(target!!)) } else { - pedro.follower.startTeleopDrive(true) + pedro.follower.startTeleopDrive(false) } } sync() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index 181aefa2d08b..51677e256732 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -20,8 +20,8 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh val atVelocity: Boolean get() { - val leftAtVelocity = abs(leftMotor.velocity - targetVelocity) <= velocityThreshold - val rightAtVelocity = abs(-rightMotor.velocity - targetVelocity) <= velocityThreshold + val leftAtVelocity = abs(targetVelocity + leftMotor.velocity) <= velocityThreshold + val rightAtVelocity = abs(targetVelocity - rightMotor.velocity) <= velocityThreshold return leftAtVelocity && rightAtVelocity } @@ -37,12 +37,13 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh State.INTAKING -> IntakingVelocity } - leftMotor.power = leftController.calculate(leftMotor.velocity, targetVelocity, 0.0) - rightMotor.power = rightController.calculate(-rightMotor.velocity, targetVelocity, 0.0) + leftMotor.power = leftController.calculate(-leftMotor.velocity, targetVelocity, voltageSensor.voltage) + rightMotor.power = rightController.calculate(rightMotor.velocity, targetVelocity, voltageSensor.voltage) tel.addData("fw state", state) - tel.addData("fw-l velocity", leftMotor.velocity) - tel.addData("fw-r velocity", -rightMotor.velocity) + tel.addData("fw-l velocity", -leftMotor.velocity) + tel.addData("fw-r velocity", rightMotor.velocity) + tel.addData("voltage", voltageSensor.voltage) sync() } } @@ -50,10 +51,11 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh private fun OpMode.teleOpControls() { if (isTeleop) { - if (gp1.current.x && gp1.current.x) { +// println("${gp1.prev.x} -> ${gp1.current.x} | ${gp1.prev} -> ${gp1.current.x}") + if (gp1.current.x && !gp1.prev.x) { state = if (state == State.SHOOTING) State.IDLE else State.SHOOTING } - if (gp1.current.b && gp1.current.b) { + if (gp1.current.b && !gp1.prev.b) { state = if (state == State.INTAKING) State.IDLE else State.INTAKING } } @@ -67,33 +69,38 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh companion object { @JvmField - var ShootingVelocity = 1600.0 + var ShootingVelocity = 2000.0 @JvmField var IntakingVelocity = -1600.0 @JvmField - var velocityThreshold = 200.0 + var velocityThreshold = 50.0 } } @Configurable class FlyWheelController() { val PID = PIDController(kP, kI, kD) - val FF = SimpleMotorFeedforward(kS, kV, 0.0) + var FF = SimpleMotorFeedforward(kS, kV, 0.0) fun calculate(currentVelocity: Double, targetVelocity: Double, voltage: Double): Double { + PID.setPID(kP, kI, kD) + if (FF.ks != kS || FF.kv != kV) { + FF = SimpleMotorFeedforward(kS, kV, 0.0) + } + val pid = PID.calculate(currentVelocity, targetVelocity) val ff = FF.calculate(targetVelocity) - val voltageCompensation = (12.0 / voltage) - val weightedVoltageCompensation = 1 + (1 - voltageCompensation) * voltageWeight + val voltageCompensation = if (voltage != 0.0) (12.0 / voltage) else 1.0 + val weightedVoltageCompensation = 1 - (1 - voltageCompensation) * voltageWeight return (pid + ff) * weightedVoltageCompensation } companion object { @JvmField - var kP = 0.0 + var kP = 0.001 @JvmField var kI = 0.0 @@ -102,12 +109,12 @@ class FlyWheelController() { var kD = 0.0 @JvmField - var kS = 0.0 + var kS = 0.04 @JvmField - var kV = 0.0 + var kV = 0.00037 @JvmField - var voltageWeight = 0.0 + var voltageWeight = 1.0 } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 43f0e8cb6383..97970385ac05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -30,7 +30,7 @@ class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleo with(opMode) { follower.update() - if (isTeleop) follower.startTeleopDrive(true) + if (isTeleop) follower.startTeleopDrive(false) WaitFor { isStarted || isStopRequested } while (!isStopRequested) { follower.update() @@ -80,6 +80,6 @@ class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleo var snapThreshold = 0.1; @JvmField - var maxRate = 0.0; + var maxRate = 4.0; } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index 900e3fad22d8..160c8bd51b70 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -87,7 +87,7 @@ class Uppies( } private fun OpMode.teleOpControls() { - if (isTeleop) return + if (!isTeleop) return if (gp1.current.leftBumper && !gp1.prev.leftBumper) prevState() if (gp1.current.rightBumper && !gp1.prev.rightBumper) nextState() } @@ -98,7 +98,7 @@ class Uppies( companion object { @JvmField - var kP = 2.0 + var kP = 1.5 @JvmField var kI = 0.0 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt index 26f5887e8fe3..dc4e7e268841 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -4,6 +4,7 @@ import com.bylazar.telemetry.PanelsTelemetry import com.millburnx.cmdx.runtimeGroups.CommandScheduler import com.qualcomm.hardware.lynx.LynxModule import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +import com.qualcomm.robotcore.hardware.VoltageSensor import com.qualcomm.robotcore.util.ElapsedTime import org.firstinspires.ftc.teamcode.common.hardware.gamepad.Gamepad import org.firstinspires.ftc.teamcode.common.hardware.gamepad.GamepadManager @@ -20,6 +21,8 @@ abstract class OpMode : LinearOpMode() { val gp2: Gamepad get() = gamepadManager.gamepad2 + lateinit var voltageSensor: VoltageSensor + val loopTimer = ElapsedTime() var deltaTime = 0.0 var hubs: List = emptyList() @@ -50,6 +53,7 @@ abstract class OpMode : LinearOpMode() { override fun runOpMode() { telemetry.isAutoClear = true + voltageSensor = hardwareMap.voltageSensor.get("Control Hub") gamepadManager = GamepadManager(this) SubsystemManager.init() From e7bb35d1a9fb7bddd2de917fc53dc68a3488c084 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Wed, 12 Nov 2025 01:01:19 -0500 Subject: [PATCH 19/29] 11/12/25 - 01 --- .../ftc/teamcode/common/subsystem/Pedro.kt | 77 ++++++++++++++++--- .../ftc/teamcode/opmode/OpMode.kt | 12 ++- 2 files changed, 76 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 97970385ac05..778811cfcf04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -5,22 +5,30 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.toDegrees +import com.millburnx.cmdxpedro.util.toRadians +import com.pedropathing.control.PIDFController +import com.pedropathing.math.MathFunctions import org.firstinspires.ftc.teamcode.common.hardware.gamepad.SlewRateLimiter +import org.firstinspires.ftc.teamcode.common.normalizeDegrees import org.firstinspires.ftc.teamcode.common.simplifiedString import org.firstinspires.ftc.teamcode.common.toPedro import org.firstinspires.ftc.teamcode.opmode.OpMode import org.firstinspires.ftc.teamcode.pedro.Constants import kotlin.math.abs +import kotlin.math.atan2 import kotlin.math.pow import kotlin.math.sign @Configurable -class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleop: Boolean = false) : Subsystem("Pedro") { +class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleop: Boolean = false) : + Subsystem("Pedro") { val follower = Constants.createFollower(opMode.hardwareMap).apply { setStartingPose(startingPose.toPedro()) } var isLocked = false; + val headingController = HeadingLock(this) + val pose: Pose2d get() = Pose2d(follower.pose.x, follower.pose.y, follower.pose.heading.toDegrees()) @@ -38,12 +46,20 @@ class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleo // axis snapping val processedX = processInput(prevX, gp1.current.leftJoyStick.x) val processedY = processInput(prevY, gp1.current.leftJoyStick.y) - val rx = gp1.current.rightJoyStick.x - follower.setTeleOpDrive( - -processedY, - -processedX, - -(abs(rx.pow(2)) * sign(rx)) - ) + if (!useAbsoluteHeading) { + val rx = gp1.current.rightJoyStick.x + follower.setTeleOpDrive( + -processedY, + -processedX, + -(abs(rx.pow(turningCurve).coerceIn(kS, 1.0)) * sign(rx)) * turnScaling + ) + } else { + val targetHeading = atan2(gp1.current.rightJoyStick.y, gp1.current.rightJoyStick.x) + headingController.targetHeading = targetHeading + follower.setTeleOpDrive( + -processedY, -processedX, headingController.calc() + ) + } tel.addData("px", processedX) tel.addData("py", processedY) @@ -67,12 +83,10 @@ class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleo val dt = opMode.deltaTime val slewedResult = SlewRateLimiter.limit( - prevSnapped, - newSnapped, - maxRate * dt + prevSnapped, newSnapped, maxRate * dt ) - return abs(slewedResult.pow(2)) * sign(slewedResult) + return abs(slewedResult.pow(driveCurve)).coerceIn(kS, 1.0) * sign(slewedResult) } companion object { @@ -81,5 +95,46 @@ class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleo @JvmField var maxRate = 4.0; + + @JvmField + var kS = 0.03 + + @JvmField + var turnScaling = 0.5 + + @JvmField + var driveCurve = 2.0 + + @JvmField + var turningCurve = 1.5 + + @JvmField + var useAbsoluteHeading = false + } +} + + +class HeadingLock(val pedro: Pedro) { + private val constants = pedro.follower.constants + + private val headingPIDF = PIDFController(constants.coefficientsHeadingPIDF) + private val secondaryHeadingPIDF = PIDFController(constants.coefficientsSecondaryHeadingPIDF) + private val headingPIDFSwitch = constants.headingPIDFSwitch + var targetHeading = 0.0 + + fun calc(): Double { + val headingError = normalizeDegrees(targetHeading - pedro.pose.heading).toRadians() + val activePID = if (constants.useSecondaryHeadingPIDF && abs(headingError) < headingPIDFSwitch) { + secondaryHeadingPIDF + } else { + headingPIDF + } + activePID.updateFeedForwardInput( + MathFunctions.getTurnDirection( + pedro.pose.radians, targetHeading.toRadians() + ) + ) + activePID.updateError(headingError) + return activePID.run().coerceIn(-1.0, 1.0) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt index dc4e7e268841..c782b864c6a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -10,6 +10,7 @@ import org.firstinspires.ftc.teamcode.common.hardware.gamepad.Gamepad import org.firstinspires.ftc.teamcode.common.hardware.gamepad.GamepadManager import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualManager import org.firstinspires.ftc.teamcode.common.subsystem.SubsystemManager +import kotlin.system.measureTimeMillis abstract class OpMode : LinearOpMode() { val tel = PanelsTelemetry.telemetry @@ -38,8 +39,15 @@ abstract class OpMode : LinearOpMode() { tel.update(telemetry) // hardware - hubs.forEach { it.clearBulkCache() } - ManualManager.update() + val bulkReadTime = measureTimeMillis { + hubs.forEach { it.clearBulkCache() } + } + val manualUpdateTime = measureTimeMillis { + ManualManager.update() + } + tel.addData("br ms", bulkReadTime) + tel.addData("mu ms", manualUpdateTime) + if (::gamepadManager.isInitialized) { gamepadManager.update() } From ba2f0397fb65fbde77a0bec3f29911a61dc86337 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Wed, 12 Nov 2025 19:45:01 -0500 Subject: [PATCH 20/29] 11/12/25 - 19 --- .../ftc/teamcode/common/commands/AutoFire.kt | 29 +++++++++----- .../teamcode/common/subsystem/Apriltags.kt | 13 ++++++- .../common/subsystem/AutoTargeting.kt | 29 +++++++++++++- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 7 +++- .../ftc/teamcode/common/subsystem/Intake.kt | 39 ++++++++++++++++++- .../ftc/teamcode/common/subsystem/Pedro.kt | 7 ++-- .../teamcode/common/vision/VisionManager.kt | 4 +- .../ftc/teamcode/opmode/teleop/Teleop.kt | 6 ++- 8 files changed, 110 insertions(+), 24 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index db14afe8d55c..a760b48db1c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -8,29 +8,34 @@ import com.millburnx.cmdxpedro.util.WaitFor import kotlinx.coroutines.isActive import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.firstBallDelay import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakeDuration +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakePower import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.jamDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.postFinishDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.postLoadDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.unjamDuration import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.unjamPower +import org.firstinspires.ftc.teamcode.common.subsystem.AutoTargeting import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode -fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = +fun TeleopAutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = Command("Auto Fire Triggerer") { with(opMode) { - val autoFire = AutoFire(this, intake, flyWheel, uppies) + val autoFire = AutoFire(this, autoTargeting, intake, flyWheel, uppies) WaitFor { isStarted || isStopRequested } while (!isStopRequested) { if (gp1.current.a && !gp1.prev.a) { if (autoFire.currentScope?.isActive ?: false) { - // already scheduled, cancel - autoFire.cancel() - unlock(intake, flyWheel, uppies) - intake.power = 0.0 - flyWheel.state = FlyWheel.State.IDLE +// // already scheduled, cancel +// autoFire.cancel() +// scheduler.schedule(Command { +// sync() +// unlock(intake, flyWheel, uppies) +// intake.power = 0.0 +// flyWheel.state = FlyWheel.State.IDLE +// }) } else { scheduler.schedule(autoFire) } @@ -40,7 +45,7 @@ fun TeleopAutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: U } } -fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Sequential { +fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Sequential { with(opMode) { suspend fun Command.shootBall() { WaitFor { (flyWheel.atVelocity && uppies.atTarget) || isStopRequested } @@ -54,7 +59,7 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) } suspend fun Command.attemptLoad() { - intake.power = 0.5 + intake.power = intakePower SleepFor({ isStopRequested }) { intakeDuration } uppies.nextState() // check for a jam @@ -105,6 +110,7 @@ fun AutoFire(opMode: OpMode, intake: Intake, flyWheel: FlyWheel, uppies: Uppies) intake.power = 0.0 flyWheel.state = FlyWheel.State.IDLE unlock(intake, flyWheel, uppies) + autoTargeting.enabled = false } } } @@ -115,11 +121,14 @@ object AutoFireSettings { @JvmField var intakeDuration = 500L + @JvmField + var intakePower = 0.8 + @JvmField var jamDuration = 500L @JvmField - var unjamDuration = 500L + var unjamDuration = 250L @JvmField var unjamPower = -0.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt index 4618d15124cf..467884fd3810 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.common.subsystem +import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d @@ -9,13 +10,14 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor +@Configurable class Apriltags : Subsystem("Apriltags") { val processor: AprilTagProcessor = AprilTagProcessor.Builder() .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) .setDrawAxes(true) .setDrawCubeProjection(true) .build().apply { - setDecimation(3F) + setDecimation(decimation) // setPoseSolver(PoseSolver.OPENCV_IPPE) } @@ -31,9 +33,16 @@ class Apriltags : Subsystem("Apriltags") { // vision portal handles running the pipeline // and calculations are handled by the user } + + companion object { + @JvmField + var decimation = 2F + } } fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d { val offset = Vec2d(ftcPose.range).rotate(currentPose.radians + ftcPose.bearing.toRadians()) - return Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw) + val apriltagPose = Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw) + val tagOffset = Vec2d(AutoTargeting.goalDepth).rotate(currentPose.radians + ftcPose.yaw.toRadians()) + return apriltagPose + tagOffset } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt index 9c626c375bd6..973287d1a362 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt @@ -1,12 +1,16 @@ package org.firstinspires.ftc.teamcode.common.subsystem +import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.roundTo import org.firstinspires.ftc.teamcode.opmode.OpMode +import kotlin.math.pow -class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") { +@Configurable +class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags, setFlyWheelVelocity: (Double) -> Unit = {}) : + Subsystem("Auto Targeting") { var enabled = false var target: Pose2d? = null @@ -22,8 +26,18 @@ class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsys tel.addData("target y", target?.y?.roundTo(2) ?: -1.0) tel.addData("target heading", target?.heading?.roundTo(2) ?: -1.0) + if (target != null) { + val distance = pedro.pose.distanceTo(target!!) + tel.addData("distance to target", distance) + setFlyWheelVelocity(getFlyWheelPower(distance)) + } else { + tel.addData("distance to target", -1.0) + setFlyWheelVelocity(FlyWheel.ShootingVelocity) + } + tel.addData("tag range", apriltag?.ftcPose?.range?.roundTo(2) ?: -1.0) tel.addData("tag bearing", apriltag?.ftcPose?.bearing?.roundTo(2) ?: -1.0) + tel.addData("tag yaw", apriltag?.ftcPose?.yaw?.roundTo(2) ?: -1.0) if (gp1.current.y && !gp1.prev.y) { enabled = !enabled @@ -41,4 +55,17 @@ class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsys } } } + + companion object { + @JvmField + var goalDepth = 8.0 + + fun getFlyWheelPower(distance: Double): Double { + return (0.00000530928 * distance.pow(4) + - 0.00314799 * distance.pow(3) + + 0.685274 * distance.pow(2) + - 56.30037 * distance + + 3233.6749) + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index 51677e256732..d76c01c83a48 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -25,6 +25,8 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh return leftAtVelocity && rightAtVelocity } + var shootingVelocity = ShootingVelocity + override val run: suspend Command.() -> Unit = { with(opMode) { WaitFor { isStarted || isStopRequested } @@ -33,7 +35,7 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh targetVelocity = when (state) { State.IDLE -> 0.0 - State.SHOOTING -> ShootingVelocity // replace with distance based + State.SHOOTING -> shootingVelocity // replace with distance based State.INTAKING -> IntakingVelocity } @@ -43,6 +45,7 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh tel.addData("fw state", state) tel.addData("fw-l velocity", -leftMotor.velocity) tel.addData("fw-r velocity", rightMotor.velocity) + tel.addData("shooting velocity", shootingVelocity) tel.addData("voltage", voltageSensor.voltage) sync() } @@ -69,7 +72,7 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh companion object { @JvmField - var ShootingVelocity = 2000.0 + var ShootingVelocity = 1800.0 @JvmField var IntakingVelocity = -1600.0 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt index 56fe44f5f2e8..bc54df51a35e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt @@ -1,24 +1,59 @@ package org.firstinspires.ftc.teamcode.common.subsystem +import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.WaitFor +import com.qualcomm.robotcore.util.ElapsedTime +import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualMotor import org.firstinspires.ftc.teamcode.opmode.OpMode +@Configurable class Intake(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Intake") { val motor = ManualMotor(opMode.hardwareMap, "m3e") var power = 0.0 + private var timer: ElapsedTime? = null + private var unjamingTimer: ElapsedTime? = null override val run: suspend Command.() -> Unit = { - with (opMode) { + with(opMode) { WaitFor { isStarted || isStopRequested } while (!isStopRequested) { if (isTeleop) { - power = gp1.current.rightTrigger - gp1.current.leftTrigger; + val triggerAmount = gp1.current.rightTrigger - gp1.current.leftTrigger + if (!jamProtection) { + power = triggerAmount + } else { + if (timer == null && unjamingTimer == null && triggerAmount > 0.1) { + timer = ElapsedTime() + } else if (triggerAmount <= 0.1) { + timer = null + unjamingTimer = null + } + if ((timer?.milliseconds() ?: -1.0) > intakeDuration) { + power = -AutoFireSettings.unjamPower + timer = null + unjamingTimer = ElapsedTime() + } else if (unjamingTimer == null) { + power = triggerAmount + } else if ((unjamingTimer?.milliseconds() ?: -1.0) > AutoFireSettings.unjamDuration) { + unjamingTimer = null + } else { + power = -AutoFireSettings.unjamPower + } + } } motor.power = power sync() } } } + + companion object { + @JvmField + var jamProtection = true + + @JvmField + var intakeDuration = 1500L + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt index 778811cfcf04..53481b81e69d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -46,19 +46,20 @@ class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleo // axis snapping val processedX = processInput(prevX, gp1.current.leftJoyStick.x) val processedY = processInput(prevY, gp1.current.leftJoyStick.y) - if (!useAbsoluteHeading) { + if (!useAbsoluteHeading || gp1.current.rightJoyStick.vector.magnitude() < 0.2) { val rx = gp1.current.rightJoyStick.x follower.setTeleOpDrive( -processedY, -processedX, - -(abs(rx.pow(turningCurve).coerceIn(kS, 1.0)) * sign(rx)) * turnScaling + abs(rx).pow(turningCurve).coerceIn(kS, 1.0) * sign(-rx) * turnScaling ) } else { - val targetHeading = atan2(gp1.current.rightJoyStick.y, gp1.current.rightJoyStick.x) + val targetHeading = atan2(-gp1.current.rightJoyStick.y, gp1.current.rightJoyStick.x).toDegrees() headingController.targetHeading = targetHeading follower.setTeleOpDrive( -processedY, -processedX, headingController.calc() ) + tel.addData("target heading", targetHeading) } tel.addData("px", processedX) tel.addData("py", processedY) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt index ef8290d0f20b..d77be16f3122 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -32,8 +32,8 @@ object VisionManager { } @JvmField - var cameraSizeX = 1280 + var cameraSizeX = 640 @JvmField - var cameraSizeY = 720 + var cameraSizeY = 360 } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index 75160c8e1a97..4fb845e7398e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -31,7 +31,9 @@ class Teleop : OpMode() { val apriltags = Apriltags() VisionManager.build(this) - val autoTargeting = AutoTargeting(this, pedro, apriltags) + val autoTargeting = AutoTargeting(this, pedro, apriltags) { velocity -> + flyWheel.shootingVelocity = velocity + } scheduler.schedule(Command("drawer") { while (!isStopRequested) { @@ -55,7 +57,7 @@ class Teleop : OpMode() { } }) - scheduler.schedule(TeleopAutoFire(this, intake, flyWheel, uppies)) + scheduler.schedule(TeleopAutoFire(this, autoTargeting, intake, flyWheel, uppies)) // scheduler.schedule(TeleOpStopper(this)) } } \ No newline at end of file From 1219daffa7f513a5add5f10d60aec2ce8313b96d Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Thu, 13 Nov 2025 02:42:15 -0500 Subject: [PATCH 21/29] Goofy auto drive triangle shooting zone thingy --- .../teamcode/common/subsystem/Apriltags.kt | 3 + .../teamcode/common/subsystem/AutoDrive.kt | 102 ++++++++++++++++++ .../common/subsystem/AutoTargeting.kt | 9 +- .../firstinspires/ftc/teamcode/common/util.kt | 13 ++- 4 files changed, 123 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt index 467884fd3810..fc8775e54dbf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -37,6 +37,9 @@ class Apriltags : Subsystem("Apriltags") { companion object { @JvmField var decimation = 2F + + const val BLUE_ID = 20 + const val RED_ID = 24 } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt new file mode 100644 index 000000000000..b1d6662a3bee --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt @@ -0,0 +1,102 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command +import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import org.firstinspires.ftc.teamcode.common.fieldMirror +import org.firstinspires.ftc.teamcode.common.normalizeRadians +import org.firstinspires.ftc.teamcode.opmode.OpMode +import kotlin.math.sign + + +class AutoDrive( + opMode: OpMode, pedro: Pedro, autoTargeting: AutoTargeting +) : Subsystem("Auto Drive") { + override val run: suspend Command.() -> Unit = Command@{ + val target = autoTargeting.target + val targetATag = autoTargeting.targetATag + if (target == null || targetATag == null) return@Command + + val tagPositionGlobal: Pose2d = + Pose2d(16.37007874, 130.3464567, 145.0).let { + if (targetATag.id == Apriltags.RED_ID) it.fieldMirror() else it + } + + val triangle = Triangle( + Vec2d(0.0, 144.0), + Vec2d(72.0, 72.0), + Vec2d(144.0, 144.0), + ).map { + globalToRobotSpace(tagPositionGlobal, target, it) + } + + val withinTriangle = triangle.contains(pedro.pose.position) + + if (!withinTriangle) { + val blueLine = LineSegment( + Vec2d(0.0, 144.0) + Vec2d(24.0, -24.0), + Vec2d(72.0, 72.0), + ).map { + globalToRobotSpace(tagPositionGlobal, target, it) + } + + val redLine = LineSegment( + Vec2d(0.0, 144.0) + Vec2d(-24.0, -24.0), + Vec2d(72.0, 72.0), + ).map { + globalToRobotSpace(tagPositionGlobal, target, it) + } + + val closestBlue = blueLine.project(pedro.pose.position) + val closestRed = redLine.project(pedro.pose.position) + val closestPoint = if (pedro.pose.distanceTo(closestBlue) <= pedro.pose.distanceTo(closestRed)) { + closestBlue + } else { + closestRed + } + // drive to closestPoint + } + } + + fun globalToRobotSpace(tagGlobal: Pose2d, tagRobotSpace: Pose2d, point: Vec2d): Vec2d { + val robotSpaceHeadingError = normalizeRadians(tagGlobal.radians - tagRobotSpace.radians) + + val tagRelative = point - tagGlobal.position + val robotSpaceTagRelative = tagRelative.rotate(robotSpaceHeadingError) + val robotSpace = robotSpaceTagRelative + tagRobotSpace.position + return robotSpace + } +} + +data class Triangle(val p0: Vec2d, val p1: Vec2d, val p2: Vec2d) { + fun map(mapper: (Vec2d) -> Vec2d): Triangle { + return Triangle(mapper(p0), mapper(p1), mapper(p2)) + } + + fun contains(point: Vec2d): Boolean { + fun halfPlaneSign(p: Vec2d, a: Vec2d, b: Vec2d): Double { + val A = (p.x-b.x) * (a.y-b.y) + val B = (a.x-b.x) * (p.y-b.y) + return A - B + } + val sign01 = halfPlaneSign(point, p0, p1) + val sign12 = halfPlaneSign(point, p1, p2) + val sign20 = halfPlaneSign(point, p2, p0) + + return sign(sign01) == sign(sign12) && sign(sign20) == sign(sign12) + } +} + +data class LineSegment(val p0: Vec2d, val p1: Vec2d) { + fun map(mapper: (Vec2d) -> Vec2d): LineSegment { + return LineSegment(mapper(p0), mapper(p1)) + } + + fun project(point: Vec2d): Vec2d { + val v01 = p1 - p0 + val v0p = point - p0 + val t = (v01.dot(v0p) / v01.dot(v01)).coerceIn(0.0, 1.0) + + return p0 + v01 * t + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt index 973287d1a362..27c0d3ceae2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt @@ -6,6 +6,7 @@ import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.roundTo import org.firstinspires.ftc.teamcode.opmode.OpMode +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection import kotlin.math.pow @Configurable @@ -14,13 +15,17 @@ class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags, setFlyWh var enabled = false var target: Pose2d? = null + var targetATag: AprilTagDetection? = null override val run: suspend Command.() -> Unit = { with(opMode) { WaitFor { isStarted || isStopRequested } while (!isStopRequested) { - val apriltag = apriltags.apriltags[20] ?: apriltags.apriltags[24] - if (apriltag != null && !enabled) target = apriltag.toPose(pedro.pose) + val apriltag = apriltags.apriltags[Apriltags.BLUE_ID] ?: apriltags.apriltags[Apriltags.RED_ID] + if (apriltag != null && !enabled) { + targetATag = apriltag + target = apriltag.toPose(pedro.pose) + } tel.addData("target x", target?.x?.roundTo(2) ?: -1.0) tel.addData("target y", target?.y?.roundTo(2) ?: -1.0) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt index 9d02e0de2cfd..ba88e2930c0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt @@ -15,9 +15,9 @@ fun normalizeRadians(radians: Double): Double { fun normalizeDegrees(angle: Double): Double = Math.toDegrees(normalizeRadians(Math.toRadians(angle))) -fun Pose2d.simplifiedString(): String = String.format(Locale.US ,"(%.2f, %.2f, %.0f)", x, y, heading) +fun Pose2d.simplifiedString(): String = String.format(Locale.US, "(%.2f, %.2f, %.0f)", x, y, heading) -fun Vec2d.simplifiedString(): String = String.format(Locale.US ,"(%.2f, %.2f)", x, y) +fun Vec2d.simplifiedString(): String = String.format(Locale.US, "(%.2f, %.2f)", x, y) fun Double.roundTo(n: Int): Double { val factor = 10.0.pow(n) @@ -26,4 +26,13 @@ fun Double.roundTo(n: Int): Double { fun Pose2d.toPedro(): Pose { return Pose(this.x, this.y, this.radians) +} + +fun Pose2d.fieldMirror(): Pose2d { + return Pose2d(144.0 - this.x, this.y, normalizeDegrees(180.0 - this.degrees)) +} + +fun Pose2d.remainingAngleTo(other: Vec2d): Double { + val angleTo = angleTo(other) + return normalizeRadians(angleTo - radians) } \ No newline at end of file From f8a5ad36e2c471dc2c1919b7acbea049d220c31d Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 14 Nov 2025 23:12:20 -0500 Subject: [PATCH 22/29] 9 Ball Close Auto --- .../ftc/teamcode/common/commands/AutoFire.kt | 10 +- .../teamcode/common/subsystem/AutoDrive.kt | 104 +++++++++++---- .../ftc/teamcode/common/subsystem/Uppies.kt | 3 +- .../firstinspires/ftc/teamcode/common/util.kt | 15 ++- .../ftc/teamcode/opmode/auton/CloseAuton.kt | 122 ++++++++++++++++++ .../ftc/teamcode/opmode/auton/SampleAuton.kt | 2 + .../ftc/teamcode/opmode/teleop/Teleop.kt | 1 + 7 files changed, 221 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index a760b48db1c3..ca04746bbabf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -23,7 +23,7 @@ import org.firstinspires.ftc.teamcode.opmode.OpMode fun TeleopAutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = Command("Auto Fire Triggerer") { with(opMode) { - val autoFire = AutoFire(this, autoTargeting, intake, flyWheel, uppies) + val autoFire = AutoFire(this, autoTargeting, intake, flyWheel, uppies, isTeleop = true) WaitFor { isStarted || isStopRequested } while (!isStopRequested) { if (gp1.current.a && !gp1.prev.a) { @@ -45,7 +45,7 @@ fun TeleopAutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, } } -fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Sequential { +fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting?, intake: Intake, flyWheel: FlyWheel, uppies: Uppies, isTeleop: Boolean = false): Sequential { with(opMode) { suspend fun Command.shootBall() { WaitFor { (flyWheel.atVelocity && uppies.atTarget) || isStopRequested } @@ -88,7 +88,7 @@ fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWh return Sequential("Auto Fire") { Command("Prepare Subsystems") { if (!isStarted) parentGroup?.cancel() - lock(intake, flyWheel, uppies) + if (isTeleop) lock(intake, flyWheel, uppies) intake.power = 0.0 flyWheel.state = FlyWheel.State.SHOOTING } @@ -109,8 +109,8 @@ fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWh SleepFor { postFinishDuration } intake.power = 0.0 flyWheel.state = FlyWheel.State.IDLE - unlock(intake, flyWheel, uppies) - autoTargeting.enabled = false + if (isTeleop) unlock(intake, flyWheel, uppies) + autoTargeting?.enabled = false } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt index b1d6662a3bee..d44644811006 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt @@ -2,10 +2,15 @@ package org.firstinspires.ftc.teamcode.common.subsystem import com.millburnx.cmdx.Command import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.millburnx.cmdxpedro.util.toDegrees +import com.pedropathing.geometry.BezierLine import org.firstinspires.ftc.teamcode.common.fieldMirror import org.firstinspires.ftc.teamcode.common.normalizeRadians +import org.firstinspires.ftc.teamcode.common.toPedro import org.firstinspires.ftc.teamcode.opmode.OpMode +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection import kotlin.math.sign @@ -13,10 +18,50 @@ class AutoDrive( opMode: OpMode, pedro: Pedro, autoTargeting: AutoTargeting ) : Subsystem("Auto Drive") { override val run: suspend Command.() -> Unit = Command@{ - val target = autoTargeting.target - val targetATag = autoTargeting.targetATag - if (target == null || targetATag == null) return@Command + with(opMode) { + WaitFor { isStarted } + + while (!isStopRequested) { + val target = autoTargeting.target + val targetATag = autoTargeting.targetATag + + if (pedro.isLocked && !pedro.follower.isBusy && !pedro.follower.isTurning) { + pedro.isLocked = false + pedro.follower.startTeleopDrive(false) + } + + if (gp1.current.dPad.up && !gp1.current.dPad.down) { + if (target == null || targetATag == null || pedro.isLocked) { + sync() + continue + } + + val targetPoint = getTargetPoint(pedro, target, targetATag) + + if (targetPoint != null) { + pedro.isLocked = true + val path = pedro.follower.pathBuilder() + .addPath( + BezierLine( + pedro.pose.toPedro(), + targetPoint.toPedro(), + ) + ) + .setLinearHeadingInterpolation(pedro.pose.radians, target.radians) + .build() + pedro.follower.followPath(path) + println("TARGET POINT $targetPoint" + + "") + } + } + + sync() + } + } + } + + fun getTargetPoint(pedro: Pedro, target: Pose2d, targetATag: AprilTagDetection): Pose2d? { val tagPositionGlobal: Pose2d = Pose2d(16.37007874, 130.3464567, 145.0).let { if (targetATag.id == Apriltags.RED_ID) it.fieldMirror() else it @@ -32,35 +77,41 @@ class AutoDrive( val withinTriangle = triangle.contains(pedro.pose.position) - if (!withinTriangle) { - val blueLine = LineSegment( - Vec2d(0.0, 144.0) + Vec2d(24.0, -24.0), - Vec2d(72.0, 72.0), - ).map { - globalToRobotSpace(tagPositionGlobal, target, it) - } + if (withinTriangle) return null - val redLine = LineSegment( - Vec2d(0.0, 144.0) + Vec2d(-24.0, -24.0), - Vec2d(72.0, 72.0), - ).map { - globalToRobotSpace(tagPositionGlobal, target, it) - } + val blueLine = LineSegment( + Vec2d(0.0, 144.0) + Vec2d(24.0, -24.0), + Vec2d(72.0, 72.0), + ).map { + globalToRobotSpace(tagPositionGlobal, target, it) + } - val closestBlue = blueLine.project(pedro.pose.position) - val closestRed = redLine.project(pedro.pose.position) - val closestPoint = if (pedro.pose.distanceTo(closestBlue) <= pedro.pose.distanceTo(closestRed)) { - closestBlue - } else { - closestRed - } - // drive to closestPoint + val redLine = LineSegment( + Vec2d(0.0, 144.0) + Vec2d(-24.0, -24.0), + Vec2d(72.0, 72.0), + ).map { + globalToRobotSpace(tagPositionGlobal, target, it) + } + + val closestBlue = blueLine.project(pedro.pose.position) + val closestRed = redLine.project(pedro.pose.position) + val closestPoint = if (pedro.pose.distanceTo(closestBlue) <= pedro.pose.distanceTo(closestRed)) { + closestBlue + } else { + closestRed } + + println("DATATA $triangle $withinTriangle $blueLine $redLine $closestBlue $closestRed $closestPoint") + + val angle: Double = closestPoint.angleTo(target.position) + return Pose2d(closestPoint, angle) } fun globalToRobotSpace(tagGlobal: Pose2d, tagRobotSpace: Pose2d, point: Vec2d): Vec2d { val robotSpaceHeadingError = normalizeRadians(tagGlobal.radians - tagRobotSpace.radians) + println("tagPositionGlobal $tagGlobal robotSpaceHeadingError ${robotSpaceHeadingError.toDegrees()}") + val tagRelative = point - tagGlobal.position val robotSpaceTagRelative = tagRelative.rotate(robotSpaceHeadingError) val robotSpace = robotSpaceTagRelative + tagRobotSpace.position @@ -75,10 +126,11 @@ data class Triangle(val p0: Vec2d, val p1: Vec2d, val p2: Vec2d) { fun contains(point: Vec2d): Boolean { fun halfPlaneSign(p: Vec2d, a: Vec2d, b: Vec2d): Double { - val A = (p.x-b.x) * (a.y-b.y) - val B = (a.x-b.x) * (p.y-b.y) + val A = (p.x - b.x) * (a.y - b.y) + val B = (a.x - b.x) * (p.y - b.y) return A - B } + val sign01 = halfPlaneSign(point, p0, p1) val sign12 = halfPlaneSign(point, p1, p2) val sign20 = halfPlaneSign(point, p2, p0) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt index 160c8bd51b70..f2266632cc1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.common.subsystem import com.arcrobotics.ftclib.controller.PIDController import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.Command -import com.millburnx.cmdxpedro.util.WaitFor import org.firstinspires.ftc.teamcode.common.hardware.manual.ManualAxon import org.firstinspires.ftc.teamcode.opmode.OpMode import kotlin.math.abs @@ -71,7 +70,7 @@ class Uppies( override val run: suspend Command.() -> Unit = { with(opMode) { - WaitFor { isStarted || isStopRequested } +// WaitFor { isStarted || isStopRequested } while (!isStopRequested) { teleOpControls() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt index ba88e2930c0e..d4a0c59ad968 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt @@ -4,13 +4,14 @@ import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d import com.pedropathing.geometry.Pose import java.util.* -import kotlin.math.floor +import kotlin.math.atan2 +import kotlin.math.cos import kotlin.math.pow import kotlin.math.round +import kotlin.math.sin fun normalizeRadians(radians: Double): Double { - val temp = (radians + Math.PI) / (2.0 * Math.PI) - return (temp - floor(temp) - 0.5) * 2.0 + return atan2(sin(radians), cos(radians)) } fun normalizeDegrees(angle: Double): Double = Math.toDegrees(normalizeRadians(Math.toRadians(angle))) @@ -32,6 +33,14 @@ fun Pose2d.fieldMirror(): Pose2d { return Pose2d(144.0 - this.x, this.y, normalizeDegrees(180.0 - this.degrees)) } +fun Vec2d.fieldMirror(): Vec2d { + return Vec2d(144.0 - this.x, this.y) +} + +fun Double.fieldMirror(): Double { + return normalizeDegrees(180.0 - this) +} + fun Pose2d.remainingAngleTo(other: Vec2d): Double { val angleTo = angleTo(other) return normalizeRadians(angleTo - radians) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt new file mode 100644 index 000000000000..ec8f14ecc04d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -0,0 +1,122 @@ +package org.firstinspires.ftc.teamcode.opmode.auton + +import com.millburnx.cmdx.commandGroups.Sequential +import com.millburnx.cmdxpedro.FollowPath +import com.millburnx.cmdxpedro.paths.PedroPath +import com.millburnx.cmdxpedro.paths.heading.LinearHeading +import com.millburnx.cmdxpedro.paths.path.CubicBezier +import com.millburnx.cmdxpedro.paths.path.Line +import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.SleepFor +import com.millburnx.cmdxpedro.util.WaitFor +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import org.firstinspires.ftc.teamcode.common.commands.AutoFire +import org.firstinspires.ftc.teamcode.common.fieldMirror +import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel +import org.firstinspires.ftc.teamcode.common.subsystem.Intake +import org.firstinspires.ftc.teamcode.common.subsystem.Pedro +import org.firstinspires.ftc.teamcode.common.subsystem.Uppies +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@Autonomous +class BlueAuton: CloseAuton(isRed = false) + +@Autonomous +class RedAuton: CloseAuton(isRed = true) + +open class CloseAuton(val isRed: Boolean) : OpMode() { + override fun run() { + val intake = Intake(this, isTeleop = false) + val flyWheel = FlyWheel(this, isTeleop = false) + val uppies = Uppies(this, { flyWheel.state }, isTeleop = false) + val pedro = Pedro(this, Pose2d(p(Vec2d(26.0,129.0)), p(145.0)), isTeleop = false) + + uppies.nextState() + + val shootBallOne = PedroPath( + pedro.follower, Line( + p(Vec2d(26, 129)), p(Vec2d(48, 104)) + ), LinearHeading(p(145.0), p(140.0)) + ) + + val preRowOne = PedroPath( + pedro.follower, Line( + p(Vec2d(48, 96)), p(Vec2d(48, 92)) + ), + LinearHeading(p(135.0), p(5.0)) + ) + + val intakeRowOne = PedroPath( + pedro.follower, Line( + p(Vec2d(48, 94)), p(Vec2d(25, 94)) + ), + LinearHeading(p(0.0), p(5.0)) + ) + + val shootRowOne = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(26, 92)), + p(Vec2d(28, 96)), + p(Vec2d(32, 96)), + p(Vec2d(48, 110)) + ), + LinearHeading(p(0.0), p(140.0)) + ) + + val preRowTwo = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(48, 96)), + p(Vec2d(60, 76)), + p(Vec2d(54, 76)), + p(Vec2d(48, 78)), + ), LinearHeading(p(135.0), p(0.0)) + ) + + val intakeRowTwo = PedroPath( + pedro.follower, Line( + p(Vec2d(48, 74)), p(Vec2d(16, 70)) + ), + LinearHeading(p(5.0), p(10.0)) + ) + + val shootRowTwo = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(9, 70)), + p(Vec2d(48, 70)), + p(Vec2d(48, 70)), + p(Vec2d(48, 120)), + ), + LinearHeading(p(0.0), p(145.0)) + ) + + scheduler.schedule(Sequential { + Command { WaitFor { isStarted } } + +FollowPath(pedro.follower, shootBallOne) { opModeIsActive() } + Command { SleepFor { 500 } } + +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) + +FollowPath(pedro.follower, preRowOne) { opModeIsActive() } + Command { intake.power = 1.0 } + +FollowPath(pedro.follower, intakeRowOne) { opModeIsActive() } + Command { SleepFor { 250 }; uppies.nextState(); SleepFor { 500 } }f + +FollowPath(pedro.follower, shootRowOne) { opModeIsActive() } + Command { SleepFor { 500 } } + +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) + +FollowPath(pedro.follower, preRowTwo) { opModeIsActive() } + Command { intake.power = 1.0 } + +FollowPath(pedro.follower, intakeRowTwo) { opModeIsActive() } + Command { SleepFor { 250 }; uppies.nextState(); SleepFor { 250 } } + +FollowPath(pedro.follower, shootRowTwo) { opModeIsActive() } + Command { SleepFor { 500 } } + +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) + }) + } + + fun p(vec2d: Vec2d): Vec2d { + return if (isRed) vec2d.fieldMirror() else vec2d + } + + fun p(double: Double): Double { + return if (isRed) double.fieldMirror() else double + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt index 8a37eb7237cd..989defa629ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.opmode.auton +import com.millburnx.cmdx.commandGroups.Sequential +import com.millburnx.cmdxpedro.paths.PedroPath import com.qualcomm.robotcore.eventloop.opmode.Autonomous import org.firstinspires.ftc.teamcode.opmode.OpMode diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt index 4fb845e7398e..3cd9b62bea7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -34,6 +34,7 @@ class Teleop : OpMode() { val autoTargeting = AutoTargeting(this, pedro, apriltags) { velocity -> flyWheel.shootingVelocity = velocity } +// val autoDrive = AutoDrive(this, pedro, autoTargeting) scheduler.schedule(Command("drawer") { while (!isStopRequested) { From 212b0a37a9f868ed3e483c373b270d191dbbdddb Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 14 Nov 2025 23:12:51 -0500 Subject: [PATCH 23/29] Update deps --- build.dependencies.gradle | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 6c3f16adbefb..c6d0c5256490 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -15,8 +15,8 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.pedropathing:ftc:2.0.3' + implementation 'com.pedropathing:ftc:2.0.4' implementation 'com.pedropathing:telemetry:1.0.0' - implementation 'com.bylazar:fullpanels:1.0.9' + implementation 'com.bylazar:fullpanels:1.0.10' } From 8f790696c19be218b961a94cd3483e95f80d08c7 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 14 Nov 2025 23:29:47 -0500 Subject: [PATCH 24/29] add path export --- .../firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt | 6 +++--- paths/closeAuton.pp | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) create mode 100644 paths/closeAuton.pp diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt index ec8f14ecc04d..b8ee92577b51 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -42,7 +42,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { val preRowOne = PedroPath( pedro.follower, Line( - p(Vec2d(48, 96)), p(Vec2d(48, 92)) + p(Vec2d(48, 104)), p(Vec2d(48, 92)) ), LinearHeading(p(135.0), p(5.0)) ) @@ -56,7 +56,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { val shootRowOne = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(26, 92)), + p(Vec2d(26, 94)), p(Vec2d(28, 96)), p(Vec2d(32, 96)), p(Vec2d(48, 110)) @@ -82,7 +82,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { val shootRowTwo = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(9, 70)), + p(Vec2d(16, 70)), p(Vec2d(48, 70)), p(Vec2d(48, 70)), p(Vec2d(48, 120)), diff --git a/paths/closeAuton.pp b/paths/closeAuton.pp new file mode 100644 index 000000000000..b0e044ba03e6 --- /dev/null +++ b/paths/closeAuton.pp @@ -0,0 +1 @@ +{"startPoint":{"x":26,"y":129,"heading":"linear","startDeg":90,"endDeg":180},"lines":[{"name":"shootBallOne","endPoint":{"x":48,"y":104,"heading":"linear","startDeg":145,"endDeg":135},"controlPoints":[],"color":"#586C75"},{"name":"preRowOne","endPoint":{"x":48,"y":92,"heading":"linear","reverse":false,"startDeg":135,"endDeg":0},"controlPoints":[],"color":"#B959D9"},{"name":"intakeRowOne","endPoint":{"x":25,"y":94,"heading":"linear","reverse":false,"startDeg":0,"endDeg":0},"controlPoints":[],"color":"#8BB8D7"},{"name":"shootRowOne","endPoint":{"x":48,"y":110,"heading":"linear","reverse":false,"startDeg":0,"endDeg":140},"controlPoints":[{"x":28,"y":96},{"x":32,"y":96}],"color":"#988685"},{"name":"preRowTwo","endPoint":{"x":48,"y":78,"heading":"linear","reverse":false,"startDeg":135,"endDeg":0},"controlPoints":[{"x":60,"y":76},{"x":54,"y":76}],"color":"#686B55"},{"name":"intakeRowTwo","endPoint":{"x":16,"y":70,"heading":"linear","reverse":false,"startDeg":5,"endDeg":10},"controlPoints":[],"color":"#CA9B89"},{"name":"shootRowTwo","endPoint":{"x":48,"y":120,"heading":"tangential","reverse":false},"controlPoints":[{"x":48,"y":70},{"x":48,"y":70}],"color":"#D99876"}],"shapes":[]} \ No newline at end of file From 8e6f7c98749f964454a777937100192d856b6aad Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 14 Nov 2025 23:30:16 -0500 Subject: [PATCH 25/29] build fix --- .../org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt index b8ee92577b51..18ac24653084 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -98,7 +98,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { +FollowPath(pedro.follower, preRowOne) { opModeIsActive() } Command { intake.power = 1.0 } +FollowPath(pedro.follower, intakeRowOne) { opModeIsActive() } - Command { SleepFor { 250 }; uppies.nextState(); SleepFor { 500 } }f + Command { SleepFor { 250 }; uppies.nextState(); SleepFor { 500 } } +FollowPath(pedro.follower, shootRowOne) { opModeIsActive() } Command { SleepFor { 500 } } +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) From 63fac8400def1b0c64e6842f5c19b6be5dc599a5 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 14 Nov 2025 23:55:24 -0500 Subject: [PATCH 26/29] better path, dashboard configurable path --- .../ftc/teamcode/opmode/auton/CloseAuton.kt | 85 ++++++++++++------- paths/closeAuton.pp | 2 +- 2 files changed, 57 insertions(+), 30 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt index 18ac24653084..cd339b284647 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmode.auton +import com.bylazar.configurables.annotations.Configurable import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.FollowPath import com.millburnx.cmdxpedro.paths.PedroPath @@ -20,74 +21,78 @@ import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode @Autonomous -class BlueAuton: CloseAuton(isRed = false) +class BlueAuton : CloseAuton(isRed = false) @Autonomous -class RedAuton: CloseAuton(isRed = true) +class RedAuton : CloseAuton(isRed = true) +@Configurable open class CloseAuton(val isRed: Boolean) : OpMode() { override fun run() { val intake = Intake(this, isTeleop = false) val flyWheel = FlyWheel(this, isTeleop = false) val uppies = Uppies(this, { flyWheel.state }, isTeleop = false) - val pedro = Pedro(this, Pose2d(p(Vec2d(26.0,129.0)), p(145.0)), isTeleop = false) + val pedro = Pedro(this, Pose2d(p(Vec2d(15.0, 111.0)), p(90.0)), isTeleop = false) uppies.nextState() val shootBallOne = PedroPath( pedro.follower, Line( - p(Vec2d(26, 129)), p(Vec2d(48, 104)) - ), LinearHeading(p(145.0), p(140.0)) + p(Vec2d(15, 111)), p(Vec2d(32, shootY0)) + ), LinearHeading(p(90.0), p(135.0)) ) val preRowOne = PedroPath( - pedro.follower, Line( - p(Vec2d(48, 104)), p(Vec2d(48, 92)) + pedro.follower, CubicBezier( + p(Vec2d(32, shootY0)), + p(Vec2d(48, shootY0)), + p(Vec2d(preRowX + 12, rowY1)), + p(Vec2d(preRowX, rowY1)) ), - LinearHeading(p(135.0), p(5.0)) + LinearHeading(p(135.0), p(0.0)) ) val intakeRowOne = PedroPath( pedro.follower, Line( - p(Vec2d(48, 94)), p(Vec2d(25, 94)) + p(Vec2d(preRowX, rowY1)), p(Vec2d(16, rowY1)) ), - LinearHeading(p(0.0), p(5.0)) + LinearHeading(p(0.0), p(0.0)) ) val shootRowOne = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(26, 94)), - p(Vec2d(28, 96)), - p(Vec2d(32, 96)), - p(Vec2d(48, 110)) + p(Vec2d(16, rowY1)), + p(Vec2d(48, rowY1)), + p(Vec2d(48, rowY1)), + p(Vec2d(48, shootY1)) ), - LinearHeading(p(0.0), p(140.0)) + LinearHeading(p(0.0), p(135.0)) ) val preRowTwo = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(48, 96)), - p(Vec2d(60, 76)), - p(Vec2d(54, 76)), - p(Vec2d(48, 78)), - ), LinearHeading(p(135.0), p(0.0)) + p(Vec2d(48, shootY1)), + p(Vec2d(60, shootY1-12)), + p(Vec2d(60, rowY2 + 2)), + p(Vec2d(preRowX, rowY2 + 2)), + ), LinearHeading(p(135.0), p(5.0)) ) val intakeRowTwo = PedroPath( pedro.follower, Line( - p(Vec2d(48, 74)), p(Vec2d(16, 70)) + p(Vec2d(preRowX, rowY2 + 2)), p(Vec2d(9, rowY2 - 2)) ), - LinearHeading(p(5.0), p(10.0)) + LinearHeading(p(5.0), p(0.0)) ) val shootRowTwo = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(16, 70)), - p(Vec2d(48, 70)), - p(Vec2d(48, 70)), - p(Vec2d(48, 120)), + p(Vec2d(9, rowY2 - 2)), + p(Vec2d(24, rowY2 - 2)), + p(Vec2d(48, 72)), + p(Vec2d(48, shootY2)), ), - LinearHeading(p(0.0), p(145.0)) + LinearHeading(p(0.0), p(135.0)) ) scheduler.schedule(Sequential { @@ -95,17 +100,19 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { +FollowPath(pedro.follower, shootBallOne) { opModeIsActive() } Command { SleepFor { 500 } } +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) + +FollowPath(pedro.follower, preRowOne) { opModeIsActive() } Command { intake.power = 1.0 } +FollowPath(pedro.follower, intakeRowOne) { opModeIsActive() } - Command { SleepFor { 250 }; uppies.nextState(); SleepFor { 500 } } + Command { SleepFor { 125 }; uppies.nextState(); SleepFor { 625 } } +FollowPath(pedro.follower, shootRowOne) { opModeIsActive() } Command { SleepFor { 500 } } +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) + +FollowPath(pedro.follower, preRowTwo) { opModeIsActive() } Command { intake.power = 1.0 } +FollowPath(pedro.follower, intakeRowTwo) { opModeIsActive() } - Command { SleepFor { 250 }; uppies.nextState(); SleepFor { 250 } } + Command { SleepFor { 125 }; uppies.nextState(); SleepFor { 375 } } +FollowPath(pedro.follower, shootRowTwo) { opModeIsActive() } Command { SleepFor { 500 } } +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) @@ -119,4 +126,24 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { fun p(double: Double): Double { return if (isRed) double.fieldMirror() else double } + + companion object { + @JvmField + var preRowX = 48.0 + + @JvmField + var rowY1 = 84.0 + + @JvmField + var rowY2 = 60.0 + + @JvmField + var shootY0 = 111 + + @JvmField + var shootY1 = 96 + + @JvmField + var shootY2 = 96 + } } \ No newline at end of file diff --git a/paths/closeAuton.pp b/paths/closeAuton.pp index b0e044ba03e6..34d306e2951c 100644 --- a/paths/closeAuton.pp +++ b/paths/closeAuton.pp @@ -1 +1 @@ -{"startPoint":{"x":26,"y":129,"heading":"linear","startDeg":90,"endDeg":180},"lines":[{"name":"shootBallOne","endPoint":{"x":48,"y":104,"heading":"linear","startDeg":145,"endDeg":135},"controlPoints":[],"color":"#586C75"},{"name":"preRowOne","endPoint":{"x":48,"y":92,"heading":"linear","reverse":false,"startDeg":135,"endDeg":0},"controlPoints":[],"color":"#B959D9"},{"name":"intakeRowOne","endPoint":{"x":25,"y":94,"heading":"linear","reverse":false,"startDeg":0,"endDeg":0},"controlPoints":[],"color":"#8BB8D7"},{"name":"shootRowOne","endPoint":{"x":48,"y":110,"heading":"linear","reverse":false,"startDeg":0,"endDeg":140},"controlPoints":[{"x":28,"y":96},{"x":32,"y":96}],"color":"#988685"},{"name":"preRowTwo","endPoint":{"x":48,"y":78,"heading":"linear","reverse":false,"startDeg":135,"endDeg":0},"controlPoints":[{"x":60,"y":76},{"x":54,"y":76}],"color":"#686B55"},{"name":"intakeRowTwo","endPoint":{"x":16,"y":70,"heading":"linear","reverse":false,"startDeg":5,"endDeg":10},"controlPoints":[],"color":"#CA9B89"},{"name":"shootRowTwo","endPoint":{"x":48,"y":120,"heading":"tangential","reverse":false},"controlPoints":[{"x":48,"y":70},{"x":48,"y":70}],"color":"#D99876"}],"shapes":[]} \ No newline at end of file +{"startPoint":{"x":15,"y":111,"heading":"linear","startDeg":90,"endDeg":180},"lines":[{"name":"shootBallOne","endPoint":{"x":32,"y":111,"heading":"linear","startDeg":90,"endDeg":135},"controlPoints":[],"color":"#586C75"},{"name":"preRowOne","endPoint":{"x":48,"y":84,"heading":"linear","reverse":false,"startDeg":135,"endDeg":0},"controlPoints":[{"x":48,"y":111},{"x":60,"y":84}],"color":"#B959D9"},{"name":"intakeRowOne","endPoint":{"x":16,"y":84,"heading":"linear","reverse":false,"startDeg":0,"endDeg":0},"controlPoints":[],"color":"#8BB8D7"},{"name":"shootRowOne","endPoint":{"x":48,"y":96,"heading":"linear","reverse":false,"startDeg":0,"endDeg":135},"controlPoints":[{"x":48,"y":84},{"x":48,"y":84}],"color":"#988685"},{"name":"preRowTwo","endPoint":{"x":48,"y":62,"heading":"linear","reverse":false,"startDeg":135,"endDeg":5},"controlPoints":[{"x":60,"y":84},{"x":60,"y":62}],"color":"#686B55"},{"name":"intakeRowTwo","endPoint":{"x":9,"y":58,"heading":"linear","reverse":false,"startDeg":5,"endDeg":0},"controlPoints":[],"color":"#CA9B89"},{"name":"shootRowTwo","endPoint":{"x":48,"y":96,"heading":"linear","reverse":false,"startDeg":0,"endDeg":135},"controlPoints":[{"x":24,"y":58},{"x":48,"y":72}],"color":"#D99876"}],"shapes":[]} \ No newline at end of file From 65fcf1318cbe6d77a2c99754e9510bf966a1e6d8 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Fri, 14 Nov 2025 23:59:10 -0500 Subject: [PATCH 27/29] even more config vars --- .../ftc/teamcode/opmode/auton/CloseAuton.kt | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt index cd339b284647..01b636e37745 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -39,7 +39,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { val shootBallOne = PedroPath( pedro.follower, Line( p(Vec2d(15, 111)), p(Vec2d(32, shootY0)) - ), LinearHeading(p(90.0), p(135.0)) + ), LinearHeading(p(90.0), p(shootingHeading0)) ) val preRowOne = PedroPath( @@ -49,24 +49,24 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { p(Vec2d(preRowX + 12, rowY1)), p(Vec2d(preRowX, rowY1)) ), - LinearHeading(p(135.0), p(0.0)) + LinearHeading(p(shootingHeading0), p(0.0)) ) val intakeRowOne = PedroPath( pedro.follower, Line( - p(Vec2d(preRowX, rowY1)), p(Vec2d(16, rowY1)) + p(Vec2d(preRowX, rowY1)), p(Vec2d(postRowX1, rowY1)) ), LinearHeading(p(0.0), p(0.0)) ) val shootRowOne = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(16, rowY1)), + p(Vec2d(postRowX1, rowY1)), p(Vec2d(48, rowY1)), p(Vec2d(48, rowY1)), p(Vec2d(48, shootY1)) ), - LinearHeading(p(0.0), p(135.0)) + LinearHeading(p(0.0), p(shootingHeading1)) ) val preRowTwo = PedroPath( @@ -75,24 +75,24 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { p(Vec2d(60, shootY1-12)), p(Vec2d(60, rowY2 + 2)), p(Vec2d(preRowX, rowY2 + 2)), - ), LinearHeading(p(135.0), p(5.0)) + ), LinearHeading(p(shootingHeading1), p(5.0)) ) val intakeRowTwo = PedroPath( pedro.follower, Line( - p(Vec2d(preRowX, rowY2 + 2)), p(Vec2d(9, rowY2 - 2)) + p(Vec2d(preRowX, rowY2 + 2)), p(Vec2d(postRowX2, rowY2 - 2)) ), LinearHeading(p(5.0), p(0.0)) ) val shootRowTwo = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(9, rowY2 - 2)), + p(Vec2d(postRowX2, rowY2 - 2)), p(Vec2d(24, rowY2 - 2)), - p(Vec2d(48, 72)), + p(Vec2d(48, shootY2 - 24)), p(Vec2d(48, shootY2)), ), - LinearHeading(p(0.0), p(135.0)) + LinearHeading(p(0.0), p(shootingHeading2)) ) scheduler.schedule(Sequential { @@ -131,6 +131,21 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { @JvmField var preRowX = 48.0 + @JvmField + var shootingHeading0 = 135.0 + + @JvmField + var shootingHeading1 = 135.0 + + @JvmField + var shootingHeading2 = 135.0 + + @JvmField + var postRowX1 = 16.0 + + @JvmField + var postRowX2 = 9.0 + @JvmField var rowY1 = 84.0 From dbcff14a646b1b5e8ba370beb13494d4b583cbba Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Sat, 15 Nov 2025 03:28:10 -0500 Subject: [PATCH 28/29] Far side auto --- .../ftc/teamcode/opmode/auton/CloseAuton.kt | 8 +- .../ftc/teamcode/opmode/auton/FarAuton.kt | 177 ++++++++++++++++++ paths/farAuton.pp | 2 + 3 files changed, 183 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt create mode 100644 paths/farAuton.pp diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt index 01b636e37745..2fcd1340fd41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -20,11 +20,11 @@ import org.firstinspires.ftc.teamcode.common.subsystem.Pedro import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode -@Autonomous -class BlueAuton : CloseAuton(isRed = false) +@Autonomous(preselectTeleOp = "Teleop") +class BlueCloseAuton : CloseAuton(isRed = false) -@Autonomous -class RedAuton : CloseAuton(isRed = true) +@Autonomous(preselectTeleOp = "Teleop") +class RedCloseAuton : CloseAuton(isRed = true) @Configurable open class CloseAuton(val isRed: Boolean) : OpMode() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt new file mode 100644 index 000000000000..fbf0dfc3cd2b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt @@ -0,0 +1,177 @@ +package org.firstinspires.ftc.teamcode.opmode.auton + +import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.commandGroups.Sequential +import com.millburnx.cmdxpedro.FollowPath +import com.millburnx.cmdxpedro.paths.PedroPath +import com.millburnx.cmdxpedro.paths.heading.LinearHeading +import com.millburnx.cmdxpedro.paths.heading.TangentialHeading +import com.millburnx.cmdxpedro.paths.path.CubicBezier +import com.millburnx.cmdxpedro.paths.path.Line +import com.millburnx.cmdxpedro.util.Pose2d +import com.millburnx.cmdxpedro.util.SleepFor +import com.millburnx.cmdxpedro.util.WaitFor +import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import org.firstinspires.ftc.teamcode.common.commands.AutoFire +import org.firstinspires.ftc.teamcode.common.fieldMirror +import org.firstinspires.ftc.teamcode.common.subsystem.FlyWheel +import org.firstinspires.ftc.teamcode.common.subsystem.Intake +import org.firstinspires.ftc.teamcode.common.subsystem.Pedro +import org.firstinspires.ftc.teamcode.common.subsystem.Uppies +import org.firstinspires.ftc.teamcode.opmode.OpMode + +@Autonomous(preselectTeleOp = "Teleop") +class BlueFarAuton : FarAuton(isRed = false) + +@Autonomous(preselectTeleOp = "Teleop") +class RedFarAuton : FarAuton(isRed = true) + +@Configurable +open class FarAuton(val isRed: Boolean) : OpMode() { + override fun run() { + val intake = Intake(this, isTeleop = false) + val flyWheel = FlyWheel(this, isTeleop = false) + val uppies = Uppies(this, { flyWheel.state }, isTeleop = false) + val pedro = Pedro(this, Pose2d(p(Vec2d(56.0, 8.0)), p(90.0)), isTeleop = false) + + uppies.nextState() + + val shootPreload = PedroPath( + pedro.follower, Line( + p(Vec2d(56.0, 8.0)), + p(Vec2d(shootX0, shootY0)), + ), LinearHeading(p(90.0), p(shootH0)) + ) + + val preRowOne = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(shootX0, shootY0)), + p(Vec2d(shootX0, postRowX1)), + p(Vec2d(shootX0, postRowX1)), + p(Vec2d(preRowX, postRowX1)), + ), LinearHeading(p(shootH0), p(0.0)) + ) + + val intakeRowOne = PedroPath( + pedro.follower, Line( + p(Vec2d(preRowX, rowY1)), + p(Vec2d(postRowX1, rowY1)), + ), TangentialHeading(reverse = true) + ) + + val shootRowOne = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(postRowX1, rowY1)), + p(Vec2d(postRowX1 + 24.0, rowY1)), + p(Vec2d(shootX1 - 24.0, shootY1)), + p(Vec2d(shootX1, rowY1)), + ), LinearHeading(p(0.0), p(shootH1)) + ) + + val preRowTwo = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(shootX1, rowY1)), + p(Vec2d(shootX1, rowY2 + 2.0)), + p(Vec2d(shootX1, rowY2 + 2.0)), + p(Vec2d(preRowX, rowY2 + 2.0)), + ), LinearHeading(p(shootH1), p(0.0)) + ) + + val intakeRowTwo = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(preRowX, rowY2 + 2.0)), + p(Vec2d(preRowX - 24.0, rowY2 + 2.0)), + p(Vec2d(postRowX2 + 24.0, rowY2 - 2.0)), + p(Vec2d(postRowX2, rowY2 - 2.0)), + ), TangentialHeading(reverse = true) + ) + + val shootRowTwo = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(postRowX2, rowY2)), + p(Vec2d(48.0, rowY2)), + p(Vec2d(shootX2, shootY2)), + p(Vec2d(shootX2, shootY2)), + ), LinearHeading(p(0.0), p(shootH2)) + ) + + scheduler.schedule(Sequential { + Command { WaitFor { isStarted } } + +FollowPath(pedro.follower, shootPreload) { opModeIsActive() } + Command { SleepFor { 500 } } + +AutoFire(this@FarAuton, null, intake, flyWheel, uppies, isTeleop = false) + + + +FollowPath(pedro.follower, preRowOne) { opModeIsActive() } + Command { intake.power = 1.0 } + +FollowPath(pedro.follower, intakeRowOne) { opModeIsActive() } + Command { SleepFor { 125 }; uppies.nextState(); SleepFor { 375 } } + +FollowPath(pedro.follower, shootRowOne) { opModeIsActive() } + Command { SleepFor { 500 } } + +AutoFire(this@FarAuton, null, intake, flyWheel, uppies, isTeleop = false) + + +FollowPath(pedro.follower, preRowTwo) { opModeIsActive() } + Command { intake.power = 1.0 } + +FollowPath(pedro.follower, intakeRowTwo) { opModeIsActive() } + Command { SleepFor { 125 }; uppies.nextState(); SleepFor { 625 } } + +FollowPath(pedro.follower, shootRowTwo) { opModeIsActive() } + Command { SleepFor { 500 } } + +AutoFire(this@FarAuton, null, intake, flyWheel, uppies, isTeleop = false) + }) + } + + fun p(vec2d: Vec2d): Vec2d { + return if (isRed) vec2d.fieldMirror() else vec2d + } + + fun p(double: Double): Double { + return if (isRed) double.fieldMirror() else double + } + + companion object { + @JvmField + var preRowX = 44.0 + + @JvmField + var postRowX1 = 9.0 + + @JvmField + var postRowX2 = 9.0 + + @JvmField + var rowY1 = 36.0 + + @JvmField + var rowY2 = 60.0 + + // shooting + + @JvmField + var shootX0 = 56.0 + + @JvmField + var shootX1 = 56.0 + + @JvmField + var shootX2 = 56.0 + + @JvmField + var shootY0 = 12.0 + + @JvmField + var shootY1 = 12.0 + + @JvmField + var shootY2 = 12.0 + + @JvmField + var shootH0 = 111.0 + + @JvmField + var shootH1 = 111.0 + + @JvmField + var shootH2 = 111.0 + } +} \ No newline at end of file diff --git a/paths/farAuton.pp b/paths/farAuton.pp new file mode 100644 index 000000000000..cd8dc600c5dd --- /dev/null +++ b/paths/farAuton.pp @@ -0,0 +1,2 @@ + +{"startPoint":{"x":56,"y":8,"heading":"linear","startDeg":90,"endDeg":180},"lines":[{"name":"shootBallOne","endPoint":{"x":56,"y":12,"heading":"linear","startDeg":90,"endDeg":111},"controlPoints":[],"color":"#CCDA58"},{"name":"preRowOne","endPoint":{"x":44,"y":36,"heading":"linear","reverse":false,"startDeg":111,"endDeg":0},"controlPoints":[{"x":56,"y":36},{"x":56,"y":36}],"color":"#ADA76C"},{"name":"intakeRowOne","endPoint":{"x":9,"y":36,"heading":"tangential","reverse":true,"startDeg":0,"endDeg":0},"controlPoints":[],"color":"#5BB998"},{"name":"shootRowOne","endPoint":{"x":56,"y":12,"heading":"linear","reverse":false,"startDeg":0,"endDeg":111},"controlPoints":[{"x":33,"y":36},{"x":32,"y":12}],"color":"#CA6CDB"},{"name":"preRowTwo","endPoint":{"x":44,"y":62,"heading":"linear","reverse":false,"startDeg":111,"endDeg":0},"controlPoints":[{"x":56,"y":62},{"x":56,"y":62}],"color":"#579C56"},{"name":"intakeRowTwo","endPoint":{"x":9,"y":58,"heading":"tangential","reverse":true,"startDeg":0},"controlPoints":[{"x":32,"y":62},{"x":33,"y":58}],"color":"#5C7755"},{"name":"shootRowTwo","endPoint":{"x":56,"y":12,"heading":"linear","reverse":false,"startDeg":0,"endDeg":111},"controlPoints":[{"x":48,"y":58},{"x":56,"y":48}],"color":"#8DDD9C"}],"shapes":[]} \ No newline at end of file From 4e16ad9a0bdbbc1ab24e64595688429db3f15df0 Mon Sep 17 00:00:00 2001 From: AshOnDiscord <68016221+AshOnDiscord@users.noreply.github.com> Date: Sun, 16 Nov 2025 02:41:55 -0500 Subject: [PATCH 29/29] LM2 - 11/16/2025 #1 League, #2 Comp, 4-1-0 --- .../ftc/teamcode/common/commands/AutoFire.kt | 26 ++++++++++++++++--- .../ftc/teamcode/common/subsystem/FlyWheel.kt | 2 +- .../ftc/teamcode/opmode/auton/CloseAuton.kt | 21 +++++++++------ .../ftc/teamcode/opmode/auton/FarAuton.kt | 20 ++++++++++---- 4 files changed, 51 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt index ca04746bbabf..3c6cba303465 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -5,6 +5,7 @@ import com.millburnx.cmdx.Command import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.util.SleepFor import com.millburnx.cmdxpedro.util.WaitFor +import kotlinx.coroutines.NonCancellable.isCancelled import kotlinx.coroutines.isActive import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.firstBallDelay import org.firstinspires.ftc.teamcode.common.commands.AutoFireSettings.intakeDuration @@ -20,7 +21,13 @@ import org.firstinspires.ftc.teamcode.common.subsystem.Intake import org.firstinspires.ftc.teamcode.common.subsystem.Uppies import org.firstinspires.ftc.teamcode.opmode.OpMode -fun TeleopAutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, flyWheel: FlyWheel, uppies: Uppies): Command = +fun TeleopAutoFire( + opMode: OpMode, + autoTargeting: AutoTargeting, + intake: Intake, + flyWheel: FlyWheel, + uppies: Uppies +): Command = Command("Auto Fire Triggerer") { with(opMode) { val autoFire = AutoFire(this, autoTargeting, intake, flyWheel, uppies, isTeleop = true) @@ -45,10 +52,18 @@ fun TeleopAutoFire(opMode: OpMode, autoTargeting: AutoTargeting, intake: Intake, } } -fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting?, intake: Intake, flyWheel: FlyWheel, uppies: Uppies, isTeleop: Boolean = false): Sequential { +fun AutoFire( + opMode: OpMode, + autoTargeting: AutoTargeting?, + intake: Intake, + flyWheel: FlyWheel, + uppies: Uppies, + isTeleop: Boolean = false +): Sequential { + with(opMode) { suspend fun Command.shootBall() { - WaitFor { (flyWheel.atVelocity && uppies.atTarget) || isStopRequested } + WaitFor { (flyWheel.atVelocity && uppies.atTarget) || isStopRequested || isCancelled } uppies.nextState() } @@ -82,22 +97,25 @@ fun AutoFire(opMode: OpMode, autoTargeting: AutoTargeting?, intake: Intake, flyW attemptLoad() } } - SleepFor { postLoadDuration } + SleepFor({ isStopRequested }) { postLoadDuration } } return Sequential("Auto Fire") { Command("Prepare Subsystems") { + if (isCancelled) return@Command if (!isStarted) parentGroup?.cancel() if (isTeleop) lock(intake, flyWheel, uppies) intake.power = 0.0 flyWheel.state = FlyWheel.State.SHOOTING } Command("Verify First Ball") { + if (isCancelled) return@Command if (uppies.state != Uppies.State.LOADED) { loadBall() } } Command("First Ball Wait") { + if (isCancelled) return@Command SleepFor { firstBallDelay } } Command("Shoot First Ball") { shootBall() } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt index d76c01c83a48..e3a4b3bb6679 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -72,7 +72,7 @@ class FlyWheel(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("FlyWh companion object { @JvmField - var ShootingVelocity = 1800.0 + var ShootingVelocity = 2550.0 @JvmField var IntakingVelocity = -1600.0 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt index 2fcd1340fd41..52c6b8486d68 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -34,18 +34,22 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { val uppies = Uppies(this, { flyWheel.state }, isTeleop = false) val pedro = Pedro(this, Pose2d(p(Vec2d(15.0, 111.0)), p(90.0)), isTeleop = false) + FlyWheel.ShootingVelocity = 1750.0 + flyWheel.shootingVelocity = 1750.0 uppies.nextState() + tel.addData("fwv", flyWheel.shootingVelocity) + val shootBallOne = PedroPath( pedro.follower, Line( - p(Vec2d(15, 111)), p(Vec2d(32, shootY0)) + p(Vec2d(15, 111)), p(Vec2d(38, shootY0)) ), LinearHeading(p(90.0), p(shootingHeading0)) ) val preRowOne = PedroPath( pedro.follower, CubicBezier( - p(Vec2d(32, shootY0)), - p(Vec2d(48, shootY0)), + p(Vec2d(54, shootY0)), + p(Vec2d(54, shootY0)), p(Vec2d(preRowX + 12, rowY1)), p(Vec2d(preRowX, rowY1)) ), @@ -72,7 +76,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { val preRowTwo = PedroPath( pedro.follower, CubicBezier( p(Vec2d(48, shootY1)), - p(Vec2d(60, shootY1-12)), + p(Vec2d(60, shootY1 - 12)), p(Vec2d(60, rowY2 + 2)), p(Vec2d(preRowX, rowY2 + 2)), ), LinearHeading(p(shootingHeading1), p(5.0)) @@ -112,7 +116,8 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { +FollowPath(pedro.follower, preRowTwo) { opModeIsActive() } Command { intake.power = 1.0 } +FollowPath(pedro.follower, intakeRowTwo) { opModeIsActive() } - Command { SleepFor { 125 }; uppies.nextState(); SleepFor { 375 } } + Command { SleepFor { 0 }; uppies.nextState(); SleepFor { 500 } } + Command { intake.power = 1.0 } +FollowPath(pedro.follower, shootRowTwo) { opModeIsActive() } Command { SleepFor { 500 } } +AutoFire(this@CloseAuton, null, intake, flyWheel, uppies, isTeleop = false) @@ -141,10 +146,10 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { var shootingHeading2 = 135.0 @JvmField - var postRowX1 = 16.0 + var postRowX1 = 15.0 @JvmField - var postRowX2 = 9.0 + var postRowX2 = 8.0 @JvmField var rowY1 = 84.0 @@ -153,7 +158,7 @@ open class CloseAuton(val isRed: Boolean) : OpMode() { var rowY2 = 60.0 @JvmField - var shootY0 = 111 + var shootY0 = 96 @JvmField var shootY1 = 96 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt index fbf0dfc3cd2b..6a056e2c8d3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.opmode.auton import com.bylazar.configurables.annotations.Configurable +import com.millburnx.cmdx.Command import com.millburnx.cmdx.commandGroups.Sequential import com.millburnx.cmdxpedro.FollowPath import com.millburnx.cmdxpedro.paths.PedroPath @@ -12,6 +13,8 @@ import com.millburnx.cmdxpedro.util.Pose2d import com.millburnx.cmdxpedro.util.SleepFor import com.millburnx.cmdxpedro.util.WaitFor import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d +import com.pedropathing.follower.Follower +import com.pedropathing.paths.PathChain import com.qualcomm.robotcore.eventloop.opmode.Autonomous import org.firstinspires.ftc.teamcode.common.commands.AutoFire import org.firstinspires.ftc.teamcode.common.fieldMirror @@ -36,6 +39,8 @@ open class FarAuton(val isRed: Boolean) : OpMode() { val pedro = Pedro(this, Pose2d(p(Vec2d(56.0, 8.0)), p(90.0)), isTeleop = false) uppies.nextState() + FlyWheel.ShootingVelocity = 2550.0 + flyWheel.shootingVelocity = 2550.0 val shootPreload = PedroPath( pedro.follower, Line( @@ -65,7 +70,7 @@ open class FarAuton(val isRed: Boolean) : OpMode() { p(Vec2d(postRowX1, rowY1)), p(Vec2d(postRowX1 + 24.0, rowY1)), p(Vec2d(shootX1 - 24.0, shootY1)), - p(Vec2d(shootX1, rowY1)), + p(Vec2d(shootX1, shootY1)), ), LinearHeading(p(0.0), p(shootH1)) ) @@ -103,7 +108,7 @@ open class FarAuton(val isRed: Boolean) : OpMode() { +AutoFire(this@FarAuton, null, intake, flyWheel, uppies, isTeleop = false) - +FollowPath(pedro.follower, preRowOne) { opModeIsActive() } + +FollowPathPower(pedro.follower, preRowOne, 0.75) { opModeIsActive() } Command { intake.power = 1.0 } +FollowPath(pedro.follower, intakeRowOne) { opModeIsActive() } Command { SleepFor { 125 }; uppies.nextState(); SleepFor { 375 } } @@ -134,13 +139,13 @@ open class FarAuton(val isRed: Boolean) : OpMode() { var preRowX = 44.0 @JvmField - var postRowX1 = 9.0 + var postRowX1 = 10.0 @JvmField - var postRowX2 = 9.0 + var postRowX2 = 10.0 @JvmField - var rowY1 = 36.0 + var rowY1 = 34.0 @JvmField var rowY2 = 60.0 @@ -174,4 +179,9 @@ open class FarAuton(val isRed: Boolean) : OpMode() { @JvmField var shootH2 = 111.0 } +} + +public fun FollowPathPower(follower: Follower, path: PathChain, maxPower: Double = 1.0, opModeIsActive: () -> Boolean): Command = Command() { + follower.followPath(path, maxPower, true) + WaitFor { !opModeIsActive() || !(follower.isBusy) } } \ No newline at end of file