From 28d98c41d91720c62a58bc0cb861263a5809d369 Mon Sep 17 00:00:00 2001 From: Anirudh Narayanan <109127469+ripark2@users.noreply.github.com> Date: Mon, 9 Feb 2026 17:15:24 -0600 Subject: [PATCH] update --- .../ftc/teamcode/Auton/blueDepot.java | 353 +++++++++++------- .../ftc/teamcode/Auton/blueDepotFar.java | 80 ++++ .../ftc/teamcode/Auton/redDepot.java | 215 +++++++++++ .../ftc/teamcode/DriverControlsDoubleV1.java | 120 ------ .../org/firstinspires/ftc/teamcode/FlyOp.java | 48 +++ .../ftc/teamcode/FlywheelTuner.java | 84 +++++ .../firstinspires/ftc/teamcode/HwareV2.java | 62 +++ .../{ => OUTDATED}/AlignmentTest.java | 6 +- .../ftc/teamcode/{ => OUTDATED}/AutoLock.java | 6 +- .../ftc/teamcode/OUTDATED/AutonV1.java | 11 + .../ftc/teamcode/{ => OUTDATED}/Hware.java | 21 +- .../teamcode/OUTDATED/OdometryAutoLock.java | 319 ++++++++++++++++ .../teamcode/{ => OUTDATED}/ServoTesting.java | 19 +- .../ftc/teamcode/OUTDATED/Testing.java | 330 ++++++++++++++++ .../ftc/teamcode/OUTDATED/Testingv2.java | 330 ++++++++++++++++ .../ftc/teamcode/OUTDATED/odoTest.java | 326 ++++++++++++++++ .../ftc/teamcode/{ => OUTDATED}/readme.md | 0 .../ftc/teamcode/OdometryAutoLock.java | 262 ------------- .../ftc/teamcode/TeleOpTurretLock.java | 143 +++++++ .../org/firstinspires/ftc/teamcode/Test.java | 60 +++ .../ftc/teamcode/TurretLock.java | 186 +++++++++ .../ftc/teamcode/drive/DriveConstants.java | 2 +- .../teamcode/drive/SampleMecanumDrive.java | 34 +- .../ftc/teamcode/drive/SampleTankDrive.java | 24 +- .../drive/StandardTrackingWheelLocalizer.java | 13 +- .../drive/opmode/LocalizationTest.java | 42 ++- .../drive/opmode/ManualFeedforwardTuner.java | 2 - .../ftc/teamcode/flywheelSystem.java | 88 +++++ 28 files changed, 2583 insertions(+), 603 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HwareV2.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => OUTDATED}/AlignmentTest.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => OUTDATED}/AutoLock.java (98%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutonV1.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => OUTDATED}/Hware.java (74%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/OdometryAutoLock.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => OUTDATED}/ServoTesting.java (60%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testing.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testingv2.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/odoTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => OUTDATED}/readme.md (100%) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/flywheelSystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java index 085fba8..079604a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java @@ -1,138 +1,215 @@ -package org.firstinspires.ftc.teamcode.Auton; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.firstinspires.ftc.teamcode.Hware; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; - -@Config -@Autonomous(name = "Blue Depot Cycle") -public class blueDepot extends LinearOpMode { - - Hware robot = new Hware(); - double TICKS_PER_DEGREE = (384.5 * 9.26) / 360; - - public void turretPower(double power) { - robot.turret.setPower(power); - } - - public void turretPos(int position) { - turretPower(0); - robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); - robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); - turretPower(1); - } - - public void spintakeStart(){ - robot.intake.set(1); - } - - public void spintakeEnd(){ - robot.intake.set(0); - } - - public void shoot(){ - robot.topFly.set(1); - robot.bottomFly.set(1); - robot.activeTransfer.setPosition(1); - } - - public void reset(){ - robot.intake.set(0); - robot.topFly.set(0); - robot.bottomFly.set(0); - robot.launchHood.setPosition(0.3); - robot.activeTransfer.setPosition(0); - } - - - public void runOpMode() throws InterruptedException { - - //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - - Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); - - drive.setPoseEstimate(startPose); - - TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose) - .strafeLeft(25) - .UNSTABLE_addTemporalMarkerOffset(.3, () -> { - shoot(); - }) - .waitSeconds(1) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - reset(); - }) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - spintakeStart(); - }) - .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), - SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - spintakeEnd(); - }) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - shoot(); - }) - .waitSeconds(1) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - reset(); - }) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .forward(10) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - spintakeStart(); - }) - .strafeLeft(10) - .forward(3) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - spintakeEnd(); - }) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - shoot(); - }) - .waitSeconds(1) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - spintakeStart(); - }) - .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), - SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - spintakeEnd(); - }) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - shoot(); - }) - .waitSeconds(1) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - reset(); - }) - .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) - .build(); - - waitForStart(); - if (opModeIsActive() && !isStopRequested()) { -// while (opModeIsActive()) { - drive.followTrajectorySequence(oneCycle); - PoseStorage.currentPose = drive.getPoseEstimate(); -// } - } - } -} \ No newline at end of file +package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +//import org.firstinspires.ftc.teamcode.drive.DriveConstants; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +// +//@Config +//@Autonomous(name = "Blue Depot Cycle") +//public class blueDepot extends LinearOpMode { +// +// Hware robot = new Hware(); +// public static double TICKS_PER_DEGREE = 9.738888; +// +// public void turretPower(double power) { +// robot.turret.setPower(power); +// } +// +// public void turretPos(int position) { +// turretPower(0); +// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// turretPower(1); +// } +// +// public void spintakeStart(){ +// robot.intake.set(1); +// } +// +// public void spintakeEnd(){ +// robot.intake.set(0); +// } +// +// public void shoot(){ +// robot.topFly.set(0.85); +// robot.bottomFly.set(0.85); +// robot.launchHood.setPosition(0.4); +// } +// +// public void reset(){ +// robot.intake.set(0); +//// robot.topFly.set(0); +//// robot.bottomFly.set(0); +// robot.launchHood.setPosition(0.35); +// robot.activeTransfer.setPosition(0.8); +// } +// +// +// public void runOpMode() throws InterruptedException { +// +// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// robot.initialize(hardwareMap); +// +// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); +// +// drive.setPoseEstimate(startPose); +// +// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// reset(); +// turretPos(5); +// }) +// .strafeLeft(35) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { +// robot.intake.set(0.9); +// }) +// .UNSTABLE_addTemporalMarkerOffset(2, () -> { +// robot.activeTransfer.setPosition(1); +// }) +//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +//// robot.launchHood.setPosition(0.5); +//// }) +// .waitSeconds(3) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(12,50, Math.toRadians(-45))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { +// robot.intake.set(0.9); +// }) +// .UNSTABLE_addTemporalMarkerOffset(2, () -> { +// robot.activeTransfer.setPosition(1); +// }) +//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +//// robot.launchHood.setPosition(0.5); +//// }) +// .waitSeconds(3) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { +// robot.intake.set(0.9); +// }) +// .UNSTABLE_addTemporalMarkerOffset(2, () -> { +// robot.activeTransfer.setPosition(1); +// }) +//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +//// robot.launchHood.setPosition(0.5); +//// }) +// .waitSeconds(3) +// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> { +// turretPos(84); +// }) +// .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45))) +// // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45))) +// .build(); +// +//// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose) +//// .strafeLeft(25) +//// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// reset(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeStart(); +//// }) +//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeEnd(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// reset(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .forward(10) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeStart(); +//// }) +//// .strafeLeft(10) +//// .forward(3) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeEnd(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeStart(); +//// }) +//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeEnd(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// reset(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .build(); +// +// waitForStart(); +// if (opModeIsActive() && !isStopRequested()) { +//// while (opModeIsActive()) { +// drive.followTrajectorySequence(threeBall); +// PoseStorage.currentPose = drive.getPoseEstimate(); +//// } +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java new file mode 100644 index 0000000..97ff20a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +// +//@Config +//@Autonomous(name = "Blue Depot Far NO USE") +//public class blueDepotFar extends LinearOpMode { +// +// Hware robot = new Hware(); +// public static double TICKS_PER_DEGREE = 9.738888; +// +// public void turretPower(double power) { +// robot.turret.setPower(power); +// } +// +// public void turretPos(int position) { +// turretPower(0); +// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// turretPower(1); +// } +// +// public void spintakeStart(){ +// robot.intake.set(1); +// } +// +// public void spintakeEnd(){ +// robot.intake.set(0); +// } +// +// public void shoot(){ +// robot.topFly.set(1); +// robot.bottomFly.set(1); +// robot.launchHood.setPosition(0.58); +// } +// +// public void reset(){ +// robot.intake.set(0); +//// robot.topFly.set(0); +//// robot.bottomFly.set(0); +// robot.launchHood.setPosition(0.35); +// robot.activeTransfer.setPosition(0.8); +// } +// +// +// public void runOpMode() throws InterruptedException { +// +// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// robot.initialize(hardwareMap); +// +// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); +// +// drive.setPoseEstimate(startPose); +// +// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) +// .waitSeconds(3) +// .forward(5) +// .build(); +// +// waitForStart(); +// if (opModeIsActive() && !isStopRequested()) { +//// while (opModeIsActive()) { +// drive.followTrajectorySequence(threeBall); +// PoseStorage.currentPose = drive.getPoseEstimate(); +//// } +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java new file mode 100644 index 0000000..b80bb27 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java @@ -0,0 +1,215 @@ +package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +//import org.firstinspires.ftc.teamcode.drive.DriveConstants; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +// +//@Config +//@Autonomous(name = "Red Depot Cycle") +//public class redDepot extends LinearOpMode { +// +// Hware robot = new Hware(); +// public static double TICKS_PER_DEGREE = 9.738888; +// +// public void turretPower(double power) { +// robot.turret.setPower(power); +// } +// +// public void turretPos(int position) { +// turretPower(0); +// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// turretPower(1); +// } +// +// public void spintakeStart(){ +// robot.intake.set(1); +// } +// +// public void spintakeEnd(){ +// robot.intake.set(0); +// } +// +// public void shoot(){ +// robot.topFly.set(0.85); +// robot.bottomFly.set(0.85); +// robot.launchHood.setPosition(0.4); +// } +// +// public void reset(){ +// robot.intake.set(0); +//// robot.topFly.set(0); +//// robot.bottomFly.set(0); +// robot.launchHood.setPosition(0.35); +// robot.activeTransfer.setPosition(0.8); +// } +// +// +// public void runOpMode() throws InterruptedException { +// +// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// robot.initialize(hardwareMap); +// +// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); +// +// drive.setPoseEstimate(startPose); +// +// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// reset(); +// turretPos(-10); +// }) +// .strafeRight(35) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { +// robot.intake.set(0.9); +// }) +// .UNSTABLE_addTemporalMarkerOffset(2, () -> { +// robot.activeTransfer.setPosition(1); +// }) +//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +//// robot.launchHood.setPosition(0.5); +//// }) +// .waitSeconds(3) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(12,-50, Math.toRadians(45))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { +// robot.intake.set(0.9); +// }) +// .UNSTABLE_addTemporalMarkerOffset(2, () -> { +// robot.activeTransfer.setPosition(1); +// }) +//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +//// robot.launchHood.setPosition(0.5); +//// }) +// .waitSeconds(3) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { +// robot.intake.set(0.9); +// }) +// .UNSTABLE_addTemporalMarkerOffset(2, () -> { +// robot.activeTransfer.setPosition(1); +// }) +//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +//// robot.launchHood.setPosition(0.5); +//// }) +// .waitSeconds(3) +// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> { +// turretPos(-84); +// }) +// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45))) +// // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45))) +// .build(); +// +//// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose) +//// .strafeLeft(25) +//// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// reset(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeStart(); +//// }) +//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeEnd(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// reset(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .forward(10) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeStart(); +//// }) +//// .strafeLeft(10) +//// .forward(3) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeEnd(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeStart(); +//// }) +//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// spintakeEnd(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// shoot(); +//// }) +//// .waitSeconds(1) +//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +//// reset(); +//// }) +//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +//// .build(); +// +// waitForStart(); +// if (opModeIsActive() && !isStopRequested()) { +//// while (opModeIsActive()) { +// drive.followTrajectorySequence(threeBall); +// PoseStorage.currentPose = drive.getPoseEstimate(); +//// } +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java deleted file mode 100644 index 4db4b9a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java +++ /dev/null @@ -1,120 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; - -import java.lang.reflect.Method; - -@TeleOp -@Config -public class DriverControlsDoubleV1 extends LinearOpMode { - - @Override - public void runOpMode() { - - Hware robot = new Hware(); - robot.initialize(hardwareMap); - - GamepadEx driverControl = new GamepadEx(gamepad1); - GamepadEx armControl = new GamepadEx(gamepad2); - - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - boolean xPrev = false; - boolean bPrev = false; - - final boolean[] shooting = { false }; - - Pose2d startPose = drive.getPoseEstimate(); - - TrajectorySequence shootSeq = drive.trajectorySequenceBuilder(startPose) - .addTemporalMarker(0.00, () -> { - robot.intake.set(0); - robot.activeTransfer.setPosition(0.7); - robot.topFly.set(1); - robot.bottomFly.set(1); - }) - .waitSeconds(0.50) - .addTemporalMarker(0.00, () -> { - robot.activeTransfer.setPosition(1); - robot.intake.set(1); - }) - .waitSeconds(0.70) - .addTemporalMarker(0.00, () -> { - stopAll(robot); - shooting[0] = false; - }) - .build(); - - waitForStart(); - - while (opModeIsActive()) { - - drive.update(); - - if (!shooting[0]) { - if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { - robot.intake.set(1); - robot.activeTransfer.setPosition(0.7); - } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { - robot.intake.set(-1); - } else { - robot.intake.set(0); - } - } - - boolean xNow = armControl.getButton(GamepadKeys.Button.X); - if (xNow && !xPrev && !shooting[0]) { - shooting[0] = true; - drive.followTrajectorySequenceAsync(shootSeq); - } - xPrev = xNow; - - boolean bNow = armControl.getButton(GamepadKeys.Button.B); - if (bNow && !bPrev && shooting[0]) { - cancelTrajectorySequenceCompat(drive); - stopAll(robot); - shooting[0] = false; - } - bPrev = bNow; - - telemetry.addData("Shooting", shooting[0]); - telemetry.addData("RR Busy", drive.isBusy()); - telemetry.update(); - } - } - - private static void stopAll(Hware robot) { - robot.intake.set(0); - robot.topFly.set(0); - robot.bottomFly.set(0); - robot.activeTransfer.setPosition(0.7); - } - - private static void cancelTrajectorySequenceCompat(Object drive) { - if (invokeIfExists(drive, "cancelTrajectorySequence")) return; - if (invokeIfExists(drive, "cancelFollowing")) return; - if (invokeIfExists(drive, "breakFollowing")) return; - try { - Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class); - m.invoke(drive, new Pose2d(0, 0, 0)); - } catch (Exception ignored) {} - } - - private static boolean invokeIfExists(Object obj, String methodName) { - try { - Method m = obj.getClass().getMethod(methodName); - m.invoke(obj); - return true; - } catch (Exception e) { - return false; - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java new file mode 100644 index 0000000..bf4f32b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; + +@Config +@TeleOp(name="FlyOpTuner", group="Test") +public class FlyOp extends LinearOpMode { + + private Hware robot; + + // Dashboard-editable + public static double TARGET = 1500.0; // ticks/sec (flip sign if needed) + public static double launchPos = 1.0; // servo position (0..1 usually) + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Hware(); + robot.initialize(hardwareMap); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + + robot.bottomFly.setVelocity(TARGET); + robot.topFly.setVelocity(TARGET); + robot.launchHood.setPosition(launchPos); + + if (gamepad1.dpadUpWasPressed()) { + robot.intake.set(.8); + } + + if (gamepad1.dpadDownWasPressed()) { + robot.intake.set(0); + } + + telemetry.addData("Target", (int)Math.round(TARGET)); + telemetry.addData("Top vel", (int)Math.round(robot.topFly.getVelocity())); + telemetry.addData("Bottom vel", (int)Math.round(robot.bottomFly.getVelocity())); + telemetry.addData("Hood pos", launchPos); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java new file mode 100644 index 0000000..d1c25e1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode; + +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.PIDFCoefficients; + +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; + +@TeleOp(name="FlywheelTuner", group="Tuning") +public class FlywheelTuner extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + Hware robot = new Hware(); + robot.initialize(hardwareMap); + + // ticks/sec targets (NOT rpm) + double highVelocity = 1500; + double lowVelocity = -900; + double curTargetVelocity = highVelocity; + + double F = 0.0; + double P = 0.0; + + double[] stepSizes = {10.0, 1.0, 0.1, 0.01, 0.001, 0.0001}; + int stepIndex = 1; + + telemetry.addLine("Y = toggle target, B = step size, DPAD U/D = P, DPAD L/R = F"); + telemetry.addLine("Press START when ready."); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + + // ---- controls ---- + if (gamepad1.yWasPressed()) { + curTargetVelocity = (curTargetVelocity == highVelocity) ? lowVelocity : highVelocity; + } + + if (gamepad1.bWasPressed()) { + stepIndex = (stepIndex + 1) % stepSizes.length; + } + + if (gamepad1.dpadLeftWasPressed()) F -= stepSizes[stepIndex]; + if (gamepad1.dpadRightWasPressed()) F += stepSizes[stepIndex]; + + if (gamepad1.dpadUpWasPressed()) P += stepSizes[stepIndex]; + if (gamepad1.dpadDownWasPressed()) P -= stepSizes[stepIndex]; + + // keep P/F non-negative (usually) + if (P < 0) P = 0; + if (F < 0) F = 0; + + // ---- apply PIDF LIVE ---- + PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F); + robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + + // ---- command velocity LIVE ---- + robot.topFly.setVelocity(curTargetVelocity); + robot.bottomFly.setVelocity(curTargetVelocity); + + // ---- read velocity ---- + double curTopVelocity = robot.topFly.getVelocity(); + double curBottomVelocity = robot.bottomFly.getVelocity(); + + // ---- telemetry ---- + telemetry.addData("Target (ticks/s)", "%.1f", curTargetVelocity); + telemetry.addData("Top (ticks/s)", "%.1f", curTopVelocity); + telemetry.addData("Bottom (ticks/s)", "%.1f", curBottomVelocity); + telemetry.addData("Top error", "%.1f", (curTargetVelocity - curTopVelocity)); + telemetry.addData("Bottom error", "%.1f", (curTargetVelocity - curBottomVelocity)); + telemetry.addLine("-----------"); + telemetry.addData("P (DPAD U/D)", "%.6f", P); + telemetry.addData("F (DPAD L/R)", "%.6f", F); + telemetry.addData("Step (B)", "%.6f", stepSizes[stepIndex]); + telemetry.update(); + + // small delay so button presses feel clean + telemetry readable + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HwareV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HwareV2.java new file mode 100644 index 0000000..b1e4869 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HwareV2.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode; + +import com.arcrobotics.ftclib.hardware.SimpleServo; +import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +public class HwareV2 { + + /* ===================== HARDWARE ===================== */ + public Motor frontRight, frontLeft, backRight, backLeft; + public Motor intake; + public Motor bottomFly, topFly; // declare flywheels here + public DcMotorEx turret; + + public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; + + private VoltageSensor battery; + private HardwareMap hardwareMap; + + /* ===================== INITIALIZE ===================== */ + public void initialize(HardwareMap hwMap) { + hardwareMap = hwMap; + + /* -------- Drivetrain -------- */ + frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435); + frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435); + backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435); + backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435); + + frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + + /* -------- Intake -------- */ + intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620); + intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + + /* -------- Flywheels -------- */ + bottomFly = new Motor(hardwareMap, "bottomFly", Motor.GoBILDA.BARE); + topFly = new Motor(hardwareMap, "topFly", Motor.GoBILDA.BARE); + + /* -------- Turret -------- */ + turret = hardwareMap.get(DcMotorEx.class, "turret"); + + /* -------- Servos -------- */ + activeTransfer = new SimpleServo(hardwareMap, "activeTransfer", 0, 100); + leftPTO = new SimpleServo(hardwareMap, "leftPTO", 0, 90); + rightPTO = new SimpleServo(hardwareMap, "rightPTO", 0, 90); + launchHood = new SimpleServo(hardwareMap, "launchHood", 0, 180); + + /* -------- Battery -------- */ + battery = hardwareMap.voltageSensor.iterator().next(); + } + + /* ===================== BATTERY ===================== */ + public double getBatteryVoltage() { + return battery.getVoltage(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AlignmentTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AlignmentTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AlignmentTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AlignmentTest.java index 3bdaf28..4ed8adb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AlignmentTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AlignmentTest.java @@ -1,19 +1,15 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.OUTDATED; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.control.PIDFController; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.arcrobotics.ftclib.hardware.motors.Motor; 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.Gamepad; -import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java index f566318..6ed88d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode; - -import android.util.Size; +package org.firstinspires.ftc.teamcode.OUTDATED; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.drivebase.MecanumDrive; @@ -12,10 +10,8 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; - import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutonV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutonV1.java new file mode 100644 index 0000000..b0cb92d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutonV1.java @@ -0,0 +1,11 @@ +package org.firstinspires.ftc.teamcode.OUTDATED; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +public class AutonV1 extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java similarity index 74% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java index c6734fd..487a25e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java @@ -1,20 +1,20 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.OUTDATED; -import com.arcrobotics.ftclib.hardware.ServoEx; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; public class Hware { HardwareMap hardwareMap; - public Motor frontRight, frontLeft, backRight, backLeft, intake, bottomFly, topFly; - public DcMotor turret; + public Motor frontRight, frontLeft, backRight, backLeft, intake; + public DcMotorEx turret, bottomFly, topFly; public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; WebcamName webcam; public void initialize(HardwareMap hwMap) { @@ -32,8 +32,11 @@ public class Hware { intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620); // Launch - bottomFly = new Motor(hardwareMap, "bottomFly", Motor.GoBILDA.BARE); - topFly = new Motor(hardwareMap, "topFly", Motor.GoBILDA.BARE); + bottomFly = hardwareMap.get(DcMotorEx.class, "bottomFly"); + topFly = hardwareMap.get(DcMotorEx.class, "topFly"); + + bottomFly.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + topFly.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // Turret turret = hardwareMap.get(DcMotorEx.class, "turret"); @@ -55,6 +58,10 @@ public class Hware { backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - bottomFly.setInverted(true); + PIDFCoefficients pidf = new PIDFCoefficients(0.45, 0, 0, 16.3); + bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + + bottomFly.setDirection(DcMotorSimple.Direction.REVERSE); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/OdometryAutoLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/OdometryAutoLock.java new file mode 100644 index 0000000..5576e48 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/OdometryAutoLock.java @@ -0,0 +1,319 @@ +package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.control.PIDFController; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.geometry.Vector2d; +//import com.arcrobotics.ftclib.gamepad.GamepadEx; +//import com.arcrobotics.ftclib.gamepad.GamepadKeys; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED; +//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +//import org.firstinspires.ftc.teamcode.util.PersonalPID; +// +//import java.lang.reflect.Method; +// +// +//@TeleOp (name = "OdoAutoLock") +//@Config +//public class OdometryAutoLock extends LinearOpMode { +// +// // Declare a PIDF Controller to regulate heading +// // Use the same gains as SampleMecanumDrive's heading controller +// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID); +// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90° +// +// // Declare a target vector you'd like your bot to align with +// // Can be any x/y coordinate of your choosing +// private Vector2d targetPosition = new Vector2d(0, 45); +// public static double multiplier = 15; +// +// public static double penguin = 0; +// private PersonalPID controller; +// +// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001; +// +// public static int target = 0; +// +// public static double TICKS_PER_DEGREE = (384.5 * 9.2) / 360; +// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90)); +// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90)); +// +// public static double goalX = 0; //OR 10 +// public static double goalY = 40;// OR 0 +// public static double goalH = - Math.PI / 2; // OR 3.14159265358979 +// +// +// @Override +// public void runOpMode() throws InterruptedException { +// +// /** SETUP **/ +// +// // Hardware Map Setup +// Hware robot = new Hware(); +// robot.initialize(hardwareMap); +// controller = new PersonalPID(p, i, d, f); +// +// // Gamepad Setup +// GamepadEx driverControl = new GamepadEx(gamepad1); +// GamepadEx armControl = new GamepadEx(gamepad2); +// +// // DriveBase Setup +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap); +// +// double SPEEDCONTROL = 1; +// Boolean auto = false; +// // Retrieve pose +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// drive.setPoseEstimate(PoseStorage.currentPose); +// +//// double cx = PoseStorage.currentPose.getX(); +//// double cy = PoseStorage.currentPose.getY(); +//// +//// double ch = PoseStorage.currentPose.getHeading(); +//// +//// double x = cx - 60; //tune this 60 +//// +//// double y = cy - 60; //TUUUNE +//// +//// double h = ch - Math.toRadians(45); +//// +//// Pose2d newPose = new Pose2d(x,y,h); +// +// +// +// drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose); +// headingController.setInputBounds(-Math.PI, Math.PI); +// +// boolean xPrev = false; +// boolean bPrev = false; +// +// final boolean[] shooting = { false }; +// +// Pose2d startPose = drive.getPoseEstimate(); +// +// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(1); +// robot.bottomFly.set(1); +// robot.activeTransfer.setPosition(1); +// robot.intake.set(1); +// }) +// .build(); +// +// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(1); +// robot.bottomFly.set(1); +// }) +// .forward(.1) +// .waitSeconds(1.5) +// .forward(.1) +// .addDisplacementMarker(()-> { +// gamepad1.rumbleBlips(2); +// gamepad2.rumbleBlips(2); +// }) +// .build(); +// robot.activeTransfer.setPosition(0.8); +// +// +// +// +// double offset = 0.0; +// +// waitForStart(); +// +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// +// while (opModeIsActive()) { +// +// double distance = Math.hypot(drive.getPoseEstimate().getX() - goalX, drive.getPoseEstimate().getY() - goalY); +// double height = 70; +// double launchAngle = Math.toDegrees(Math.atan2(height, distance)); +// if(launchAngle > 80){ +// launchAngle = 80; +// } else if (launchAngle < 40){ +// launchAngle = 40; +// } +// double offsetLaunch = (launchAngle / 40); +// if(offsetLaunch > 0.5){ +// offsetLaunch = 0.5; +// } +// double hoodPosition = 0.85 - offsetLaunch; +// robot.launchHood.setPosition(hoodPosition); +// /** DRIVER CONTROLS **/ +// +// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; } +// +// // Read pose +// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate(); +// +// // Declare a drive direction +// // Pose representing desired x, y, and angular velocity +// Pose2d driveDirection = new Pose2d(); +// +// // Standard teleop control +// // Convert gamepad input into desired pose velocity +// driveDirection = new Pose2d( +// -gamepad1.left_stick_y * SPEEDCONTROL, +// -gamepad1.left_stick_x * SPEEDCONTROL, +// -gamepad1.right_stick_x * SPEEDCONTROL +// ); +// +// +// drive.setWeightedDrivePower(driveDirection); +// +// double headingPose = poseEstimate.getHeading(); +// //9.26 +// +// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI); +//// +// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading(); +//// theta = Math.atan2(Math.sin(theta), Math.cos(theta)); +// +// boolean withinAngle = true; +// //Math.abs(theta) < MAX_LOCK_ANGLE; +// +//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){ +//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +//// } +// +// +//// controller.setPIDF(p, i, d, f); +//// int armPos = robot.turret.getCurrentPosition(); +//// double pid = controller.calculate(armPos, target); +// +// double deltaX = drive.getPoseEstimate().getX() - goalX; +// double deltaY = drive.getPoseEstimate().getY() - goalY; +// double deltaH = drive.getPoseEstimate().getHeading() - goalH; +// +// double desiredTurretAngleDeg = Math.toDegrees( +// Math.atan2(deltaY, deltaX) +// ); double robotHeadingDeg = Math.toDegrees(deltaH); double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; +// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc. +//// Normalize to [-180, 180] +// while (turretAngleDeg > 180) turretAngleDeg -= 360; +// while (turretAngleDeg < -180) turretAngleDeg += 360; +// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars +//// Clamp to physical limits +// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS)); +// +// double desiredTarget = targetTurretPos; +// +// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// // Clamp to turret mechanical limits +// desiredTarget = Math.max( +// TURRET_MIN_TICKS, +// Math.min(TURRET_MAX_TICKS, desiredTarget) +// ); +// +// robot.turret.setTargetPosition((int) desiredTarget); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// robot.turret.setPower(1); +// +// +// +//// target = desiredTarget; +//// controller.setPIDF(p, i, d, f); +//// pid = controller.calculate(armPos, target); +// // robot.turret.setPower(pid); +// +// telemetry.addData("target", desiredTarget); +// telemetry.addData("target", target); +//// telemetry.addData("pid", pid); +// +// // auton updates +// // AUTO END -> take x and y and subtract (subtract x and y) +// // heading add 45 +// +// driveRIGGED.update(); +// +// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(1); +// robot.activeTransfer.setPosition(0.8); +// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(-1); +// } else { +// if (!auto) { +// robot.intake.set(0); +// } +// } +// +// if(gamepad2.dpad_down) { +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// } +// +// +// if (gamepad2.dpad_up) { +// if(!auto) { +// auto = true; +// driveRIGGED.followTrajectorySequenceAsync(shootSeq); +// } +// } +// +// if (gamepad2.dpad_right) { +// driveRIGGED.followTrajectorySequenceAsync(runUp); +// } +// +// telemetry.addData("Shooting", shooting[0]); +// telemetry.addData("RR Busy", drive.isBusy()); +// +// // Update the localizer +// drive.getLocalizer().update(); +// // Print pose to telemetry +// telemetry.addData("totalHeading", totalHeading); +// telemetry.addData("theta", theta); +// telemetry.addData("x", poseEstimate.getX()); +// telemetry.addData("y", poseEstimate.getY()); +// telemetry.addData("heading", poseEstimate.getHeading()); +// telemetry.addData("auton", auto); +// telemetry.update(); +// +// } +// +// +// +// while (!isStopRequested() && opModeIsActive()); +// } +// +// private static void stopAll(Hware robot) { +// robot.intake.set(0); +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.85); +// } +// +// private static void cancelTrajectorySequenceCompat(Object drive) { +// if (invokeIfExists(drive, "cancelTrajectorySequence")) return; +// if (invokeIfExists(drive, "cancelFollowing")) return; +// if (invokeIfExists(drive, "breakFollowing")) return; +// try { +// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class); +// m.invoke(drive, new Pose2d(0, 0, 0)); +// } catch (Exception ignored) {} +// } +// +// private static boolean invokeIfExists(Object obj, String methodName) { +// try { +// Method m = obj.getClass().getMethod(methodName); +// m.invoke(obj); +// return true; +// } catch (Exception e) { +// return false; +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/ServoTesting.java similarity index 60% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/ServoTesting.java index adb3644..1554b09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/ServoTesting.java @@ -1,25 +1,10 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.OUTDATED; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.control.PIDFController; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.arcrobotics.ftclib.hardware.motors.Motor; 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.Gamepad; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; - -import java.lang.reflect.Method; @TeleOp (name = "Servo") @@ -50,7 +35,7 @@ public class ServoTesting extends LinearOpMode { if (armControl.getButton(GamepadKeys.Button.DPAD_UP)) { robot.activeTransfer.setPosition(1); } else if (armControl.getButton(GamepadKeys.Button.DPAD_DOWN)) { - robot.activeTransfer.setPosition(0.85); + robot.activeTransfer.setPosition(0.8); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testing.java new file mode 100644 index 0000000..6abb64e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testing.java @@ -0,0 +1,330 @@ +package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.control.PIDFController; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.geometry.Vector2d; +//import com.arcrobotics.ftclib.gamepad.GamepadEx; +//import com.arcrobotics.ftclib.gamepad.GamepadKeys; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED; +//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +//import org.firstinspires.ftc.teamcode.util.PersonalPID; +// +//import java.lang.reflect.Method; +// +// +//@TeleOp (name = "TeleOp") +//@Config +//public class Testing extends LinearOpMode { +// +// // Declare a PIDF Controller to regulate heading +// // Use the same gains as SampleMecanumDrive's heading controller +// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID); +// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90° +// +// // Declare a target vector you'd like your bot to align with +// // Can be any x/y coordinate of your choosing +// private Vector2d targetPosition = new Vector2d(70, 45); +// public static double multiplier = 15; +// +// public static double penguin = 0; +// private PersonalPID controller; +// +// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001; +// +// public static int target = 0; +// +// public static double TICKS_PER_DEGREE = 9.738888; +// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90)); +// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90)); +// +// public static double goalX = 64; +// public static double goalY = 75; +// public static double goalH = - Math.PI / 2; // OR 3.14159265358979 +// +// public static double launchMultiplier = 5; +// +// +// +// +// @Override +// public void runOpMode() throws InterruptedException { +// +// /** SETUP **/ +// +// // Hardware Map Setup +// Hware robot = new Hware(); +// robot.initialize(hardwareMap); +// controller = new PersonalPID(p, i, d, f); +// +// // Gamepad Setup +// GamepadEx driverControl = new GamepadEx(gamepad1); +// GamepadEx armControl = new GamepadEx(gamepad2); +// +// // DriveBase Setup +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap); +// +// double SPEEDCONTROL = 1; +// double gamepadLaunch = 0; +// +// Boolean auto = false; +// // Retrieve pose +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose); +// drive.setPoseEstimate(PoseStorage.currentPose); +// headingController.setInputBounds(-Math.PI, Math.PI); +// +// boolean xPrev = false; +// boolean bPrev = false; +// +// final boolean[] shooting = { false }; +// +// Pose2d startPose = drive.getPoseEstimate(); +// +// +// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(1); +// robot.bottomFly.set(1); +// robot.activeTransfer.setPosition(1); +// robot.intake.set(0.9); +// }) +// .waitSeconds(.5) +// .build(); +// +// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(0.8); +// robot.bottomFly.set(0.8); +// }) +// .forward(.1) +// .waitSeconds(1.5) +// .forward(.1) +// .addDisplacementMarker(()-> { +// gamepad1.rumbleBlips(2); +// gamepad2.rumbleBlips(2); +// }) +// .build(); +// robot.activeTransfer.setPosition(0.78); +// +// +// +// +// double offset = 0.0; +// +// waitForStart(); +// +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// +// while (opModeIsActive()) { +// +// double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY()); +// double height = 70; +// double launchAngle = Math.toDegrees(Math.atan2(distance, height)); +// telemetry.addData("angle", launchAngle); +// if(launchAngle > 70){ +// launchAngle = 70; +// } else if (launchAngle < 15){ +// launchAngle = 15; +// } +// double offsetLaunch = (launchAngle / (55 * launchMultiplier)); +// if(offsetLaunch > 0.5){ +// offsetLaunch = 0.5; +// } +// double hoodPosition = 0.35 + offsetLaunch; +// if(gamepad2.right_bumper){ +// gamepadLaunch += 0.02; +// } else if(gamepad2.left_bumper){ +// gamepadLaunch -= 0.02; +// } +// hoodPosition += gamepadLaunch; +// telemetry.addData("hood", hoodPosition); +// robot.launchHood.setPosition(hoodPosition); +// /** DRIVER CONTROLS **/ +// +// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; } +// +// // Read pose +// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate(); +// +// // Declare a drive direction +// // Pose representing desired x, y, and angular velocity +// Pose2d driveDirection = new Pose2d(); +// +// // Standard teleop control +// // Convert gamepad input into desired pose velocity +// driveDirection = new Pose2d( +// -gamepad1.left_stick_y * SPEEDCONTROL, +// -gamepad1.left_stick_x * SPEEDCONTROL, +// -gamepad1.right_stick_x * SPEEDCONTROL +// ); +// +// +// drive.setWeightedDrivePower(driveDirection); +// +// double headingPose = poseEstimate.getHeading(); +// //9.26 +// +// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI); +//// +// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading(); +//// theta = Math.atan2(Math.sin(theta), Math.cos(theta)); +// +// boolean withinAngle = true; +// //Math.abs(theta) < MAX_LOCK_ANGLE; +// +//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){ +//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +//// } +// +// +// controller.setPIDF(p, i, d, f); +// int armPos = robot.turret.getCurrentPosition(); +// double pid = controller.calculate(armPos, target); +// +// double deltaX = drive.getPoseEstimate().getX() - goalX; +// double deltaY = drive.getPoseEstimate().getY() - goalY; +// double deltaH = drive.getPoseEstimate().getHeading() - goalH; +// +// double desiredTurretAngleDeg = Math.toDegrees( +// Math.atan2(deltaY, deltaX) +// ); double robotHeadingDeg = Math.toDegrees(deltaH); double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; +// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc. +//// Normalize to [-180, 180] +// while (turretAngleDeg > 180) turretAngleDeg -= 360; +// while (turretAngleDeg < -180) turretAngleDeg += 360; +// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars +//// Clamp to physical limits +// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS)); +// +// +// double desiredTarget = targetTurretPos; +// +// offset += multiplier * (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// // Clamp to turret mechanical limits +// desiredTarget = Math.max( +// TURRET_MIN_TICKS, +// Math.min(TURRET_MAX_TICKS, desiredTarget) +// ); +// +// desiredTarget = TURRET_MIN_TICKS; +// //forBlueSIde MAX TICKS +// +// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// desiredTarget = Math.max( +// TURRET_MIN_TICKS, +// Math.min(TURRET_MAX_TICKS, desiredTarget) +// ); +// +// +// robot.turret.setTargetPosition((int) desiredTarget); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// robot.turret.setPower(1); +// +// +// +//// target = desiredTarget; +//// controller.setPIDF(p, i, d, f); +//// pid = controller.calculate(armPos, target); +// // robot.turret.setPower(pid); +// +//// telemetry.addData("target", desiredTarget); +//// telemetry.addData("target", target); +//// telemetry.addData("pid", pid); +//// telemetry.update(); +// +// driveRIGGED.update(); +// +// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(0.9); +// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(-0.9); +// } else { +// if (!auto) { +// robot.intake.set(0); +// } +// } +// +// if(gamepad2.dpad_down) { +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.78); +// } +// +// +// if (gamepad2.dpad_up) { +// if(!auto) { +// auto = true; +// driveRIGGED.followTrajectorySequenceAsync(shootSeq); +// } +// } +// +// if (gamepad2.dpad_right) { +// driveRIGGED.followTrajectorySequenceAsync(runUp); +// } +// +//// telemetry.addData("Shooting", shooting[0]); +//// telemetry.addData("RR Busy", drive.isBusy()); +// +// // Update the localizer +// drive.getLocalizer().update(); +// // Print pose to telemetry +//// telemetry.addData("totalHeading", totalHeading); +//// telemetry.addData("theta", theta); +// telemetry.addData("x", poseEstimate.getX()); +// telemetry.addData("y", poseEstimate.getY()); +// telemetry.addData("heading", poseEstimate.getHeading()); +// telemetry.addData("auton", auto); +// telemetry.update(); +// +// } +// +// +// +// while (!isStopRequested() && opModeIsActive()); +// } +// +// private static void stopAll(Hware robot) { +// robot.intake.set(0); +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.78); +// } +// +// private static void cancelTrajectorySequenceCompat(Object drive) { +// if (invokeIfExists(drive, "cancelTrajectorySequence")) return; +// if (invokeIfExists(drive, "cancelFollowing")) return; +// if (invokeIfExists(drive, "breakFollowing")) return; +// try { +// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class); +// m.invoke(drive, new Pose2d(0, 0, 0)); +// } catch (Exception ignored) {} +// } +// +// private static boolean invokeIfExists(Object obj, String methodName) { +// try { +// Method m = obj.getClass().getMethod(methodName); +// m.invoke(obj); +// return true; +// } catch (Exception e) { +// return false; +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testingv2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testingv2.java new file mode 100644 index 0000000..7544b16 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testingv2.java @@ -0,0 +1,330 @@ +package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.control.PIDFController; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.geometry.Vector2d; +//import com.arcrobotics.ftclib.gamepad.GamepadEx; +//import com.arcrobotics.ftclib.gamepad.GamepadKeys; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED; +//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +//import org.firstinspires.ftc.teamcode.util.PersonalPID; +// +//import java.lang.reflect.Method; +// +// +//@TeleOp (name = "TeleOp") +//@Config +//public class Testingv2 extends LinearOpMode { +// +// // Declare a PIDF Controller to regulate heading +// // Use the same gains as SampleMecanumDrive's heading controller +// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID); +// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90° +// +// // Declare a target vector you'd like your bot to align with +// // Can be any x/y coordinate of your choosing +// private Vector2d targetPosition = new Vector2d(70, 45); +// public static double multiplier = 15; +// +// public static double penguin = 0; +// private PersonalPID controller; +// +// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001; +// +// public static int target = 0; +// +// public static double TICKS_PER_DEGREE = 9.738888; +// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90)); +// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90)); +// +// public static double goalX = 64; +// public static double goalY = 75; +// public static double goalH = - Math.PI / 2; // OR 3.14159265358979 +// +// public static double launchMultiplier = 5; +// +// +// +// +// @Override +// public void runOpMode() throws InterruptedException { +// +// /** SETUP **/ +// +// // Hardware Map Setup +// Hware robot = new Hware(); +// robot.initialize(hardwareMap); +// controller = new PersonalPID(p, i, d, f); +// +// // Gamepad Setup +// GamepadEx driverControl = new GamepadEx(gamepad1); +// GamepadEx armControl = new GamepadEx(gamepad2); +// +// // DriveBase Setup +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap); +// +// double SPEEDCONTROL = 1; +// double gamepadLaunch = 0; +// +// Boolean auto = false; +// // Retrieve pose +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose); +// drive.setPoseEstimate(PoseStorage.currentPose); +// headingController.setInputBounds(-Math.PI, Math.PI); +// +// boolean xPrev = false; +// boolean bPrev = false; +// +// final boolean[] shooting = { false }; +// +// Pose2d startPose = drive.getPoseEstimate(); +// +// +// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(1); +// robot.bottomFly.set(1); +// robot.activeTransfer.setPosition(1); +// robot.intake.set(0.9); +// }) +// .waitSeconds(.5) +// .build(); +// +// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(0.8); +// robot.bottomFly.set(0.8); +// }) +// .forward(.1) +// .waitSeconds(1.5) +// .forward(.1) +// .addDisplacementMarker(()-> { +// gamepad1.rumbleBlips(2); +// gamepad2.rumbleBlips(2); +// }) +// .build(); +// robot.activeTransfer.setPosition(0.78); +// +// +// +// +// double offset = 0.0; +// +// waitForStart(); +// +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// +// while (opModeIsActive()) { +// +// double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY()); +// double height = 70; +// double launchAngle = Math.toDegrees(Math.atan2(distance, height)); +// telemetry.addData("angle", launchAngle); +// if(launchAngle > 70){ +// launchAngle = 70; +// } else if (launchAngle < 15){ +// launchAngle = 15; +// } +// double offsetLaunch = (launchAngle / (55 * launchMultiplier)); +// if(offsetLaunch > 0.5){ +// offsetLaunch = 0.5; +// } +// double hoodPosition = 0.35 + offsetLaunch; +// if(gamepad2.right_bumper){ +// gamepadLaunch += 0.02; +// } else if(gamepad2.left_bumper){ +// gamepadLaunch -= 0.02; +// } +// hoodPosition += gamepadLaunch; +// telemetry.addData("hood", hoodPosition); +// robot.launchHood.setPosition(hoodPosition); +// /** DRIVER CONTROLS **/ +// +// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; } +// +// // Read pose +// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate(); +// +// // Declare a drive direction +// // Pose representing desired x, y, and angular velocity +// Pose2d driveDirection = new Pose2d(); +// +// // Standard teleop control +// // Convert gamepad input into desired pose velocity +// driveDirection = new Pose2d( +// -gamepad1.left_stick_y * SPEEDCONTROL, +// -gamepad1.left_stick_x * SPEEDCONTROL, +// -gamepad1.right_stick_x * SPEEDCONTROL +// ); +// +// +// drive.setWeightedDrivePower(driveDirection); +// +// double headingPose = poseEstimate.getHeading(); +// //9.26 +// +// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI); +//// +// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading(); +//// theta = Math.atan2(Math.sin(theta), Math.cos(theta)); +// +// boolean withinAngle = true; +// //Math.abs(theta) < MAX_LOCK_ANGLE; +// +//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){ +//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +//// } +// +// +// controller.setPIDF(p, i, d, f); +// int armPos = robot.turret.getCurrentPosition(); +// double pid = controller.calculate(armPos, target); +// +// double deltaX = drive.getPoseEstimate().getX() - goalX; +// double deltaY = drive.getPoseEstimate().getY() - goalY; +// double deltaH = drive.getPoseEstimate().getHeading() - goalH; +// +// double desiredTurretAngleDeg = Math.toDegrees( +// Math.atan2(deltaY, deltaX) +// ); double robotHeadingDeg = Math.toDegrees(deltaH); double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; +// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc. +//// Normalize to [-180, 180] +// while (turretAngleDeg > 180) turretAngleDeg -= 360; +// while (turretAngleDeg < -180) turretAngleDeg += 360; +// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars +//// Clamp to physical limits +// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS)); +// +// +// double desiredTarget = targetTurretPos; +// +// offset += multiplier * (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// // Clamp to turret mechanical limits +// desiredTarget = Math.max( +// TURRET_MIN_TICKS, +// Math.min(TURRET_MAX_TICKS, desiredTarget) +// ); +// +// desiredTarget = TURRET_MIN_TICKS; +// //forBlueSIde MAX TICKS +// +// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// desiredTarget = Math.max( +// TURRET_MIN_TICKS, +// Math.min(TURRET_MAX_TICKS, desiredTarget) +// ); +// +// +// robot.turret.setTargetPosition((int) desiredTarget); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// robot.turret.setPower(1); +// +// +// +//// target = desiredTarget; +//// controller.setPIDF(p, i, d, f); +//// pid = controller.calculate(armPos, target); +// // robot.turret.setPower(pid); +// +//// telemetry.addData("target", desiredTarget); +//// telemetry.addData("target", target); +//// telemetry.addData("pid", pid); +//// telemetry.update(); +// +// driveRIGGED.update(); +// +// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(0.9); +// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(-0.9); +// } else { +// if (!auto) { +// robot.intake.set(0); +// } +// } +// +// if(gamepad2.dpad_down) { +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.78); +// } +// +// +// if (gamepad2.dpad_up) { +// if(!auto) { +// auto = true; +// driveRIGGED.followTrajectorySequenceAsync(shootSeq); +// } +// } +// +// if (gamepad2.dpad_right) { +// driveRIGGED.followTrajectorySequenceAsync(runUp); +// } +// +//// telemetry.addData("Shooting", shooting[0]); +//// telemetry.addData("RR Busy", drive.isBusy()); +// +// // Update the localizer +// drive.getLocalizer().update(); +// // Print pose to telemetry +//// telemetry.addData("totalHeading", totalHeading); +//// telemetry.addData("theta", theta); +// telemetry.addData("x", poseEstimate.getX()); +// telemetry.addData("y", poseEstimate.getY()); +// telemetry.addData("heading", poseEstimate.getHeading()); +// telemetry.addData("auton", auto); +// telemetry.update(); +// +// } +// +// +// +// while (!isStopRequested() && opModeIsActive()); +// } +// +// private static void stopAll(Hware robot) { +// robot.intake.set(0); +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.78); +// } +// +// private static void cancelTrajectorySequenceCompat(Object drive) { +// if (invokeIfExists(drive, "cancelTrajectorySequence")) return; +// if (invokeIfExists(drive, "cancelFollowing")) return; +// if (invokeIfExists(drive, "breakFollowing")) return; +// try { +// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class); +// m.invoke(drive, new Pose2d(0, 0, 0)); +// } catch (Exception ignored) {} +// } +// +// private static boolean invokeIfExists(Object obj, String methodName) { +// try { +// Method m = obj.getClass().getMethod(methodName); +// m.invoke(obj); +// return true; +// } catch (Exception e) { +// return false; +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/odoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/odoTest.java new file mode 100644 index 0000000..a3a3cab --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/odoTest.java @@ -0,0 +1,326 @@ +package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.control.PIDFController; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.geometry.Vector2d; +//import com.arcrobotics.ftclib.gamepad.GamepadEx; +//import com.arcrobotics.ftclib.gamepad.GamepadKeys; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.hardware.DcMotor; +// +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED; +//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +//import org.firstinspires.ftc.teamcode.util.PersonalPID; +// +//import java.lang.reflect.Method; +// +// +//@TeleOp (name = "odoTest") +//@Config +//public class odoTest extends LinearOpMode { +// +// // Declare a PIDF Controller to regulate heading +// // Use the same gains as SampleMecanumDrive's heading controller +// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID); +// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90° +// +// // Declare a target vector you'd like your bot to align with +// // Can be any x/y coordinate of your choosing +// private Vector2d targetPosition = new Vector2d(45, 70); +// public static double multiplier = 15; +// +// public static double penguin = 0; +// private PersonalPID controller; +// +// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001; +// +// public static int target = 0; +// +// public static double TICKS_PER_DEGREE = 9.738888; +// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90)); +// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90)); +// +// public static double goalX = 64; +// public static double goalY = 75; +// public static double goalH = ((2 * Math.PI) / 3); // OR 3.14159265358979 +// +// public static double launchMultiplier = 5; +// +// +// +// +// @Override +// public void runOpMode() throws InterruptedException { +// +// /** SETUP **/ +// +// // Hardware Map Setup +// Hware robot = new Hware(); +// robot.initialize(hardwareMap); +// controller = new PersonalPID(p, i, d, f); +// +// // Gamepad Setup +// GamepadEx driverControl = new GamepadEx(gamepad1); +// GamepadEx armControl = new GamepadEx(gamepad2); +// +// // DriveBase Setup +// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap); +// +// double SPEEDCONTROL = 1; +// double gamepadLaunch = 0; +// +// Boolean auto = false; +// // Retrieve pose +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// drive.getLocalizer().setPoseEstimate(new Pose2d(0,0,0)); +// drive.setPoseEstimate(new Pose2d(0,0,0)); +// headingController.setInputBounds(-Math.PI, Math.PI); +// +// boolean xPrev = false; +// boolean bPrev = false; +// +// final boolean[] shooting = { false }; +// +// Pose2d startPose = drive.getPoseEstimate(); +// +// +// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(1); +// robot.bottomFly.set(1); +// robot.activeTransfer.setPosition(1); +// robot.intake.set(0.9); +// }) +// .waitSeconds(.5) +// .build(); +// +// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose) +// .forward(.1) +// .addDisplacementMarker(()-> { +// robot.topFly.set(0.8); +// robot.bottomFly.set(0.8); +// }) +// .forward(.1) +// .waitSeconds(1.5) +// .forward(.1) +// .addDisplacementMarker(()-> { +// gamepad1.rumbleBlips(2); +// gamepad2.rumbleBlips(2); +// }) +// .build(); +// robot.activeTransfer.setPosition(0.78); +// +// +// +// +// double offset = 0.0; +// +// waitForStart(); +// +// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// +// while (opModeIsActive()) { +// +// double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY()); +// double height = 70; +// double launchAngle = Math.toDegrees(Math.atan2(distance, height)); +// telemetry.addData("angle", launchAngle); +// if(launchAngle > 70){ +// launchAngle = 70; +// } else if (launchAngle < 15){ +// launchAngle = 15; +// } +// double offsetLaunch = (launchAngle / (55 * launchMultiplier)); +// if(offsetLaunch > 0.5){ +// offsetLaunch = 0.5; +// } +// double hoodPosition = 0.35 + offsetLaunch; +// if(gamepad2.right_bumper){ +// gamepadLaunch += 0.02; +// } else if(gamepad2.left_bumper){ +// gamepadLaunch -= 0.02; +// } +// hoodPosition += gamepadLaunch; +// telemetry.addData("hood", hoodPosition); +// robot.launchHood.setPosition(hoodPosition); +// /** DRIVER CONTROLS **/ +// +// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; } +// +// // Read pose +// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate(); +// +// // Declare a drive direction +// // Pose representing desired x, y, and angular velocity +// Pose2d driveDirection = new Pose2d(); +// +// // Standard teleop control +// // Convert gamepad input into desired pose velocity +// driveDirection = new Pose2d( +// -gamepad1.left_stick_y * SPEEDCONTROL, +// -gamepad1.left_stick_x * SPEEDCONTROL, +// -gamepad1.right_stick_x * SPEEDCONTROL +// ); +// +// +// drive.setWeightedDrivePower(driveDirection); +// +// double headingPose = poseEstimate.getHeading(); +// //9.26 +// +// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI); +//// +// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading(); +//// theta = Math.atan2(Math.sin(theta), Math.cos(theta)); +// +// boolean withinAngle = true; +// //Math.abs(theta) < MAX_LOCK_ANGLE; +// +//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){ +//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +//// } +// +// +// controller.setPIDF(p, i, d, f); +// int armPos = robot.turret.getCurrentPosition(); +// double pid = controller.calculate(armPos, target); +// +// double deltaX = drive.getPoseEstimate().getX() - targetPosition.getX(); +// double deltaY = drive.getPoseEstimate().getY() - targetPosition.getY(); +// double deltaH = drive.getPoseEstimate().getHeading();// - goalH; +// +// double desiredTurretAngleDeg = Math.toDegrees( +// Math.atan2(deltaY, deltaX) +// ); +// double robotHeadingDeg = Math.toDegrees(deltaH); +// double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; +// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc. +//// Normalize to [-180, 180] +// while (turretAngleDeg > 180) turretAngleDeg -= 360; +// while (turretAngleDeg < -180) turretAngleDeg += 360; +// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars +//// Clamp to physical limits +// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS)); +// +// +// double desiredTarget = targetTurretPos; +// +// offset += multiplier * (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// // desiredTarget = TURRET_MIN_TICKS; +// //forBlueSIde MAX TICKS +// +// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger); +// +// desiredTarget+=offset; +// +// // Clamp to turret mechanical limits +// desiredTarget = Math.max( +// TURRET_MIN_TICKS, +// Math.min(TURRET_MAX_TICKS, desiredTarget) +// ); +// +// +// robot.turret.setTargetPosition((int) desiredTarget); +// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// robot.turret.setPower(1); +// +// +// +//// target = desiredTarget; +//// controller.setPIDF(p, i, d, f); +//// pid = controller.calculate(armPos, target); +// // robot.turret.setPower(pid); +// +//// telemetry.addData("target", desiredTarget); +//// telemetry.addData("target", target); +//// telemetry.addData("pid", pid); +//// telemetry.update(); +// +// driveRIGGED.update(); +// +// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(0.9); +// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { +// auto = false; +// robot.intake.set(-0.9); +// } else { +// if (!auto) { +// robot.intake.set(0); +// } +// } +// +// if(gamepad2.dpad_down) { +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.78); +// } +// +// +// if (gamepad2.dpad_up) { +// if(!auto) { +// auto = true; +// driveRIGGED.followTrajectorySequenceAsync(shootSeq); +// } +// } +// +// if (gamepad2.dpad_right) { +// driveRIGGED.followTrajectorySequenceAsync(runUp); +// } +// +//// telemetry.addData("Shooting", shooting[0]); +//// telemetry.addData("RR Busy", drive.isBusy()); +// +// // Update the localizer +// drive.getLocalizer().update(); +// // Print pose to telemetry +//// telemetry.addData("totalHeading", totalHeading); +//// telemetry.addData("theta", theta); +// telemetry.addData("x", poseEstimate.getX()); +// telemetry.addData("y", poseEstimate.getY()); +// telemetry.addData("heading", poseEstimate.getHeading()); +// telemetry.addData("auton", auto); +// telemetry.update(); +// +// } +// +// +// +// while (!isStopRequested() && opModeIsActive()); +// } +// +// private static void stopAll(Hware robot) { +// robot.intake.set(0); +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.activeTransfer.setPosition(0.78); +// } +// +// private static void cancelTrajectorySequenceCompat(Object drive) { +// if (invokeIfExists(drive, "cancelTrajectorySequence")) return; +// if (invokeIfExists(drive, "cancelFollowing")) return; +// if (invokeIfExists(drive, "breakFollowing")) return; +// try { +// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class); +// m.invoke(drive, new Pose2d(0, 0, 0)); +// } catch (Exception ignored) {} +// } +// +// private static boolean invokeIfExists(Object obj, String methodName) { +// try { +// Method m = obj.getClass().getMethod(methodName); +// m.invoke(obj); +// return true; +// } catch (Exception e) { +// return false; +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/readme.md similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/readme.md diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java deleted file mode 100644 index 258d01f..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java +++ /dev/null @@ -1,262 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.control.PIDFController; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED; -import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -import org.firstinspires.ftc.teamcode.util.PersonalPID; - -import java.lang.reflect.Method; -import java.util.concurrent.atomic.AtomicBoolean; - - -@TeleOp (name = "OdoAutoLock") -@Config -public class OdometryAutoLock extends LinearOpMode { - - // Declare a PIDF Controller to regulate heading - // Use the same gains as SampleMecanumDrive's heading controller - private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID); - public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90° - - // Declare a target vector you'd like your bot to align with - // Can be any x/y coordinate of your choosing - private Vector2d targetPosition = new Vector2d(0, 45); - - private PersonalPID controller; - - public static double p = 0.025, i = 0, d = 0.0001, f = 0.001; - - public static int target = 0; - - public static double TICKS_PER_DEGREE = (384.5 * 9.26) / 360; - public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90)); - public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90)); - - - @Override - public void runOpMode() throws InterruptedException { - - /** SETUP **/ - - // Hardware Map Setup - Hware robot = new Hware(); - robot.initialize(hardwareMap); - controller = new PersonalPID(p, i, d, f); - - // Gamepad Setup - GamepadEx driverControl = new GamepadEx(gamepad1); - GamepadEx armControl = new GamepadEx(gamepad2); - - - // DriveBase Setup - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap); - - double SPEEDCONTROL = 1; - Boolean auto = false; - // Retrieve pose - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose); - headingController.setInputBounds(-Math.PI, Math.PI); - - boolean xPrev = false; - boolean bPrev = false; - - final boolean[] shooting = { false }; - - Pose2d startPose = drive.getPoseEstimate(); - - TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose) - .forward(.1) - .addDisplacementMarker(()-> { - robot.activeTransfer.setPosition(1); - robot.intake.set(1); - }) - .build(); - - TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose) - .forward(.1) - .addDisplacementMarker(()-> { - robot.topFly.set(1); - robot.bottomFly.set(1); - }) - .forward(.1) - .waitSeconds(1.5) - .forward(.1) - .addDisplacementMarker(()-> { - gamepad1.rumbleBlips(2); - gamepad2.rumbleBlips(2); - }) - .build(); - - - waitForStart(); - - robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - while (opModeIsActive()) { - - double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY()); - double height = 62; - double launchAngle = Math.atan2(height, distance); - /** DRIVER CONTROLS **/ - - if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; } - - // Read pose - Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate(); - - // Declare a drive direction - // Pose representing desired x, y, and angular velocity - Pose2d driveDirection = new Pose2d(); - - // Standard teleop control - // Convert gamepad input into desired pose velocity - driveDirection = new Pose2d( - -gamepad1.left_stick_y * SPEEDCONTROL, - -gamepad1.left_stick_x * SPEEDCONTROL, - -gamepad1.right_stick_x * SPEEDCONTROL - ); - - - drive.setWeightedDrivePower(driveDirection); - - double headingPose = poseEstimate.getHeading(); - //9.26 - - double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI); -// - double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading(); -// theta = Math.atan2(Math.sin(theta), Math.cos(theta)); - - boolean withinAngle = - Math.abs(theta) < MAX_LOCK_ANGLE; - -// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){ -// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); -// } - - - controller.setPIDF(p, i, d, f); - int armPos = robot.turret.getCurrentPosition(); - double pid = controller.calculate(armPos, target); - - - if (withinAngle) { - - int desiredTarget = (int)(TICKS_PER_DEGREE * Math.toDegrees(theta)); - - // Clamp to turret mechanical limits - desiredTarget = Math.max( - TURRET_MIN_TICKS, - Math.min(TURRET_MAX_TICKS, desiredTarget) - ); - - target = desiredTarget; - - controller.setPIDF(p, i, d, f); - pid = controller.calculate(armPos, target); - - robot.turret.setPower(pid); - - } else { - // Outside lock range → stop or hold - robot.turret.setPower(0); - } - - telemetry.addData("armPos", armPos); - telemetry.addData("target", target); - telemetry.addData("pid", pid); - telemetry.update(); - - driveRIGGED.update(); - - if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { - auto = false; - robot.intake.set(1); - robot.activeTransfer.setPosition(0.85); - } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { - auto = false; - robot.intake.set(-1); - } else { - if (!auto) { - robot.intake.set(0); - } - } - - if(gamepad2.dpad_down) { - robot.topFly.set(0); - robot.bottomFly.set(0); - } - - - if (gamepad2.dpad_up) { - if(!auto) { - auto = true; - driveRIGGED.followTrajectorySequenceAsync(shootSeq); - } - } - - if (gamepad2.dpad_right) { - driveRIGGED.followTrajectorySequenceAsync(runUp); - } - - telemetry.addData("Shooting", shooting[0]); - telemetry.addData("RR Busy", drive.isBusy()); - - // Update the localizer - drive.getLocalizer().update(); - // Print pose to telemetry - telemetry.addData("totalHeading", totalHeading); - telemetry.addData("theta", theta); - telemetry.addData("x", poseEstimate.getX()); - telemetry.addData("y", poseEstimate.getY()); - telemetry.addData("heading", poseEstimate.getHeading()); - telemetry.addData("auton", auto); - telemetry.update(); - - } - - - - while (!isStopRequested() && opModeIsActive()); - } - - private static void stopAll(Hware robot) { - robot.intake.set(0); - robot.topFly.set(0); - robot.bottomFly.set(0); - robot.activeTransfer.setPosition(0.85); - } - - private static void cancelTrajectorySequenceCompat(Object drive) { - if (invokeIfExists(drive, "cancelTrajectorySequence")) return; - if (invokeIfExists(drive, "cancelFollowing")) return; - if (invokeIfExists(drive, "breakFollowing")) return; - try { - Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class); - m.invoke(drive, new Pose2d(0, 0, 0)); - } catch (Exception ignored) {} - } - - private static boolean invokeIfExists(Object obj, String methodName) { - try { - Method m = obj.getClass().getMethod(methodName); - m.invoke(obj); - return true; - } catch (Exception e) { - return false; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java new file mode 100644 index 0000000..53c51e1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java @@ -0,0 +1,143 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +@TeleOp(name = "TeleOp TurretLock (Odom + AprilTag)", group = "TeleOp") +@Config +public class TeleOpTurretLock extends LinearOpMode { + + /* ------------ CONFIG (Dashboard editable) ------------ */ + + // Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system. + // You said your start is (0,0). So measure these from that same origin. + public static double GOAL_X = 64.0; + public static double GOAL_Y = 75.0; + + // Set to a specific tag ID if you want (ex: 5). Use -1 to lock to any visible tag. + public static int TARGET_TAG_ID = -1; + + // Manual trim speed: degrees added per loop from triggers + public static double MANUAL_TRIM_DEG_PER_LOOP = 0.8; + + // Basic drive scaling + public static double DRIVE_SPEED = 1.0; + public static double SLOW_SPEED = 0.45; + + // If your webcam name in RC config is different, change this + public static String WEBCAM_NAME = "Webcam 1"; + + /* ----------------------------------------------------- */ + + private VisionPortal visionPortal; + private AprilTagProcessor aprilTag; + + @Override + public void runOpMode() { + + // Hardware + Hware robot = new Hware(); + robot.initialize(hardwareMap); + + // Drive + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + // Start pose (you said 0,0) + drive.setPoseEstimate(new Pose2d(0, 0, 0)); + + // AprilTag + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, WEBCAM_NAME)) + .addProcessor(aprilTag) + .build(); + + // TurretLock subsystem + TurretLock turretLock = new TurretLock(robot.turret); + turretLock.goalX = GOAL_X; + turretLock.goalY = GOAL_Y; + turretLock.targetTagId = TARGET_TAG_ID; + + // Optional: if you want these controlled from dashboard too, you can, + // but you already have them inside TurretLock. + // turretLock.ticksPerDegree = ... + // turretLock.minTicks = ... + // turretLock.maxTicks = ... + + boolean autoLockEnabled = true; + boolean xPrev = false; + + waitForStart(); + + while (opModeIsActive()) { + + /* ---------------- DRIVE ---------------- */ + double speed = DRIVE_SPEED; + if (gamepad1.left_bumper || gamepad1.right_bumper) speed = SLOW_SPEED; + + Pose2d drivePower = new Pose2d( + -gamepad1.left_stick_y * speed, + -gamepad1.left_stick_x * speed, + -gamepad1.right_stick_x * speed + ); + + drive.setWeightedDrivePower(drivePower); + drive.update(); + + Pose2d pose = drive.getPoseEstimate(); + + /* ---------------- TOGGLE AUTOLOCK ---------------- */ + boolean xNow = gamepad2.x; + if (xNow && !xPrev) autoLockEnabled = !autoLockEnabled; + xPrev = xNow; + + /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */ + turretLock.goalX = GOAL_X; + turretLock.goalY = GOAL_Y; + turretLock.targetTagId = TARGET_TAG_ID; + + /* ---------------- MANUAL TRIM ---------------- */ + // Left trigger = +trim, Right trigger = -trim (same style as your old code) + double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger); + if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg); + + if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim + if (gamepad2.b) turretLock.resetVisionTrim(); // reset vision trim + + /* ---------------- AUTOLOCK UPDATE ---------------- */ + List detections = aprilTag.getDetections(); + + if (autoLockEnabled) { + turretLock.update(pose, detections); + } else { + // If autolock is off, just stop turret motor where it is + // (or you can put your own manual turret code here) + robot.turret.setPower(0); + } + + /* ---------------- TELEMETRY ---------------- */ + telemetry.addData("AutoLock", autoLockEnabled); + telemetry.addData("Pose", "x=%.1f y=%.1f h=%.1fdeg", + pose.getX(), pose.getY(), Math.toDegrees(pose.getHeading())); + telemetry.addData("Goal", "x=%.1f y=%.1f", GOAL_X, GOAL_Y); + telemetry.addData("TargetTag", TARGET_TAG_ID); + telemetry.addData("TagCount", (detections == null) ? 0 : detections.size()); + telemetry.addData("TurretTicks", robot.turret.getCurrentPosition()); + telemetry.addLine("Controls: GP2 X=toggle, triggers=trim, Y=reset trim, B=reset vision"); + telemetry.update(); + } + + if (visionPortal != null) visionPortal.close(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java new file mode 100644 index 0000000..0aee7c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode; + +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 org.firstinspires.ftc.teamcode.OUTDATED.Hware; + +@TeleOp(name = "ENCODER TEST - Flywheel", group = "Test") +public class Test extends LinearOpMode { + + @Override + public void runOpMode() { + + Hware robot = new Hware(); + robot.initialize(hardwareMap); + + DcMotorEx top = (DcMotorEx) robot.topFly; + DcMotorEx bottom = (DcMotorEx) robot.bottomFly; + + // Reset encoders + top.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + bottom.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + top.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + bottom.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + telemetry.addLine("A = Spin Motors"); + telemetry.addLine("Watch Position + Velocity"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + + if (gamepad1.a) { + top.setPower(0.7); + bottom.setPower(0.7); + } else { + top.setPower(0); + bottom.setPower(0); + } + + telemetry.addLine("=== TOP MOTOR ==="); + telemetry.addData("Position", top.getCurrentPosition()); + telemetry.addData("Velocity", top.getVelocity()); + + telemetry.addLine(""); + + telemetry.addLine("=== BOTTOM MOTOR ==="); + telemetry.addData("Position", bottom.getCurrentPosition()); + telemetry.addData("Velocity", bottom.getVelocity()); + + telemetry.update(); + + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java new file mode 100644 index 0000000..c6908e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java @@ -0,0 +1,186 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.List; + +public class TurretLock { + + /* ------------------- TUNABLES ------------------- */ + + // Field target (in RoadRunner field units; usually inches) + // Set this to the AprilTag/backdrop location you want to aim at. + public double goalX = 64.0; + public double goalY = 75.0; + + // Turret conversion + // If you already trust your TICKS_PER_DEGREE, keep it. + public double ticksPerDegree = 9.738888; + + // Mechanical limits (ticks). Keep your existing numbers. + public int minTicks = (int) (ticksPerDegree * (-90)); + public int maxTicks = (int) (ticksPerDegree * ( 90)); + + // Which AprilTag to lock to (set -1 to accept any visible tag) + public int targetTagId = -1; + + // Vision correction: degrees of turret change per degree of tag "bearing"/yaw error. + // Start small (0.6–1.2) and tune. + public double visionDegPerDegError = 0.9; + + // Deadband so it doesn’t twitch + public double visionDeadbandDeg = 0.6; + + // How hard we trust vision vs odom. + // 0 = ignore vision, 1 = full correction. + public double visionBlend = 1.0; + + // Smooth the vision correction so it doesn’t jump + // 0.0 = no smoothing, 0.8 = heavy smoothing + public double visionLowPass = 0.65; + + // If we haven't seen a tag in this many seconds, slowly decay vision trim back to 0 + public double trimDecayPerSec = 12.0; // deg/sec back toward 0 + + // Optional: flip direction if your turret sign is backwards + public boolean invertTurret = false; + + /* ------------------- STATE ------------------- */ + + private final DcMotorEx turret; + private double manualTrimDeg = 0.0; // driver trim (triggers) + private double visionTrimDeg = 0.0; // learned trim from camera + private double filteredVisionTrimDeg = 0.0; + + private final ElapsedTime tagTimer = new ElapsedTime(); + + public TurretLock(DcMotorEx turretMotor) { + this.turret = turretMotor; + tagTimer.reset(); + } + + /** Call this if you want driver trim like your triggers (degrees). */ + public void addManualTrimDeg(double deltaDeg) { + manualTrimDeg += deltaDeg; + } + + public void resetManualTrim() { + manualTrimDeg = 0.0; + } + + public void resetVisionTrim() { + visionTrimDeg = 0.0; + filteredVisionTrimDeg = 0.0; + } + + /** Main update: call every loop. */ + public void update(Pose2d robotPose, List detections) { + // 1) ODOM: compute turret angle needed to face goal (in degrees) + double dx = goalX - robotPose.getX(); + double dy = goalY - robotPose.getY(); + + // angle to goal in field frame + double fieldToGoalDeg = Math.toDegrees(Math.atan2(dy, dx)); + + // robot heading in field frame + double robotHeadingDeg = Math.toDegrees(robotPose.getHeading()); + + // turret angle relative to robot (so turret points at goal) + double turretCmdDeg = fieldToGoalDeg - robotHeadingDeg; + + // normalize to [-180, 180] + turretCmdDeg = normalizeDeg(turretCmdDeg); + + if (invertTurret) turretCmdDeg = -turretCmdDeg; + + // 2) VISION: find tag error and update vision trim + Double tagErrorDeg = getTagErrorDeg(detections); + if (tagErrorDeg != null) { + // deadband + if (Math.abs(tagErrorDeg) < visionDeadbandDeg) tagErrorDeg = 0.0; + + // Convert tag error into turret trim change. + // Sign: if tag is to the right (+error), turret should move right or left? + // If it goes the wrong way, flip the sign here (multiply by -1). + double deltaTrim = (tagErrorDeg * visionDegPerDegError) * visionBlend; + + // We want trim to counteract the observed error, so subtract. + visionTrimDeg -= deltaTrim; + + tagTimer.reset(); + } else { + // no tag: decay vision trim toward 0 over time + double dt = 0.02; // assume ~50Hz; fine for a simple decay + double decay = trimDecayPerSec * dt; + if (visionTrimDeg > 0) visionTrimDeg = Math.max(0, visionTrimDeg - decay); + else if (visionTrimDeg < 0) visionTrimDeg = Math.min(0, visionTrimDeg + decay); + } + + // Low-pass filter the trim so it doesn’t jerk + filteredVisionTrimDeg = filteredVisionTrimDeg * visionLowPass + visionTrimDeg * (1.0 - visionLowPass); + + // 3) Combine + double finalDeg = turretCmdDeg + manualTrimDeg + filteredVisionTrimDeg; + + // Optional: keep within your allowed lock range (like ±90) + finalDeg = clamp(finalDeg, -90, 90); + + // 4) Convert degrees -> ticks and command motor + int targetTicks = (int) Math.round(finalDeg * ticksPerDegree); + targetTicks = clamp(targetTicks, minTicks, maxTicks); + + turret.setTargetPosition(targetTicks); + turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); + turret.setPower(1.0); + } + + /** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */ + private Double getTagErrorDeg(List detections) { + if (detections == null || detections.isEmpty()) return null; + + AprilTagDetection best = null; + + for (AprilTagDetection d : detections) { + if (d == null || d.metadata == null) continue; // metadata can be null on some setups + if (targetTagId != -1 && d.id != targetTagId) continue; + + // pick the first good one (or you can pick closest by range if you want) + best = d; + break; + } + + if (best == null) return null; + + // FTC AprilTagDetection typically gives: + // best.ftcPose.yaw (degrees) // yaw ~ left-right rotation of tag relative to camera + // best.ftcPose.bearing (degrees) on some versions + // + // We'll prefer "bearing" if present; otherwise use yaw. + try { + // If your SDK has bearing: + // return best.ftcPose.bearing; + // Otherwise: + return best.ftcPose.yaw; + } catch (Exception e) { + return null; + } + } + + private static double normalizeDeg(double deg) { + while (deg > 180) deg -= 360; + while (deg < -180) deg += 360; + return deg; + } + + private static double clamp(double v, double lo, double hi) { + return Math.max(lo, Math.min(hi, v)); + } + + private static int clamp(int v, int lo, int hi) { + return Math.max(lo, Math.min(hi, v)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java index 80dd0e2..9ed3f0d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java @@ -44,7 +44,7 @@ public class DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - public static double WHEEL_RADIUS = 1.8898; // in + public static double WHEEL_RADIUS = 2.047; // in public static double GEAR_RATIO = (24 / 22); // output (wheel) speed / input (motor) speed public static double TRACK_WIDTH = 12.3; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java index 125af1f..8c50eae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java @@ -1,5 +1,17 @@ package org.firstinspires.ftc.teamcode.drive; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + import androidx.annotation.NonNull; import com.acmerobotics.dashboard.config.Config; @@ -18,7 +30,6 @@ import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationCon import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -28,7 +39,6 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner; @@ -38,25 +48,13 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; - /* * Simple mecanum drive hardware implementation for REV hardware. */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 1); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0.5); public static double LATERAL_MULTIPLIER = 1; @@ -293,12 +291,12 @@ public class SampleMecanumDrive extends MecanumDrive { @Override public double getRawExternalHeading() { - return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + return 0; } @Override public Double getExternalHeadingVelocity() { - return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; + return 0.0; } public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java index 55aeb3a..9fa57b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java @@ -1,5 +1,17 @@ package org.firstinspires.ftc.teamcode.drive; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + import androidx.annotation.NonNull; import com.acmerobotics.dashboard.config.Config; @@ -37,18 +49,6 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; - /* * Simple tank drive hardware implementation for REV hardware. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java index 261898d..2fd11f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java @@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; + import org.firstinspires.ftc.teamcode.util.Encoder; import java.util.Arrays; @@ -28,11 +29,11 @@ import java.util.List; @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { public static double TICKS_PER_REV = 2000; - public static double WHEEL_RADIUS = 0.944882; // in + public static double WHEEL_RADIUS = 0.6299; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 6.2; // in; distance between the left and right wheels - public static double FORWARD_OFFSET = -6; // in; offset of the lateral wheel + public static double LATERAL_DISTANCE = 6; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = -6.45; // in; offset of the lateral wheel private Encoder leftEncoder, rightEncoder, frontEncoder; @@ -52,9 +53,9 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer lastEncPositions = lastTrackingEncPositions; lastEncVels = lastTrackingEncVels; - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake")); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "topFly")); - frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "bottomFly")); + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake")); + frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) // leftEncoder.setDirection(Encoder.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java index 8411792..46aebd6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java @@ -7,15 +7,28 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -/** - * This is a simple teleop routine for testing localization. Drive the robot around like a normal - * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight - * errors are not out of the ordinary, especially with sudden drive motions). The goal of this - * exercise is to ascertain whether the localizer has been configured properly (note: the pure - * encoder localizer heading may be significantly off if the track width has not been tuned). - */ @TeleOp(group = "drive") public class LocalizationTest extends LinearOpMode { + + private static final double TILE = 24.0; // inches + private static final double HALF = 72.0; // 144/2 + + private static int clamp(int v, int lo, int hi) { + return Math.max(lo, Math.min(hi, v)); + } + + // Col 1..6 left->right, nearest tile + private static int colFromX(double x) { + int col = (int) Math.round((x + HALF) / TILE) + 1; + return clamp(col, 1, 6); + } + + // Row 1..6 top->bottom, nearest tile + private static int rowFromY(double y) { + int row = (int) Math.round((HALF - y) / TILE) + 1; + return clamp(row, 1, 6); + } + @Override public void runOpMode() throws InterruptedException { SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); @@ -36,9 +49,18 @@ public class LocalizationTest extends LinearOpMode { drive.update(); Pose2d poseEstimate = drive.getPoseEstimate(); - telemetry.addData("x", poseEstimate.getX()); - telemetry.addData("y", poseEstimate.getY()); - telemetry.addData("heading", poseEstimate.getHeading()); + + int xInt = (int) Math.round(poseEstimate.getX()); + int yInt = (int) Math.round(poseEstimate.getY()); + int hDeg = (int) Math.round(Math.toDegrees(poseEstimate.getHeading())); + + int row = rowFromY(poseEstimate.getY()); + int col = colFromX(poseEstimate.getX()); + + telemetry.addData("x", xInt); + telemetry.addData("y", yInt); + telemetry.addData("heading (deg)", hDeg); + telemetry.addData("tile", "R" + row + " C" + col); telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java index 0d01bcb..a3510f2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java @@ -10,7 +10,6 @@ import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.drive.DriveSignal; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.kinematics.Kinematics; import com.acmerobotics.roadrunner.profile.MotionProfile; @@ -23,7 +22,6 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import java.util.Objects; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/flywheelSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/flywheelSystem.java new file mode 100644 index 0000000..772dd5f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/flywheelSystem.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode; + +import com.arcrobotics.ftclib.hardware.motors.Motor; + +/** + * Flywheel subsystem for FTC using FTCLib Motor + * - Velocity control for consistent RPM + * - RPM is estimated from encoder ticks over time + */ +public class flywheelSystem { + + private Motor bottomFly, topFly; + private static final double TICKS_PER_REV = 28; // goBILDA bare motor + + // Variables for RPM calculation + private int lastBottomTicks = 0; + private int lastTopTicks = 0; + private long lastTime = 0; // in ms + + /** + * Constructor + * @param bottom bottom flywheel motor from Hware + * @param top top flywheel motor from Hware + */ + public flywheelSystem(Motor bottom, Motor top) { + bottomFly = bottom; + topFly = top; + + // Set velocity control mode + bottomFly.setRunMode(Motor.RunMode.VelocityControl); + topFly.setRunMode(Motor.RunMode.VelocityControl); + + // Float to stabilize flywheel + bottomFly.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT); + topFly.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT); + + bottomFly.setInverted(true); // flip if needed + + // Initialize encoder tracking + lastBottomTicks = bottomFly.getCurrentPosition(); + lastTopTicks = topFly.getCurrentPosition(); + lastTime = System.currentTimeMillis(); + } + + /** Set target RPM for both flywheels */ + public void setRPM(double rpm) { + double ticksPerSecond = rpm * TICKS_PER_REV / 60.0; + bottomFly.set(ticksPerSecond); + topFly.set(ticksPerSecond); + } + + /** Stop both flywheels */ + public void stop() { + bottomFly.stopMotor(); + topFly.stopMotor(); + } + + /** + * Get current average RPM + * Calculates RPM from encoder ticks since last call + */ + public double getRPM() { + long currentTime = System.currentTimeMillis(); + int currentBottomTicks = bottomFly.getCurrentPosition(); + int currentTopTicks = topFly.getCurrentPosition(); + + double deltaTime = (currentTime - lastTime) / 1000.0; // seconds + if (deltaTime <= 0) deltaTime = 0.001; // prevent divide by zero + + int deltaBottom = currentBottomTicks - lastBottomTicks; + int deltaTop = currentTopTicks - lastTopTicks; + + lastBottomTicks = currentBottomTicks; + lastTopTicks = currentTopTicks; + lastTime = currentTime; + + double avgTPS = ((deltaBottom + deltaTop) / 2.0) / deltaTime; // ticks/sec + return avgTPS * 60.0 / TICKS_PER_REV; // convert to RPM + } + + /** + * Check if flywheels are within tolerance of target RPM + */ + public boolean atTargetRPM(double targetRPM, double tolerance) { + double currentRPM = getRPM(); + return Math.abs(currentRPM - targetRPM) <= tolerance; + } +}