diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 350a5d409d72..a957a74587f3 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -21,13 +21,14 @@ 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:aef9d51a78' + implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:aef9d51a78' +// maven local +// 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' + implementation 'com.google.firebase:firebase-crashlytics-buildtools:3.0.6' } 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/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/AutoFire.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt new file mode 100644 index 000000000000..3c6cba303465 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/AutoFire.kt @@ -0,0 +1,189 @@ +package org.firstinspires.ftc.teamcode.common.commands + +import com.bylazar.configurables.annotations.Configurable +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 +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, + 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) + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + if (gp1.current.a && !gp1.prev.a) { + if (autoFire.currentScope?.isActive ?: false) { +// // 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) + } + } + sync() + } + } + } + +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 || isCancelled } + uppies.nextState() + } + + suspend fun Command.attemptUnjam() { + uppies.prevState() + intake.power = -unjamPower + SleepFor({ isStopRequested }) { unjamDuration } + } + + suspend fun Command.attemptLoad() { + intake.power = intakePower + 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 = AutoFireSettings.jamProtection) { + if (!jamProtection) { + intake.power = 0.5 + SleepFor({ isStopRequested }) { intakeDuration } + uppies.nextState() + } else { + attemptLoad() + while (isJammed()) { + attemptUnjam() + attemptLoad() + } + } + 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() } + 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 + if (isTeleop) unlock(intake, flyWheel, uppies) + autoTargeting?.enabled = false + } + } + } +} + +@Configurable +object AutoFireSettings { + @JvmField + var intakeDuration = 500L + + @JvmField + var intakePower = 0.8 + + @JvmField + var jamDuration = 500L + + @JvmField + var unjamDuration = 250L + + @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") { +// 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 + 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/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 64% 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..30bd25d81b9f 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( @@ -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/util/CachedMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/cached/CachedMotor.kt similarity index 76% 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..35d48dbf9c09 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( @@ -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 } } @@ -47,4 +48,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/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/util/ManualAxon.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/manual/ManualAxon.kt similarity index 71% 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..f07ab827b551 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( @@ -17,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/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 69% 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..e6aa17fb7f77 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( @@ -16,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/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt new file mode 100644 index 000000000000..fc8775e54dbf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt @@ -0,0 +1,51 @@ +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 +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 + +@Configurable +class Apriltags : Subsystem("Apriltags") { + val processor: AprilTagProcessor = AprilTagProcessor.Builder() + .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) + .setDrawAxes(true) + .setDrawCubeProjection(true) + .build().apply { + setDecimation(decimation) +// setPoseSolver(PoseSolver.OPENCV_IPPE) + } + + init { + VisionManager.processors.add(processor) + } + + val apriltags: Map + get() = processor.detections.associateBy { it.id } + + 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 + } + + companion object { + @JvmField + var decimation = 2F + + const val BLUE_ID = 20 + const val RED_ID = 24 + } +} + +fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d { + val offset = Vec2d(ftcPose.range).rotate(currentPose.radians + ftcPose.bearing.toRadians()) + 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/AutoDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt new file mode 100644 index 000000000000..d44644811006 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoDrive.kt @@ -0,0 +1,154 @@ +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 + + +class AutoDrive( + opMode: OpMode, pedro: Pedro, autoTargeting: AutoTargeting +) : Subsystem("Auto Drive") { + override val run: suspend Command.() -> Unit = 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 + } + + 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) return null + + 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 + } + + 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 + 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 new file mode 100644 index 000000000000..27c0d3ceae2e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt @@ -0,0 +1,76 @@ +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 org.firstinspires.ftc.vision.apriltag.AprilTagDetection +import kotlin.math.pow + +@Configurable +class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags, setFlyWheelVelocity: (Double) -> Unit = {}) : + Subsystem("Auto Targeting") { + + 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[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) + 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 + } + val newLocked = enabled && target != null + if (newLocked != pedro.isLocked) { + pedro.isLocked = newLocked + if (newLocked) { + pedro.follower.turnTo(pedro.pose.angleTo(target!!)) + } else { + pedro.follower.startTeleopDrive(false) + } + } + sync() + } + } + } + + 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 new file mode 100644 index 000000000000..e3a4b3bb6679 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/FlyWheel.kt @@ -0,0 +1,123 @@ +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 +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") { + 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 targetVelocity = 0.0 + + val atVelocity: Boolean + get() { + val leftAtVelocity = abs(targetVelocity + leftMotor.velocity) <= velocityThreshold + val rightAtVelocity = abs(targetVelocity - rightMotor.velocity) <= velocityThreshold + return leftAtVelocity && rightAtVelocity + } + + var shootingVelocity = ShootingVelocity + + override val run: suspend Command.() -> Unit = { + with(opMode) { + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + teleOpControls() + + targetVelocity = when (state) { + State.IDLE -> 0.0 + State.SHOOTING -> shootingVelocity // replace with distance based + State.INTAKING -> IntakingVelocity + } + + 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("shooting velocity", shootingVelocity) + tel.addData("voltage", voltageSensor.voltage) + sync() + } + } + } + + private fun OpMode.teleOpControls() { + if (isTeleop) { +// 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.prev.b) { + state = if (state == State.INTAKING) State.IDLE else State.INTAKING + } + } + } + + enum class State { + IDLE, + SHOOTING, + INTAKING + } + + companion object { + @JvmField + var ShootingVelocity = 2550.0 + + @JvmField + var IntakingVelocity = -1600.0 + + @JvmField + var velocityThreshold = 50.0 + } +} + +@Configurable +class FlyWheelController() { + val PID = PIDController(kP, kI, kD) + 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 = 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.001 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.0 + + @JvmField + var kS = 0.04 + + @JvmField + var kV = 0.00037 + + @JvmField + var voltageWeight = 1.0 + } +} \ 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..bc54df51a35e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Intake.kt @@ -0,0 +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) { + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + if (isTeleop) { + 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 new file mode 100644 index 000000000000..53481b81e69d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt @@ -0,0 +1,141 @@ +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 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") { + 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()) + + override val run: suspend Command.() -> Unit = { + var prevX = 0.0 + var prevY = 0.0 + + with(opMode) { + follower.update() + if (isTeleop) follower.startTeleopDrive(false) + WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + follower.update() + if (isTeleop && !isLocked) { + // axis snapping + val processedX = processInput(prevX, gp1.current.leftJoyStick.x) + val processedY = processInput(prevY, gp1.current.leftJoyStick.y) + 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 + ) + } else { + 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) + + prevX = processedX + prevY = processedY + } + tel.addData("pose", pose.simplifiedString()) + sync() + } + } + } + + private fun snapTo(raw: Double, target: Double, threshold: Double): Double { + if (abs(raw - target) < threshold) return target + 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(driveCurve)).coerceIn(kS, 1.0) * sign(slewedResult) + } + + companion object { + @JvmField + var snapThreshold = 0.1; + + @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/common/subsystem/Subsystem.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt new file mode 100644 index 000000000000..a7a852465451 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Subsystem.kt @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode.common.subsystem + +import com.millburnx.cmdx.Command + +abstract class Subsystem( + val name: String +) { + init { + SubsystemManager.subsystems.add(this) + } + + 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/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/common/subsystem/Uppies.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt new file mode 100644 index 000000000000..f2266632cc1e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Uppies.kt @@ -0,0 +1,120 @@ +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 org.firstinspires.ftc.teamcode.common.hardware.manual.ManualAxon +import org.firstinspires.ftc.teamcode.opmode.OpMode +import kotlin.math.abs + +@Configurable +class Uppies( + opMode: OpMode, + val flyWheelState: () -> FlyWheel.State, + var isTeleop: Boolean = false +) : Subsystem("Uppies") { + 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 atTarget: Boolean + get() = abs(servo.position - target) <= threshold + + val pid = PIDController(kP, kI, kD) + + fun prevState() { + state = if (state == State.LOADED) { + if (flyWheelState() == FlyWheel.State.INTAKING) { + State.LOADER_READY + } else { + State.INTAKE_READY + } + } else { + if (state == State.INTAKE_READY) { + rotations-- + } + State.LOADED + } + } + + fun nextState() { + state = if (state == State.LOADED) { + if (flyWheelState() == FlyWheel.State.INTAKING) { + rotations-- + State.LOADER_READY + } else { + rotations++ + State.INTAKE_READY + } + } else { + State.LOADED + } + } + + fun flywheelSync() { + if (flyWheelState() == FlyWheel.State.INTAKING && state == State.INTAKE_READY) { + rotations-- + state = State.LOADER_READY + } + if (flyWheelState() != FlyWheel.State.INTAKING && state == State.LOADER_READY) { + rotations++ + state = State.INTAKE_READY + } + } + + override val run: suspend Command.() -> Unit = { + with(opMode) { +// WaitFor { isStarted || isStopRequested } + while (!isStopRequested) { + teleOpControls() + + flywheelSync() + pid.setPID(kP, kI, kD) + servo.power = pid.calculate(servo.position, target) + + tel.addData("uppies state", state) + tel.addData("uppies position", servo.position) + sync() + } + } + } + + private fun OpMode.teleOpControls() { + 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, + } + + companion object { + @JvmField + var kP = 1.5 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.06 + + @JvmField + var threshold = 0.05 + + @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/common/util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt new file mode 100644 index 000000000000..d4a0c59ad968 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt @@ -0,0 +1,47 @@ +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.atan2 +import kotlin.math.cos +import kotlin.math.pow +import kotlin.math.round +import kotlin.math.sin + +fun normalizeRadians(radians: Double): Double { + return atan2(sin(radians), cos(radians)) +} + +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 +} + +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 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) +} \ No newline at end of file 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 new file mode 100644 index 000000000000..d77be16f3122 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.common.vision + +import android.util.Size +import com.bylazar.configurables.annotations.Configurable +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 + +@Suppress("SpreadOperator") +@Configurable +object VisionManager { + val processors = mutableListOf() + + fun init() { + processors.clear() + } + + fun build(opMode: OpMode): VisionPortal { +// val panelsIntegration = PanelsIntegration() + val portal = VisionPortal + .Builder() + .setCamera(opMode.hardwareMap["cam1"] as WebcamName) + .addProcessors(*processors.toTypedArray()) + .setCameraResolution(Size(cameraSizeX, cameraSizeY)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + .enableLiveView(true) + .setAutoStopLiveView(true) + .build() +// PanelsCameraStream.startStream(panelsIntegration) + return portal + } + + @JvmField + var cameraSizeX = 640 + + @JvmField + var cameraSizeY = 360 +} \ 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..c782b864c6a1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt @@ -0,0 +1,79 @@ +package org.firstinspires.ftc.teamcode.opmode + +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 +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 + + lateinit var gamepadManager: GamepadManager + + val gp1: Gamepad + get() = gamepadManager.gamepad1 + val gp2: Gamepad + get() = gamepadManager.gamepad2 + + lateinit var voltageSensor: VoltageSensor + + 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) + tel.addData("ms", ms) + tel.update(telemetry) + + // hardware + 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() + } + } + } + + // 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 + + voltageSensor = hardwareMap.voltageSensor.get("Control Hub") + gamepadManager = GamepadManager(this) + + 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 +// PanelsCameraStream.stopStream() + } +} \ No newline at end of file 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..52c6b8486d68 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/CloseAuton.kt @@ -0,0 +1,169 @@ +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.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 BlueCloseAuton : CloseAuton(isRed = false) + +@Autonomous(preselectTeleOp = "Teleop") +class RedCloseAuton : 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(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(38, shootY0)) + ), LinearHeading(p(90.0), p(shootingHeading0)) + ) + + val preRowOne = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(54, shootY0)), + p(Vec2d(54, shootY0)), + p(Vec2d(preRowX + 12, rowY1)), + p(Vec2d(preRowX, rowY1)) + ), + LinearHeading(p(shootingHeading0), p(0.0)) + ) + + val intakeRowOne = PedroPath( + pedro.follower, Line( + p(Vec2d(preRowX, rowY1)), p(Vec2d(postRowX1, rowY1)) + ), + LinearHeading(p(0.0), p(0.0)) + ) + + val shootRowOne = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(postRowX1, rowY1)), + p(Vec2d(48, rowY1)), + p(Vec2d(48, rowY1)), + p(Vec2d(48, shootY1)) + ), + LinearHeading(p(0.0), p(shootingHeading1)) + ) + + val preRowTwo = PedroPath( + pedro.follower, CubicBezier( + p(Vec2d(48, shootY1)), + p(Vec2d(60, shootY1 - 12)), + p(Vec2d(60, rowY2 + 2)), + p(Vec2d(preRowX, rowY2 + 2)), + ), LinearHeading(p(shootingHeading1), p(5.0)) + ) + + val intakeRowTwo = PedroPath( + pedro.follower, Line( + 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(postRowX2, rowY2 - 2)), + p(Vec2d(24, rowY2 - 2)), + p(Vec2d(48, shootY2 - 24)), + p(Vec2d(48, shootY2)), + ), + LinearHeading(p(0.0), p(shootingHeading2)) + ) + + 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 { 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 { 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) + }) + } + + 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 = 48.0 + + @JvmField + var shootingHeading0 = 135.0 + + @JvmField + var shootingHeading1 = 135.0 + + @JvmField + var shootingHeading2 = 135.0 + + @JvmField + var postRowX1 = 15.0 + + @JvmField + var postRowX2 = 8.0 + + @JvmField + var rowY1 = 84.0 + + @JvmField + var rowY2 = 60.0 + + @JvmField + var shootY0 = 96 + + @JvmField + var shootY1 = 96 + + @JvmField + var shootY2 = 96 + } +} \ No newline at end of file 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..6a056e2c8d3c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/FarAuton.kt @@ -0,0 +1,187 @@ +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 +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.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 +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() + FlyWheel.ShootingVelocity = 2550.0 + flyWheel.shootingVelocity = 2550.0 + + 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, shootY1)), + ), 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) + + + +FollowPathPower(pedro.follower, preRowOne, 0.75) { 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 = 10.0 + + @JvmField + var postRowX2 = 10.0 + + @JvmField + var rowY1 = 34.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 + } +} + +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 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..989defa629ef --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/SampleAuton.kt @@ -0,0 +1,12 @@ +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 + +@Autonomous +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/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 new file mode 100644 index 000000000000..65218f73a56d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/SampleTeleop.kt @@ -0,0 +1,12 @@ +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)) + } +} \ No newline at end of file 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..3cd9b62bea7f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt @@ -0,0 +1,64 @@ +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.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 +import org.firstinspires.ftc.teamcode.common.subsystem.Uppies +import org.firstinspires.ftc.teamcode.common.vision.VisionManager +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, Pose2d(72.0, 72.0, 90.0), 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(this) + + val autoTargeting = AutoTargeting(this, pedro, apriltags) { velocity -> + flyWheel.shootingVelocity = velocity + } +// val autoDrive = AutoDrive(this, pedro, autoTargeting) + + 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 = autoTargeting.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, autoTargeting, 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 new file mode 100644 index 000000000000..0519c041670f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.kt @@ -0,0 +1,80 @@ +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 +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.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") + 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, .25, 2.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/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.. 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..c6d0c5256490 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -14,8 +14,9 @@ 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:ftc:2.0.4' implementation 'com.pedropathing:telemetry:1.0.0' - implementation 'com.bylazar:fullpanels:1.0.6' + implementation 'com.bylazar:fullpanels:1.0.10' } diff --git a/paths/closeAuton.pp b/paths/closeAuton.pp new file mode 100644 index 000000000000..34d306e2951c --- /dev/null +++ b/paths/closeAuton.pp @@ -0,0 +1 @@ +{"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 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