From 75b9b7b6b1bb986cf5b5767fbf2ab8490767af80 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 2 Jun 2026 17:04:28 -0500 Subject: [PATCH 01/10] middle of tuning --- .../ftc/teamcode/pedroPathing/Constants.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 748524e..268c78c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -18,9 +18,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class Constants { public static FollowerConstants followerConstants = new FollowerConstants() .mass(14.37888) - .forwardZeroPowerAcceleration(-29.512) - .lateralZeroPowerAcceleration(-72.872) - .centripetalScaling(0); + .forwardZeroPowerAcceleration(-30.322) + .lateralZeroPowerAcceleration(-60.876) + .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02)) + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015)) + .centripetalScaling(0.0005); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) @@ -32,15 +35,15 @@ public class Constants { .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) - .xVelocity(64.675) - .yVelocity(49.583); + .xVelocity(84.376) + .yVelocity(64.052); public static double breakingStrength = 1; public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); public static PinpointConstants localizerConstants = new PinpointConstants() - .forwardPodY(-3.676) - .strafePodX(3.780) + .forwardPodY(3.7795) + .strafePodX(-3.676) .distanceUnit(DistanceUnit.INCH) .hardwareMapName("pinpoint") .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) From 946deca751c97b1da05ac4f0365513513bfe7566 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 2 Jun 2026 17:04:45 -0500 Subject: [PATCH 02/10] middle of tuning --- .../org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 55cc2fc..7d55931 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -416,7 +416,7 @@ class ForwardVelocityTuner extends OpMode { if (!end) { - if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE)) { end = true; stopRobot(); } else { @@ -524,7 +524,7 @@ class LateralVelocityTuner extends OpMode { drawCurrentAndHistory(); if (!end) { - if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE)) { end = true; stopRobot(); } else { From 5c9ebf6eac1d927f6d6ee139459ef82cdaa6f57e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 2 Jun 2026 18:22:28 -0500 Subject: [PATCH 03/10] some changes --- .../autonomous/Auto12BallPedroPathing.java | 90 +++++++++---------- .../ftc/teamcode/tests/Hardware_Tester.java | 3 +- 2 files changed, 47 insertions(+), 46 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index c10685d..1fa8480 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -33,17 +33,17 @@ import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.List; @Config -@Autonomous (preselectTeleOp = "TeleopV3") +@Autonomous (preselectTeleOp = "TeleopV4") public class Auto12BallPedroPathing extends LinearOpMode { Robot robot; MultipleTelemetry TELE; - Flywheel flywheel; - Targeting targeting; - Targeting.Settings targetingSettings; +// Flywheel flywheel; +// Targeting targeting; +// Targeting.Settings targetingSettings; Follower follower; - Turret turret; - Spindexer spindexer; - Servos servos; +// Turret turret; +// Spindexer spindexer; +// Servos servos; MeasuringLoopTimes loopTimes; // Wait Times @@ -222,10 +222,10 @@ public class Auto12BallPedroPathing extends LinearOpMode { driveShoot(PathState.WAIT_SHOOT3, currentTime); break; case WAIT_SHOOT3: - if (spindexer.shootAllComplete()){ - spindexer.resetSpindexer(); - TELE.addLine("Done Auto"); - } +// if (spindexer.shootAllComplete()){ +// spindexer.resetSpindexer(); +// TELE.addLine("Done Auto"); +// } break; default: break; @@ -237,21 +237,21 @@ public class Auto12BallPedroPathing extends LinearOpMode { private void intakePowerDown(double stamp, double currentTime) { double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust if (pow < -1) {pow = 0;} - spindexer.setIntakePower(pow); +// spindexer.setIntakePower(pow); } private void driveShoot(PathState nextState, double currentTime){ if (!follower.isBusy()){ pathState = nextState; timeStamp = currentTime; - spindexer.prepareShootAllContinous(); +// spindexer.prepareShootAllContinous(); } } private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { - if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime) { - spindexer.resetSpindexer(); + if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || +// spindexer.resetSpindexer(); pathState = nextState; follower.followPath(nextPath, true); - spindexer.setIntakePower(1); +// spindexer.setIntakePower(1); } } private void drivePickup(PathState nextState, PathChain nextPath) { @@ -292,18 +292,18 @@ public class Auto12BallPedroPathing extends LinearOpMode { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); } TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - flywheel = new Flywheel(hardwareMap); - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0,0); +// flywheel = new Flywheel(hardwareMap); +// targeting = new Targeting(); +// targetingSettings = new Targeting.Settings(0,0); follower = Constants.createFollower(hardwareMap); follower.setStartingPose(new Pose(72,72,0)); - turret = new Turret(robot, TELE, robot.limelight); - spindexer = new Spindexer(hardwareMap); - servos = new Servos(hardwareMap); +// turret = new Turret(robot, TELE, robot.limelight); +// spindexer = new Spindexer(hardwareMap); +// servos = new Servos(hardwareMap); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); - robot.light.setPosition(Color.LightRed); +// robot.light.setPosition(Color.LightRed); boolean initializeRobot = false; while (opModeInInit()){ @@ -311,11 +311,11 @@ public class Auto12BallPedroPathing extends LinearOpMode { if (gamepad1.crossWasPressed() && !initializeRobot){ Color.redAlliance = !Color.redAlliance; - if (Color.redAlliance){ - robot.light.setPosition(Color.LightRed); - } else { - robot.light.setPosition(Color.LightBlue); - } +// if (Color.redAlliance){ +// robot.light.setPosition(Color.LightRed); +// } else { +// robot.light.setPosition(Color.LightBlue); +// } double[] xPoses = {startPoseX, shoot0X, drivePickup1X, pickup1X, shoot1X, @@ -338,8 +338,8 @@ public class Auto12BallPedroPathing extends LinearOpMode { buildPaths(); sleep(2000); - turret.setTurret(turrDefault); - servos.setSpinPos(spinStartPos); +// turret.setTurret(turrDefault); +// servos.setSpinPos(spinStartPos); } TELE.addData("Red Alliance?", Color.redAlliance); @@ -352,27 +352,27 @@ public class Auto12BallPedroPathing extends LinearOpMode { if (isStopRequested()) return; - robot.transfer.setPower(1); +// robot.transfer.setPower(1); limelightUsed = false; while (opModeIsActive()){ follower.update(); pathStateMachine(); Pose currentPose = follower.getPose(); - teleStartPoseX = currentPose.getX(); - teleStartPoseY = currentPose.getY(); - teleStartPoseH = Math.toDegrees(currentPose.getHeading()); - - turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); - targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - servos.setHoodPos(targetingSettings.hoodAngle); - - if (driveToShoot()){servos.setSpinPos(spinStartPos);} - else {spindexer.processIntake();} +// teleStartPoseX = currentPose.getX(); +// teleStartPoseY = currentPose.getY(); +// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); +// +// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); +// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); +// +// double voltage = robot.voltage.getVoltage(); +// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); +// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +// servos.setHoodPos(targetingSettings.hoodAngle); +// +// if (driveToShoot()){servos.setSpinPos(spinStartPos);} +// else {spindexer.processIntake();} for (LynxModule hub : allHubs) { hub.clearBulkCache(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index 0dc08ab..bfaba30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.tests; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -51,7 +52,7 @@ public class Hardware_Tester extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - TELE = new MultipleTelemetry(); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); From 8eba32de94da6a2b9c949517ee9bb8f0999ad76c Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 2 Jun 2026 18:22:41 -0500 Subject: [PATCH 04/10] new auto in progress --- .../autonomous/Auto12Ball_Back_Sorted.java | 340 ++++++++++++++++++ 1 file changed, 340 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java new file mode 100644 index 0000000..b9ad6c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -0,0 +1,340 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; +import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; + +import java.util.List; + +@Config +@Autonomous (preselectTeleOp = "TeleopV4") +public class Auto12Ball_Back_Sorted extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + // Flywheel flywheel; +// Targeting targeting; +// Targeting.Settings targetingSettings; + Follower follower; + // Turret turret; +// Spindexer spindexer; +// Servos servos; + MeasuringLoopTimes loopTimes; + + // Wait Times + public static double shootTime = 2; + + // Extra Variables + public static double intakePower = 0.3; + double shootX, shootY, shootH; + + // Initialize path state machine + private enum PathState { + PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, + PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, + DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, + DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 + } + PathState pathState = PathState.DRIVE_SHOOT0; + + // Poses + public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; + public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; + public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; + public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; + public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; + public static double openGateX = 129, openGateY = 74, openGateH = 0; + public static double shoot1ControlX = 112, shoot1ControlY = 75; + public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; + public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; + public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; + public static double shoot2ControlX = 102, shoot2ControlY = 63; + public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; + public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; + public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; + public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; + public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; + Pose startPose, pushBot, shoot0, + pickup1, openGateControl, openGate, shoot1Control, shoot1, + drivePickup2, pickup2, shoot2Control, shoot2, + drivePickup3, pickup3, shoot3Control, shoot3; + private void initializePoses(){ + startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); + pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); + shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); + pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); + openGateControl = new Pose(openGateControlX, openGateControlY); + openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); + shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); + shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); + drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); + pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); + shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); + shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); + drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); + pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); + shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); + shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); + } + + //Building Paths + PathChain startPose_pushBot, pushBot_shoot0, + shoot0_pickup1, pickup1_openGate, openGate_shoot1, + shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, + shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; + private void buildPaths(){ + startPose_shoot0 = follower.pathBuilder() + .addPath(new BezierLine(startPose, shoot0)) + .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading()) + .build(); + } + + //Path State Machine + private boolean startAuto = true; + private double timeStamp = 0; + private void pathStateMachine(){ + double currentTime = (double) System.currentTimeMillis() / 1000; + switch(pathState){ + case DRIVE_SHOOT0: + if (startAuto){ + follower.followPath(startPose_shoot0, true); + startAuto = false; + shootX = shoot0X; + shootY = shoot0Y; + shootH = shoot0H; + } + driveShoot(PathState.WAIT_SHOOT0, currentTime); + break; + case WAIT_SHOOT0: + waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime); + break; + case DRIVE_PICKUP1: + drivePickup(PathState.PICKUP1, drivePickup1_pickup1); + break; + case PICKUP1: + pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1); + shootX = shoot1X; + shootY = shoot1Y; + shootH = shoot1H; + break; + case DRIVE_SHOOT1: + intakePowerDown(timeStamp, currentTime); + driveShoot(PathState.WAIT_SHOOT1, currentTime); + break; + case WAIT_SHOOT1: + waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime); + break; + case DRIVE_PICKUP2: + drivePickup(PathState.PICKUP2, drivePickup2_pickup2); + break; + case PICKUP2: + pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2); + shootX = shoot2X; + shootY = shoot2Y; + shootH = shoot2H; + break; + case DRIVE_SHOOT2: + intakePowerDown(timeStamp, currentTime); + driveShoot(PathState.WAIT_SHOOT2, currentTime); + break; + case WAIT_SHOOT2: + waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime); + break; + case DRIVE_PICKUP3: + drivePickup(PathState.PICKUP3, drivePickup3_pickup3); + break; + case PICKUP3: + pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3); + shootX = shoot3X; + shootY = shoot3Y; + shootH = shoot3H; + break; + case DRIVE_SHOOT3: + intakePowerDown(timeStamp, currentTime); + driveShoot(PathState.WAIT_SHOOT3, currentTime); + break; + case WAIT_SHOOT3: +// if (spindexer.shootAllComplete()){ +// spindexer.resetSpindexer(); +// TELE.addLine("Done Auto"); +// } + break; + default: + break; + } + TELE.update(); // use for debugging + } + + // Voids for State Machine + private void intakePowerDown(double stamp, double currentTime) { + double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust + if (pow < -1) {pow = 0;} +// spindexer.setIntakePower(pow); + } + private void driveShoot(PathState nextState, double currentTime){ + if (!follower.isBusy()){ + pathState = nextState; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } + } + private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { + if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || +// spindexer.resetSpindexer(); + pathState = nextState; + follower.followPath(nextPath, true); +// spindexer.setIntakePower(1); + } + } + private void drivePickup(PathState nextState, PathChain nextPath) { + if (!follower.isBusy()) { + pathState = nextState; + follower.followPath(nextPath, intakePower, false); + } + } + private void pickup(PathState nextState, PathChain nextPath) { + if (!follower.isBusy()) { + pathState = nextState; + follower.followPath(nextPath, true); + } + } + + // Used for changing alliance + private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} + private double adjustHeadingBasedOnAlliance(double heading){ + heading = 180 - heading; + while (heading > 180) {heading-=360;} + while (heading <= -180) {heading+=360;} + return heading; + } + + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + List allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); +// flywheel = new Flywheel(hardwareMap); +// targeting = new Targeting(); +// targetingSettings = new Targeting.Settings(0,0); + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(72,72,0)); +// turret = new Turret(robot, TELE, robot.limelight); +// spindexer = new Spindexer(hardwareMap); +// servos = new Servos(hardwareMap); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + +// robot.light.setPosition(Color.LightRed); + + boolean initializeRobot = false; + while (opModeInInit()){ + follower.update(); + + if (gamepad1.crossWasPressed() && !initializeRobot){ + Color.redAlliance = !Color.redAlliance; +// if (Color.redAlliance){ +// robot.light.setPosition(Color.LightRed); +// } else { +// robot.light.setPosition(Color.LightBlue); +// } + + double[] xPoses = {startPoseX, shoot0X, + drivePickup1X, pickup1X, shoot1X, + drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, + drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; + + double[] headings = {startPoseH, shoot0H, + drivePickup1H, pickup1H, shoot1H, + drivePickup2H, pickup2H, shoot2H, + drivePickup3H, pickup3H, shoot3H}; + + for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} + for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} + } + + if (gamepad1.triangleWasPressed()){ + initializeRobot = true; + initializePoses(); + follower.setPose(startPose); + buildPaths(); + sleep(2000); + +// turret.setTurret(turrDefault); +// servos.setSpinPos(spinStartPos); + } + + TELE.addData("Red Alliance?", Color.redAlliance); + TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); + TELE.addData("Start Pose", follower.getPose()); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + +// robot.transfer.setPower(1); + limelightUsed = false; + + while (opModeIsActive()){ + follower.update(); + pathStateMachine(); + Pose currentPose = follower.getPose(); +// teleStartPoseX = currentPose.getX(); +// teleStartPoseY = currentPose.getY(); +// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); +// +// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); +// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); +// +// double voltage = robot.voltage.getVoltage(); +// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); +// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +// servos.setHoodPos(targetingSettings.hoodAngle); +// +// if (driveToShoot()){servos.setSpinPos(spinStartPos);} +// else {spindexer.processIntake();} + + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); + } + loopTimes.loop(); + + TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); + TELE.addData("X:", currentPose.getX()); + TELE.addData("Y:", currentPose.getY()); + TELE.addData("H:", currentPose.getHeading()); + TELE.update(); + } + } +} \ No newline at end of file From 7665957c7a27b26b3b057e7ed7fc54cb808ec7de Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 2 Jun 2026 19:14:58 -0500 Subject: [PATCH 05/10] readjusted shooter test @KeshavAnandCode please merge --- .../autonomous/Auto12BallPedroPathing.java | 9 +- .../autonomous/Auto12Ball_Back_Sorted.java | 673 +++++++++--------- .../ftc/teamcode/tests/Hardware_Tester.java | 5 +- .../ftc/teamcode/tests/NewShooterTest.java | 23 +- .../ftc/teamcode/utilsv2/Flywheel.java | 31 +- .../ftc/teamcode/utilsv2/Robot.java | 6 +- 6 files changed, 376 insertions(+), 371 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index 1fa8480..48a3ba8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -22,13 +22,13 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; +import org.firstinspires.ftc.teamcode.utilsv2.Turret; import java.util.List; @@ -286,7 +286,8 @@ public class Auto12BallPedroPathing extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); List allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index b9ad6c6..3af8eae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -1,340 +1,341 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; -import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; -import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; -import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.BezierCurve; -import com.pedropathing.geometry.BezierLine; -import com.pedropathing.geometry.Pose; -import com.pedropathing.paths.PathChain; -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.constants.Color; -import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.teamcode.utils.Spindexer; -import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; - -import java.util.List; - -@Config -@Autonomous (preselectTeleOp = "TeleopV4") -public class Auto12Ball_Back_Sorted extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - // Flywheel flywheel; -// Targeting targeting; -// Targeting.Settings targetingSettings; - Follower follower; - // Turret turret; -// Spindexer spindexer; -// Servos servos; - MeasuringLoopTimes loopTimes; - - // Wait Times - public static double shootTime = 2; - - // Extra Variables - public static double intakePower = 0.3; - double shootX, shootY, shootH; - - // Initialize path state machine - private enum PathState { - PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, - PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, - DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, - DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 - } - PathState pathState = PathState.DRIVE_SHOOT0; - - // Poses - public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; - public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; - public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; - public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; - public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; - public static double openGateX = 129, openGateY = 74, openGateH = 0; - public static double shoot1ControlX = 112, shoot1ControlY = 75; - public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; - public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; - public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; - public static double shoot2ControlX = 102, shoot2ControlY = 63; - public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; - public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; - public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; - public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; - public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; - Pose startPose, pushBot, shoot0, - pickup1, openGateControl, openGate, shoot1Control, shoot1, - drivePickup2, pickup2, shoot2Control, shoot2, - drivePickup3, pickup3, shoot3Control, shoot3; - private void initializePoses(){ - startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); - pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); - shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); - pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); - openGateControl = new Pose(openGateControlX, openGateControlY); - openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); - shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); - shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); - drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); - pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); - shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); - shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); - drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); - pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); - shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); - shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); - } - - //Building Paths - PathChain startPose_pushBot, pushBot_shoot0, - shoot0_pickup1, pickup1_openGate, openGate_shoot1, - shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, - shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; - private void buildPaths(){ - startPose_shoot0 = follower.pathBuilder() - .addPath(new BezierLine(startPose, shoot0)) - .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading()) - .build(); - } - - //Path State Machine - private boolean startAuto = true; - private double timeStamp = 0; - private void pathStateMachine(){ - double currentTime = (double) System.currentTimeMillis() / 1000; - switch(pathState){ - case DRIVE_SHOOT0: - if (startAuto){ - follower.followPath(startPose_shoot0, true); - startAuto = false; - shootX = shoot0X; - shootY = shoot0Y; - shootH = shoot0H; - } - driveShoot(PathState.WAIT_SHOOT0, currentTime); - break; - case WAIT_SHOOT0: - waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime); - break; - case DRIVE_PICKUP1: - drivePickup(PathState.PICKUP1, drivePickup1_pickup1); - break; - case PICKUP1: - pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1); - shootX = shoot1X; - shootY = shoot1Y; - shootH = shoot1H; - break; - case DRIVE_SHOOT1: - intakePowerDown(timeStamp, currentTime); - driveShoot(PathState.WAIT_SHOOT1, currentTime); - break; - case WAIT_SHOOT1: - waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime); - break; - case DRIVE_PICKUP2: - drivePickup(PathState.PICKUP2, drivePickup2_pickup2); - break; - case PICKUP2: - pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2); - shootX = shoot2X; - shootY = shoot2Y; - shootH = shoot2H; - break; - case DRIVE_SHOOT2: - intakePowerDown(timeStamp, currentTime); - driveShoot(PathState.WAIT_SHOOT2, currentTime); - break; - case WAIT_SHOOT2: - waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime); - break; - case DRIVE_PICKUP3: - drivePickup(PathState.PICKUP3, drivePickup3_pickup3); - break; - case PICKUP3: - pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3); - shootX = shoot3X; - shootY = shoot3Y; - shootH = shoot3H; - break; - case DRIVE_SHOOT3: - intakePowerDown(timeStamp, currentTime); - driveShoot(PathState.WAIT_SHOOT3, currentTime); - break; - case WAIT_SHOOT3: -// if (spindexer.shootAllComplete()){ -// spindexer.resetSpindexer(); -// TELE.addLine("Done Auto"); +//package org.firstinspires.ftc.teamcode.autonomous; +// +//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +//import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +//import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; +//import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; +//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +// +//import com.acmerobotics.dashboard.FtcDashboard; +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +//import com.pedropathing.follower.Follower; +//import com.pedropathing.geometry.BezierCurve; +//import com.pedropathing.geometry.BezierLine; +//import com.pedropathing.geometry.Pose; +//import com.pedropathing.paths.PathChain; +//import com.qualcomm.hardware.lynx.LynxModule; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +// +//import org.firstinspires.ftc.teamcode.constants.Color; +//import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +//import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; +//import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; +//import org.firstinspires.ftc.teamcode.utilsv2.Robot; +//import org.firstinspires.ftc.teamcode.utils.Servos; +//import org.firstinspires.ftc.teamcode.utils.Spindexer; +//import org.firstinspires.ftc.teamcode.utils.Targeting; +//import org.firstinspires.ftc.teamcode.utilsv2.Turret; +// +//import java.util.List; +// +//@Config +//@Autonomous (preselectTeleOp = "TeleopV4") +//public class Auto12Ball_Back_Sorted extends LinearOpMode { +// Robot robot; +// MultipleTelemetry TELE; +// // Flywheel flywheel; +//// Targeting targeting; +//// Targeting.Settings targetingSettings; +// Follower follower; +// // Turret turret; +//// Spindexer spindexer; +//// Servos servos; +// MeasuringLoopTimes loopTimes; +// +// // Wait Times +// public static double shootTime = 2; +// +// // Extra Variables +// public static double intakePower = 0.3; +// double shootX, shootY, shootH; +// +// // Initialize path state machine +// private enum PathState { +// PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, +// PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, +// DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, +// DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 +// } +// PathState pathState = PathState.DRIVE_SHOOT0; +// +// // Poses +// public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; +// public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; +// public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; +// public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; +// public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; +// public static double openGateX = 129, openGateY = 74, openGateH = 0; +// public static double shoot1ControlX = 112, shoot1ControlY = 75; +// public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; +// public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; +// public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; +// public static double shoot2ControlX = 102, shoot2ControlY = 63; +// public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; +// public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; +// public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; +// public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; +// public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; +// Pose startPose, pushBot, shoot0, +// pickup1, openGateControl, openGate, shoot1Control, shoot1, +// drivePickup2, pickup2, shoot2Control, shoot2, +// drivePickup3, pickup3, shoot3Control, shoot3; +// private void initializePoses(){ +// startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); +// pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); +// shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); +// pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); +// openGateControl = new Pose(openGateControlX, openGateControlY); +// openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); +// shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); +// shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); +// drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); +// pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); +// shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); +// shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); +// drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); +// pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); +// shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); +// shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); +// } +// +// //Building Paths +// PathChain startPose_pushBot, pushBot_shoot0, +// shoot0_pickup1, pickup1_openGate, openGate_shoot1, +// shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, +// shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; +// private void buildPaths(){ +// startPose_pushBot = follower.pathBuilder() +// .addPath(new BezierLine(startPose, pushBot)) +// .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading()) +// .build(); +// } +// +// //Path State Machine +// private boolean startAuto = true; +// private double timeStamp = 0; +// private void pathStateMachine(){ +// double currentTime = (double) System.currentTimeMillis() / 1000; +// switch(pathState){ +// case DRIVE_SHOOT0: +// if (startAuto){ +// follower.followPath(startPose_shoot0, true); +// startAuto = false; +// shootX = shoot0X; +// shootY = shoot0Y; +// shootH = shoot0H; // } - break; - default: - break; - } - TELE.update(); // use for debugging - } - - // Voids for State Machine - private void intakePowerDown(double stamp, double currentTime) { - double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust - if (pow < -1) {pow = 0;} -// spindexer.setIntakePower(pow); - } - private void driveShoot(PathState nextState, double currentTime){ - if (!follower.isBusy()){ - pathState = nextState; - timeStamp = currentTime; -// spindexer.prepareShootAllContinous(); - } - } - private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { - if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || -// spindexer.resetSpindexer(); - pathState = nextState; - follower.followPath(nextPath, true); -// spindexer.setIntakePower(1); - } - } - private void drivePickup(PathState nextState, PathChain nextPath) { - if (!follower.isBusy()) { - pathState = nextState; - follower.followPath(nextPath, intakePower, false); - } - } - private void pickup(PathState nextState, PathChain nextPath) { - if (!follower.isBusy()) { - pathState = nextState; - follower.followPath(nextPath, true); - } - } - - // Used for changing alliance - private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} - private double adjustHeadingBasedOnAlliance(double heading){ - heading = 180 - heading; - while (heading > 180) {heading-=360;} - while (heading <= -180) {heading+=360;} - return heading; - } - - @Override - public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); - List allHubs = hardwareMap.getAll(LynxModule.class); - for (LynxModule hub : allHubs) { - hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); - } - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); -// flywheel = new Flywheel(hardwareMap); -// targeting = new Targeting(); -// targetingSettings = new Targeting.Settings(0,0); - follower = Constants.createFollower(hardwareMap); - follower.setStartingPose(new Pose(72,72,0)); -// turret = new Turret(robot, TELE, robot.limelight); -// spindexer = new Spindexer(hardwareMap); -// servos = new Servos(hardwareMap); - loopTimes = new MeasuringLoopTimes(); - loopTimes.init(); - -// robot.light.setPosition(Color.LightRed); - - boolean initializeRobot = false; - while (opModeInInit()){ - follower.update(); - - if (gamepad1.crossWasPressed() && !initializeRobot){ - Color.redAlliance = !Color.redAlliance; -// if (Color.redAlliance){ -// robot.light.setPosition(Color.LightRed); -// } else { -// robot.light.setPosition(Color.LightBlue); -// } - - double[] xPoses = {startPoseX, shoot0X, - drivePickup1X, pickup1X, shoot1X, - drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, - drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; - - double[] headings = {startPoseH, shoot0H, - drivePickup1H, pickup1H, shoot1H, - drivePickup2H, pickup2H, shoot2H, - drivePickup3H, pickup3H, shoot3H}; - - for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} - for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} - } - - if (gamepad1.triangleWasPressed()){ - initializeRobot = true; - initializePoses(); - follower.setPose(startPose); - buildPaths(); - sleep(2000); - -// turret.setTurret(turrDefault); -// servos.setSpinPos(spinStartPos); - } - - TELE.addData("Red Alliance?", Color.redAlliance); - TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); - TELE.addData("Start Pose", follower.getPose()); - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - -// robot.transfer.setPower(1); - limelightUsed = false; - - while (opModeIsActive()){ - follower.update(); - pathStateMachine(); - Pose currentPose = follower.getPose(); -// teleStartPoseX = currentPose.getX(); -// teleStartPoseY = currentPose.getY(); -// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); +// driveShoot(PathState.WAIT_SHOOT0, currentTime); +// break; +// case WAIT_SHOOT0: +// waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime); +// break; +// case DRIVE_PICKUP1: +// drivePickup(PathState.PICKUP1, drivePickup1_pickup1); +// break; +// case PICKUP1: +// pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1); +// shootX = shoot1X; +// shootY = shoot1Y; +// shootH = shoot1H; +// break; +// case DRIVE_SHOOT1: +// intakePowerDown(timeStamp, currentTime); +// driveShoot(PathState.WAIT_SHOOT1, currentTime); +// break; +// case WAIT_SHOOT1: +// waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime); +// break; +// case DRIVE_PICKUP2: +// drivePickup(PathState.PICKUP2, drivePickup2_pickup2); +// break; +// case PICKUP2: +// pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2); +// shootX = shoot2X; +// shootY = shoot2Y; +// shootH = shoot2H; +// break; +// case DRIVE_SHOOT2: +// intakePowerDown(timeStamp, currentTime); +// driveShoot(PathState.WAIT_SHOOT2, currentTime); +// break; +// case WAIT_SHOOT2: +// waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime); +// break; +// case DRIVE_PICKUP3: +// drivePickup(PathState.PICKUP3, drivePickup3_pickup3); +// break; +// case PICKUP3: +// pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3); +// shootX = shoot3X; +// shootY = shoot3Y; +// shootH = shoot3H; +// break; +// case DRIVE_SHOOT3: +// intakePowerDown(timeStamp, currentTime); +// driveShoot(PathState.WAIT_SHOOT3, currentTime); +// break; +// case WAIT_SHOOT3: +//// if (spindexer.shootAllComplete()){ +//// spindexer.resetSpindexer(); +//// TELE.addLine("Done Auto"); +//// } +// break; +// default: +// break; +// } +// TELE.update(); // use for debugging +// } // -// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); -// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); +// // Voids for State Machine +// private void intakePowerDown(double stamp, double currentTime) { +// double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust +// if (pow < -1) {pow = 0;} +//// spindexer.setIntakePower(pow); +// } +// private void driveShoot(PathState nextState, double currentTime){ +// if (!follower.isBusy()){ +// pathState = nextState; +// timeStamp = currentTime; +//// spindexer.prepareShootAllContinous(); +// } +// } +// private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { +// if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || +//// spindexer.resetSpindexer(); +// pathState = nextState; +// follower.followPath(nextPath, true); +//// spindexer.setIntakePower(1); +// } +// } +// private void drivePickup(PathState nextState, PathChain nextPath) { +// if (!follower.isBusy()) { +// pathState = nextState; +// follower.followPath(nextPath, intakePower, false); +// } +// } +// private void pickup(PathState nextState, PathChain nextPath) { +// if (!follower.isBusy()) { +// pathState = nextState; +// follower.followPath(nextPath, true); +// } +// } // -// double voltage = robot.voltage.getVoltage(); -// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); -// flywheel.manageFlywheel(targetingSettings.flywheelRPM); -// servos.setHoodPos(targetingSettings.hoodAngle); +// // Used for changing alliance +// private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} +// private double adjustHeadingBasedOnAlliance(double heading){ +// heading = 180 - heading; +// while (heading > 180) {heading-=360;} +// while (heading <= -180) {heading+=360;} +// return heading; +// } // -// if (driveToShoot()){servos.setSpinPos(spinStartPos);} -// else {spindexer.processIntake();} - - for (LynxModule hub : allHubs) { - hub.clearBulkCache(); - } - loopTimes.loop(); - - TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); - TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); - TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("X:", currentPose.getX()); - TELE.addData("Y:", currentPose.getY()); - TELE.addData("H:", currentPose.getHeading()); - TELE.update(); - } - } -} \ No newline at end of file +// @Override +// public void runOpMode() throws InterruptedException { +// Robot.resetInstance(); +// robot = Robot.getInstance(hardwareMap); +// List allHubs = hardwareMap.getAll(LynxModule.class); +// for (LynxModule hub : allHubs) { +// hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); +// } +// TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); +//// flywheel = new Flywheel(hardwareMap); +//// targeting = new Targeting(); +//// targetingSettings = new Targeting.Settings(0,0); +// follower = Constants.createFollower(hardwareMap); +// follower.setStartingPose(new Pose(72,72,0)); +//// turret = new Turret(robot, TELE, robot.limelight); +//// spindexer = new Spindexer(hardwareMap); +//// servos = new Servos(hardwareMap); +// loopTimes = new MeasuringLoopTimes(); +// loopTimes.init(); +// +//// robot.light.setPosition(Color.LightRed); +// +// boolean initializeRobot = false; +// while (opModeInInit()){ +// follower.update(); +// +// if (gamepad1.crossWasPressed() && !initializeRobot){ +// Color.redAlliance = !Color.redAlliance; +//// if (Color.redAlliance){ +//// robot.light.setPosition(Color.LightRed); +//// } else { +//// robot.light.setPosition(Color.LightBlue); +//// } +// +//// double[] xPoses = {startPoseX, shoot0X, +//// drivePickup1X, pickup1X, shoot1X, +//// drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, +//// drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; +//// +//// double[] headings = {startPoseH, shoot0H, +//// drivePickup1H, pickup1H, shoot1H, +//// drivePickup2H, pickup2H, shoot2H, +//// drivePickup3H, pickup3H, shoot3H}; +// +//// for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} +//// for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} +// } +// +// if (gamepad1.triangleWasPressed()){ +// initializeRobot = true; +// initializePoses(); +// follower.setPose(startPose); +// buildPaths(); +// sleep(2000); +// +//// turret.setTurret(turrDefault); +//// servos.setSpinPos(spinStartPos); +// } +// +// TELE.addData("Red Alliance?", Color.redAlliance); +// TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); +// TELE.addData("Start Pose", follower.getPose()); +// TELE.update(); +// } +// +// waitForStart(); +// +// if (isStopRequested()) return; +// +//// robot.transfer.setPower(1); +// limelightUsed = false; +// +// while (opModeIsActive()){ +// follower.update(); +// pathStateMachine(); +// Pose currentPose = follower.getPose(); +//// teleStartPoseX = currentPose.getX(); +//// teleStartPoseY = currentPose.getY(); +//// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); +//// +//// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); +//// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); +//// +//// double voltage = robot.voltage.getVoltage(); +//// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); +//// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +//// servos.setHoodPos(targetingSettings.hoodAngle); +//// +//// if (driveToShoot()){servos.setSpinPos(spinStartPos);} +//// else {spindexer.processIntake();} +// +// for (LynxModule hub : allHubs) { +// hub.clearBulkCache(); +// } +// loopTimes.loop(); +// +// TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); +// TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); +// TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); +// TELE.addData("X:", currentPose.getX()); +// TELE.addData("Y:", currentPose.getY()); +// TELE.addData("H:", currentPose.getHeading()); +// TELE.update(); +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index bfaba30..2f95d38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; @Config @TeleOp @@ -51,7 +51,8 @@ public class Hardware_Tester extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index fafc88c..97d58f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -8,6 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; import org.firstinspires.ftc.teamcode.utilsv2.Robot; import org.firstinspires.ftc.teamcode.utilsv2.Shooter; @@ -17,6 +18,10 @@ public class NewShooterTest extends LinearOpMode { Robot robot; + MultipleTelemetry TELE; + + Flywheel rpmFlywheel; + public static boolean intake = true; public static boolean shoot = false; public static double intakePower = 1.0; @@ -26,6 +31,8 @@ public class NewShooterTest extends LinearOpMode { public static double hoodPos = 0.51; public static double flywheel = 0; + public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75; + private enum ShootState { IDLE, WAIT_GATE, @@ -38,10 +45,10 @@ public class NewShooterTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - Robot.resetInstance(); - robot = Robot.getInstance(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + Shooter shooter = new Shooter( robot, new MultipleTelemetry( @@ -52,6 +59,8 @@ public class NewShooterTest extends LinearOpMode { true ); + rpmFlywheel = new Flywheel(robot); + shooter.setState(Shooter.ShooterState.MANUAL); waitForStart(); @@ -63,6 +72,9 @@ public class NewShooterTest extends LinearOpMode { robot.setHoodPos(hoodPos); shooter.setTurretPosition(turretPos); shooter.setFlywheelVelocity(flywheel); + double voltage = robot.voltage.getVoltage(); + rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage); + robot.setSpinPos(ServoPositions.spindexer_A2); if (intake && !shoot) { @@ -76,9 +88,7 @@ public class NewShooterTest extends LinearOpMode { robot.setIntakePower(intakePower); robot.setTransferServoPos( ServoPositions.transferServo_out); - } - - else if (shoot) { + } else if (shoot) { robot.setIntakePower(intakePower); @@ -120,6 +130,9 @@ public class NewShooterTest extends LinearOpMode { } } + TELE.addData("Flywheel Velocity", (robot.shooter1.getVelocity() * 60) / 28); + TELE.update(); + shooter.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index a5cfada..030f018 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -1,12 +1,10 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.qualcomm.robotcore.hardware.DcMotor; + import com.qualcomm.robotcore.hardware.HardwareMap; - import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import org.firstinspires.ftc.teamcode.utilsv2.Robot; - import java.util.LinkedList; public class Flywheel { @@ -27,10 +25,8 @@ public class Flywheel { private final LinkedList velocityHistory = new LinkedList<>(); public Flywheel(Robot rob) { - robot = rob; - shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); - shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); + shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / 12); } public double getVelo() { @@ -65,19 +61,14 @@ public class Flywheel { shooterPIDF1.d = d; shooterPIDF1.f = f; - shooterPIDF2.p = p; - shooterPIDF2.i = i; - shooterPIDF2.d = d; - shooterPIDF2.f = f; + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); - if (Math.abs(prevF - f) > voltagePIDFDifference) { - - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); - - robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); - - prevF = f; - } +// if (Math.abs(prevF - f) > voltagePIDFDifference) { +// +// robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); +// +// prevF = f; +// } } // Convert from RPM to Ticks per Second @@ -115,8 +106,8 @@ public class Flywheel { } robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - - robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + double power = robot.shooter1.getPower(); + robot.shooter2.setPower(power); velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 224cdca..da7bed3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -106,13 +106,11 @@ public class Robot { shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter1.setDirection(DcMotorSimple.Direction.REVERSE); - shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); + shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); shooter1.setVelocity(0); - shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter2.setVelocity(0); + shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); hood = hardwareMap.get(Servo.class, "hood"); From 9c3b4c2010fe56d08c6a6adafd5ba5342209dc1f Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Wed, 3 Jun 2026 00:04:20 -0500 Subject: [PATCH 06/10] Add beam break sensors to Hardware_Tester --- .../org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java | 3 ++- .../java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java | 4 +++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index 2f95d38..f6ee1ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -124,7 +124,8 @@ public class Hardware_Tester extends LinearOpMode { // Sensor Data -// TELE.addData("Beam Break 1?", robot.beam1.isPressed()); + TELE.addData("Beam Break insideBeam?", robot.insideBeam.isPressed()); + TELE.addData("Beam Break outsideBeam?", robot.outsideBeam.isPressed()); // TELE.addData("Beam Break 2?", robot.beam2.isPressed()); // TELE.addData("Beam Break 3?", robot.beam3.isPressed()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index da7bed3..4a17e9c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import android.view.View; + import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; @@ -19,6 +21,7 @@ public class Robot { // Singleton instance private static Robot instance; + /** * Returns the existing Robot instance or creates one if it doesn't exist. */ @@ -65,7 +68,6 @@ public class Robot { public Servo spin2; public TouchSensor insideBeam; public TouchSensor outsideBeam; - public RevColorSensorV3 revSensor; public VoltageSensor voltage; From 28451ce26d3fcd585cefa344bdf65e100ca1c647 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 3 Jun 2026 00:21:51 -0500 Subject: [PATCH 07/10] auto coded --- .../autonomous/Auto12Ball_Back_Sorted.java | 707 +++++++++--------- 1 file changed, 366 insertions(+), 341 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index 3af8eae..599865d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -1,341 +1,366 @@ -//package org.firstinspires.ftc.teamcode.autonomous; -// -//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; -//import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; -//import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; -//import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; -//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; -//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; -//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; -// -//import com.acmerobotics.dashboard.FtcDashboard; -//import com.acmerobotics.dashboard.config.Config; -//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -//import com.pedropathing.follower.Follower; -//import com.pedropathing.geometry.BezierCurve; -//import com.pedropathing.geometry.BezierLine; -//import com.pedropathing.geometry.Pose; -//import com.pedropathing.paths.PathChain; -//import com.qualcomm.hardware.lynx.LynxModule; -//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -// -//import org.firstinspires.ftc.teamcode.constants.Color; -//import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -//import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; -//import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; -//import org.firstinspires.ftc.teamcode.utilsv2.Robot; -//import org.firstinspires.ftc.teamcode.utils.Servos; -//import org.firstinspires.ftc.teamcode.utils.Spindexer; -//import org.firstinspires.ftc.teamcode.utils.Targeting; -//import org.firstinspires.ftc.teamcode.utilsv2.Turret; -// -//import java.util.List; -// -//@Config -//@Autonomous (preselectTeleOp = "TeleopV4") -//public class Auto12Ball_Back_Sorted extends LinearOpMode { -// Robot robot; -// MultipleTelemetry TELE; -// // Flywheel flywheel; -//// Targeting targeting; -//// Targeting.Settings targetingSettings; -// Follower follower; -// // Turret turret; -//// Spindexer spindexer; -//// Servos servos; -// MeasuringLoopTimes loopTimes; -// -// // Wait Times -// public static double shootTime = 2; -// -// // Extra Variables -// public static double intakePower = 0.3; -// double shootX, shootY, shootH; -// -// // Initialize path state machine -// private enum PathState { -// PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, -// PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, -// DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, -// DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 -// } -// PathState pathState = PathState.DRIVE_SHOOT0; -// -// // Poses -// public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; -// public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; -// public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; -// public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; -// public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; -// public static double openGateX = 129, openGateY = 74, openGateH = 0; -// public static double shoot1ControlX = 112, shoot1ControlY = 75; -// public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; -// public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; -// public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; -// public static double shoot2ControlX = 102, shoot2ControlY = 63; -// public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; -// public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; -// public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; -// public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; -// public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; -// Pose startPose, pushBot, shoot0, -// pickup1, openGateControl, openGate, shoot1Control, shoot1, -// drivePickup2, pickup2, shoot2Control, shoot2, -// drivePickup3, pickup3, shoot3Control, shoot3; -// private void initializePoses(){ -// startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); -// pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); -// shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); -// pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); -// openGateControl = new Pose(openGateControlX, openGateControlY); -// openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); -// shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); -// shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); -// drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); -// pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); -// shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); -// shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); -// drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); -// pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); -// shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); -// shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); -// } -// -// //Building Paths -// PathChain startPose_pushBot, pushBot_shoot0, -// shoot0_pickup1, pickup1_openGate, openGate_shoot1, -// shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, -// shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; -// private void buildPaths(){ -// startPose_pushBot = follower.pathBuilder() -// .addPath(new BezierLine(startPose, pushBot)) -// .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading()) -// .build(); -// } -// -// //Path State Machine -// private boolean startAuto = true; -// private double timeStamp = 0; -// private void pathStateMachine(){ -// double currentTime = (double) System.currentTimeMillis() / 1000; -// switch(pathState){ -// case DRIVE_SHOOT0: -// if (startAuto){ -// follower.followPath(startPose_shoot0, true); -// startAuto = false; -// shootX = shoot0X; -// shootY = shoot0Y; -// shootH = shoot0H; -// } -// driveShoot(PathState.WAIT_SHOOT0, currentTime); -// break; -// case WAIT_SHOOT0: -// waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime); -// break; -// case DRIVE_PICKUP1: -// drivePickup(PathState.PICKUP1, drivePickup1_pickup1); -// break; -// case PICKUP1: -// pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1); -// shootX = shoot1X; -// shootY = shoot1Y; -// shootH = shoot1H; -// break; -// case DRIVE_SHOOT1: -// intakePowerDown(timeStamp, currentTime); -// driveShoot(PathState.WAIT_SHOOT1, currentTime); -// break; -// case WAIT_SHOOT1: -// waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime); -// break; -// case DRIVE_PICKUP2: -// drivePickup(PathState.PICKUP2, drivePickup2_pickup2); -// break; -// case PICKUP2: -// pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2); -// shootX = shoot2X; -// shootY = shoot2Y; -// shootH = shoot2H; -// break; -// case DRIVE_SHOOT2: -// intakePowerDown(timeStamp, currentTime); -// driveShoot(PathState.WAIT_SHOOT2, currentTime); -// break; -// case WAIT_SHOOT2: -// waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime); -// break; -// case DRIVE_PICKUP3: -// drivePickup(PathState.PICKUP3, drivePickup3_pickup3); -// break; -// case PICKUP3: -// pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3); -// shootX = shoot3X; -// shootY = shoot3Y; -// shootH = shoot3H; -// break; -// case DRIVE_SHOOT3: -// intakePowerDown(timeStamp, currentTime); -// driveShoot(PathState.WAIT_SHOOT3, currentTime); -// break; -// case WAIT_SHOOT3: -//// if (spindexer.shootAllComplete()){ -//// spindexer.resetSpindexer(); -//// TELE.addLine("Done Auto"); -//// } -// break; -// default: -// break; -// } -// TELE.update(); // use for debugging -// } -// -// // Voids for State Machine -// private void intakePowerDown(double stamp, double currentTime) { -// double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust -// if (pow < -1) {pow = 0;} -//// spindexer.setIntakePower(pow); -// } -// private void driveShoot(PathState nextState, double currentTime){ -// if (!follower.isBusy()){ -// pathState = nextState; -// timeStamp = currentTime; -//// spindexer.prepareShootAllContinous(); -// } -// } -// private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { -// if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || -//// spindexer.resetSpindexer(); -// pathState = nextState; -// follower.followPath(nextPath, true); -//// spindexer.setIntakePower(1); -// } -// } -// private void drivePickup(PathState nextState, PathChain nextPath) { -// if (!follower.isBusy()) { -// pathState = nextState; -// follower.followPath(nextPath, intakePower, false); -// } -// } -// private void pickup(PathState nextState, PathChain nextPath) { -// if (!follower.isBusy()) { -// pathState = nextState; -// follower.followPath(nextPath, true); -// } -// } -// -// // Used for changing alliance -// private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} -// private double adjustHeadingBasedOnAlliance(double heading){ -// heading = 180 - heading; -// while (heading > 180) {heading-=360;} -// while (heading <= -180) {heading+=360;} -// return heading; -// } -// -// @Override -// public void runOpMode() throws InterruptedException { -// Robot.resetInstance(); -// robot = Robot.getInstance(hardwareMap); -// List allHubs = hardwareMap.getAll(LynxModule.class); -// for (LynxModule hub : allHubs) { -// hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); -// } -// TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); -//// flywheel = new Flywheel(hardwareMap); -//// targeting = new Targeting(); -//// targetingSettings = new Targeting.Settings(0,0); -// follower = Constants.createFollower(hardwareMap); -// follower.setStartingPose(new Pose(72,72,0)); -//// turret = new Turret(robot, TELE, robot.limelight); -//// spindexer = new Spindexer(hardwareMap); -//// servos = new Servos(hardwareMap); -// loopTimes = new MeasuringLoopTimes(); -// loopTimes.init(); -// -//// robot.light.setPosition(Color.LightRed); -// -// boolean initializeRobot = false; -// while (opModeInInit()){ -// follower.update(); -// -// if (gamepad1.crossWasPressed() && !initializeRobot){ -// Color.redAlliance = !Color.redAlliance; -//// if (Color.redAlliance){ -//// robot.light.setPosition(Color.LightRed); -//// } else { -//// robot.light.setPosition(Color.LightBlue); -//// } -// -//// double[] xPoses = {startPoseX, shoot0X, -//// drivePickup1X, pickup1X, shoot1X, -//// drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, -//// drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; -//// -//// double[] headings = {startPoseH, shoot0H, -//// drivePickup1H, pickup1H, shoot1H, -//// drivePickup2H, pickup2H, shoot2H, -//// drivePickup3H, pickup3H, shoot3H}; -// -//// for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} -//// for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} -// } -// -// if (gamepad1.triangleWasPressed()){ -// initializeRobot = true; -// initializePoses(); -// follower.setPose(startPose); -// buildPaths(); -// sleep(2000); -// -//// turret.setTurret(turrDefault); -//// servos.setSpinPos(spinStartPos); -// } -// -// TELE.addData("Red Alliance?", Color.redAlliance); -// TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); -// TELE.addData("Start Pose", follower.getPose()); -// TELE.update(); -// } -// -// waitForStart(); -// -// if (isStopRequested()) return; -// -//// robot.transfer.setPower(1); -// limelightUsed = false; -// -// while (opModeIsActive()){ -// follower.update(); -// pathStateMachine(); -// Pose currentPose = follower.getPose(); -//// teleStartPoseX = currentPose.getX(); -//// teleStartPoseY = currentPose.getY(); -//// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); -//// -//// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); -//// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); -//// -//// double voltage = robot.voltage.getVoltage(); -//// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); -//// flywheel.manageFlywheel(targetingSettings.flywheelRPM); -//// servos.setHoodPos(targetingSettings.hoodAngle); -//// -//// if (driveToShoot()){servos.setSpinPos(spinStartPos);} -//// else {spindexer.processIntake();} -// -// for (LynxModule hub : allHubs) { -// hub.clearBulkCache(); -// } -// loopTimes.loop(); -// -// TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); -// TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); -// TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); -// TELE.addData("X:", currentPose.getX()); -// TELE.addData("Y:", currentPose.getY()); -// TELE.addData("H:", currentPose.getHeading()); -// TELE.update(); -// } -// } -//} \ No newline at end of file +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Turret; + +import java.util.List; + +@Config +@Autonomous (preselectTeleOp = "TeleopV4") +public class Auto12Ball_Back_Sorted extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + Follower follower; + MeasuringLoopTimes loopTimes; + + // Wait Times + public static double shootTime = 2; + public static double openGateTime = 1.5; + + // Extra Variables + public static double intakePower = 0.3; + double shootX, shootY, shootH; + + // Initialize path state machine + private enum PathState { + PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, + PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, + DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, + DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 + } + PathState pathState = PathState.PUSHBOT; + + // Poses + public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; + public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; + public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454; + public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; + public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031; + public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; + public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; + public static double openGateX = 129, openGateY = 74, openGateH = 0; + public static double shoot1ControlX = 112, shoot1ControlY = 75; + public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; + public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; + public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; + public static double shoot2ControlX = 102, shoot2ControlY = 63; + public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; + public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; + public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; + public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; + public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; + Pose startPose, pushBot, shoot0Control, shoot0, + pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1, + drivePickup2, pickup2, shoot2Control, shoot2, + drivePickup3, pickup3, shoot3Control, shoot3; + private void initializePoses(){ + startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); + pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); + shoot0Control = new Pose(shoot0ControlX, shoot0ControlY); + shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); + pickup1Control = new Pose(pickup1ControlX, pickup1ControlY); + pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); + openGateControl = new Pose(openGateControlX, openGateControlY); + openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); + shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); + shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); + drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); + pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); + shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); + shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); + drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); + pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); + shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); + shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); + } + + //Building Paths + PathChain startPose_pushBot, pushBot_shoot0, + shoot0_pickup1, pickup1_openGate, openGate_shoot1, + shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, + shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; + private void buildPaths(){ + startPose_pushBot = follower.pathBuilder() + .addPath(new BezierLine(startPose, pushBot)) + .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading()) + .build(); + + pushBot_shoot0 = follower.pathBuilder() + .addPath(new BezierCurve(pushBot, shoot0Control, shoot0)) + .setLinearHeadingInterpolation(pushBot.getHeading(), shoot0.getHeading()) + .build(); + + shoot0_pickup1 = follower.pathBuilder() + .addPath(new BezierCurve(shoot0, pickup1Control, pickup1)) + .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading()) + .build(); + + pickup1_openGate = follower.pathBuilder() + .addPath(new BezierCurve(pickup1, openGateControl, openGate)) + .setLinearHeadingInterpolation(pickup1.getHeading(), openGate.getHeading()) + .build(); + + openGate_shoot1 = follower.pathBuilder() + .addPath(new BezierCurve(openGate, shoot1Control, shoot1)) + .setLinearHeadingInterpolation(openGate.getHeading(), shoot1.getHeading()) + .build(); + + shoot1_drivePickup2 = follower.pathBuilder() + .addPath(new BezierLine(shoot1, drivePickup2)) + .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading()) + .build(); + + drivePickup2_pickup2 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup2, pickup2)) + .setLinearHeadingInterpolation(drivePickup2.getHeading(), pickup2.getHeading()) + .build(); + + pickup2_shoot2 = follower.pathBuilder() + .addPath(new BezierCurve(pickup2, shoot2Control, shoot2)) + .setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading()) + .build(); + + shoot2_drivePickup3 = follower.pathBuilder() + .addPath(new BezierLine(shoot2, drivePickup3)) + .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading()) + .build(); + + drivePickup3_pickup3 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup3, pickup3)) + .setLinearHeadingInterpolation(drivePickup3.getHeading(), pickup3.getHeading()) + .build(); + + pickup3_shoot3 = follower.pathBuilder() + .addPath(new BezierCurve(pickup3, shoot3Control, shoot3)) + .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading()) + .build(); + } + + //Path State Machine + private boolean startAuto = true; + private double timeStamp = 0; + private void pathStateMachine(){ + double currentTime = (double) System.currentTimeMillis() / 1000; + switch(pathState){ + case PUSHBOT: + if (startAuto){ + follower.followPath(startPose_pushBot, true); + startAuto = false; + shootX = shoot0X; + shootY = shoot0Y; + shootH = shoot0H; + } + if (!follower.isBusy()){ + follower.followPath(pushBot_shoot0, true); + pathState = PathState.DRIVE_SHOOT0; + } + break; + case DRIVE_SHOOT0: + if (!follower.isBusy()){ + timeStamp = currentTime; + pathState = PathState.WAIT_SHOOT0; + } + break; + case WAIT_SHOOT0: + if (currentTime - timeStamp > shootTime){ + follower.followPath(shoot0_pickup1, intakePower, false); + pathState = PathState.PICKUP1; + } + break; + case PICKUP1: + if (!follower.isBusy()){ + follower.followPath(pickup1_openGate, true); + pathState = PathState.OPENGATE; + shootX = shoot1X; + shootY = shoot1Y; + shootH = shoot1H; + } + break; + case DRIVE_OPENGATE: + if (!follower.isBusy()){ + pathState = PathState.OPENGATE; + timeStamp = currentTime; + } + break; + case OPENGATE: + if (currentTime - timeStamp > openGateTime){ + follower.followPath(openGate_shoot1, true); + pathState = PathState.DRIVE_SHOOT1; + } + break; + case DRIVE_SHOOT1: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT1; + timeStamp = currentTime; + } + break; + case WAIT_SHOOT1: + if (currentTime - timeStamp > shootTime){ + follower.followPath(shoot1_drivePickup2, true); + pathState = PathState.DRIVE_PICKUP2; + } + break; + case DRIVE_PICKUP2: + if (!follower.isBusy()) { + follower.followPath(drivePickup2_pickup2, intakePower, false); + pathState = PathState.PICKUP2; + } + break; + case PICKUP2: + if (!follower.isBusy()){ + follower.followPath(pickup2_shoot2, true); + pathState = PathState.DRIVE_SHOOT2; + } + shootX = shoot2X; + shootY = shoot2Y; + shootH = shoot2H; + break; + case DRIVE_SHOOT2: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT2; + timeStamp = currentTime; + } + break; + case WAIT_SHOOT2: + if (currentTime - timeStamp > shootTime){ + follower.followPath(shoot2_drivePickup3, true); + pathState = PathState.DRIVE_PICKUP3; + } + break; + case DRIVE_PICKUP3: + if (!follower.isBusy()){ + follower.followPath(drivePickup3_pickup3, intakePower, false); + pathState = PathState.PICKUP3; + } + break; + case PICKUP3: + if (!follower.isBusy()){ + follower.followPath(pickup3_shoot3, true); + pathState = PathState.DRIVE_SHOOT3; + } + shootX = shoot3X; + shootY = shoot3Y; + shootH = shoot3H; + break; + case DRIVE_SHOOT3: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT3; + } + break; + case WAIT_SHOOT3: + // add line here to say "done auto' + break; + default: + break; + } + TELE.update(); // use for debugging + } + + // Used for changing alliance + private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} + private double adjustHeadingBasedOnAlliance(double heading){ + heading = 180 - heading; + while (heading > 180) {heading-=360;} + while (heading <= -180) {heading+=360;} + return heading; + } + + @Override + public void runOpMode() throws InterruptedException { + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); + List allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(72,72,0)); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + + boolean initializeRobot = false; + while (opModeInInit()){ + follower.update(); + + if (gamepad1.crossWasPressed() && !initializeRobot){ + Color.redAlliance = !Color.redAlliance; + + double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X, + pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X, + drivePickup2X, pickup2X, shoot2ControlX, shoot2X, + drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; + + double[] headings = {startPoseH, pushBotH, shoot0H, + pickup1H, openGateH, shoot1H, + drivePickup2H, pickup2H, shoot2H, + drivePickup3H, pickup3H, shoot3H}; + + for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} + for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} + } + + if (gamepad1.triangleWasPressed()){ + initializeRobot = true; + initializePoses(); + follower.setPose(startPose); + buildPaths(); + sleep(2000); + } + + TELE.addData("Red Alliance?", Color.redAlliance); + TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); + TELE.addData("Start Pose", follower.getPose()); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + limelightUsed = false; + + while (opModeIsActive()){ + follower.update(); + pathStateMachine(); + Pose currentPose = follower.getPose(); + teleStartPoseX = currentPose.getX(); + teleStartPoseY = currentPose.getY(); + teleStartPoseH = Math.toDegrees(currentPose.getHeading()); + + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); + } + loopTimes.loop(); + + TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); + TELE.addData("X:", currentPose.getX()); + TELE.addData("Y:", currentPose.getY()); + TELE.addData("H:", currentPose.getHeading()); + TELE.update(); + } + } +} \ No newline at end of file From 47c505742a69d71ba9340bd9611c0dc41fe991b9 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 3 Jun 2026 10:03:34 -0500 Subject: [PATCH 08/10] fixes to flywheel in order to operate more globally --- .../ftc/teamcode/tests/NewShooterTest.java | 19 ++++----- .../ftc/teamcode/utilsv2/Flywheel.java | 40 +++++++++++-------- .../ftc/teamcode/utilsv2/Robot.java | 10 +---- 3 files changed, 36 insertions(+), 33 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 97d58f1..8286c29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -17,16 +17,15 @@ import org.firstinspires.ftc.teamcode.utilsv2.Shooter; public class NewShooterTest extends LinearOpMode { Robot robot; - MultipleTelemetry TELE; - + Shooter shooter; Flywheel rpmFlywheel; public static boolean intake = true; public static boolean shoot = false; public static double intakePower = 1.0; public static double transferShootPower = -1; - public static double transferIntakePower = -1.0; + public static double transferIntakePower = -1; public static double turretPos = 0.51; public static double hoodPos = 0.51; public static double flywheel = 0; @@ -45,16 +44,15 @@ public class NewShooterTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - Shooter shooter = new Shooter( + shooter = new Shooter( robot, - new MultipleTelemetry( - telemetry, - FtcDashboard.getInstance().getTelemetry() - ), + TELE, Constants.createFollower(hardwareMap), true ); @@ -130,7 +128,10 @@ public class NewShooterTest extends LinearOpMode { } } - TELE.addData("Flywheel Velocity", (robot.shooter1.getVelocity() * 60) / 28); + TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28); + TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28); + TELE.addData("Flywheel Averag Velocity", rpmFlywheel.getAverageVelocity()); + TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF); TELE.update(); shooter.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index 030f018..14cdd43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -5,12 +5,19 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; + import java.util.LinkedList; public class Flywheel { Robot robot; - public PIDFCoefficients shooterPIDF1, shooterPIDF2; +// public PIDFCoefficients shooterPIDF1, shooterPIDF2; + public static PIDFCoefficients shooterPIDF; + public static double shooterPIDF_P = 255; + public static double shooterPIDF_I = 0.0; + public static double shooterPIDF_D = 0.0; + public static double shooterPIDF_F = 75; private double velo = 0.0; private double velo1 = 0.0; @@ -26,7 +33,7 @@ public class Flywheel { public Flywheel(Robot rob) { robot = rob; - shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / 12); + shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); } public double getVelo() { @@ -50,25 +57,26 @@ public class Flywheel { } // Set the robot PIDF for the next cycle. - private double prevF = 0; - - public static double voltagePIDFDifference = 0.8; public void setPIDF(double p, double i, double d, double f) { - shooterPIDF1.p = p; - shooterPIDF1.i = i; - shooterPIDF1.d = d; - shooterPIDF1.f = f; + shooterPIDF.p = p; + shooterPIDF.i = i; + shooterPIDF.d = d; + shooterPIDF.f = f; - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + } -// if (Math.abs(prevF - f) > voltagePIDFDifference) { -// -// robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); -// -// prevF = f; -// } + private double prevF = 0; + + public static double voltagePIDFDifference = 0.8; + public void setF(double f){ + if (Math.abs(prevF - f) > voltagePIDFDifference) { + shooterPIDF.f = f; + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + prevF = f; + } } // Convert from RPM to Ticks per Second diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 4a17e9c..7cbedc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -48,11 +48,6 @@ public class Robot { public DcMotorEx backRight; public DcMotorEx intake; public DcMotorEx transfer; - public PIDFCoefficients shooterPIDF; - public static double shooterPIDF_P = 255; - public static double shooterPIDF_I = 0.0; - public static double shooterPIDF_D = 0.0; - public static double shooterPIDF_F = 75; // public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; @@ -108,10 +103,9 @@ public class Robot { shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter1.setDirection(DcMotorSimple.Direction.REVERSE); - shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); +// shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter1.setVelocity(0); +// shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); hood = hardwareMap.get(Servo.class, "hood"); From 12e5fba9382023507fea18fdc57f6c66789f9bc7 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 3 Jun 2026 10:18:13 -0500 Subject: [PATCH 09/10] fixed issue - two flywheel instances created a conflict --- .../firstinspires/ftc/teamcode/tests/NewShooterTest.java | 2 ++ .../org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java | 4 +++- .../org/firstinspires/ftc/teamcode/utilsv2/Shooter.java | 7 +------ 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 8286c29..9666aad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -70,6 +70,7 @@ public class NewShooterTest extends LinearOpMode { robot.setHoodPos(hoodPos); shooter.setTurretPosition(turretPos); shooter.setFlywheelVelocity(flywheel); + rpmFlywheel.manageFlywheel(shooter.getFlywheelVelocity()); double voltage = robot.voltage.getVoltage(); rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage); @@ -132,6 +133,7 @@ public class NewShooterTest extends LinearOpMode { TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28); TELE.addData("Flywheel Averag Velocity", rpmFlywheel.getAverageVelocity()); TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF); + TELE.addData("Power", rpmFlywheel.getShooterPower()); TELE.update(); shooter.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index 14cdd43..a4dc26e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -107,6 +107,7 @@ public class Flywheel { averageVelocity = sum / velocityHistory.size(); } + double power; public void manageFlywheel(double commandedVelocity) { if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { @@ -114,7 +115,7 @@ public class Flywheel { } robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - double power = robot.shooter1.getPower(); + power = robot.shooter1.getPower(); robot.shooter2.setPower(power); velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); @@ -127,4 +128,5 @@ public class Flywheel { steady = (Math.abs(commandedVelocity - averageVelocity) < 50); } + public double getShooterPower(){return power;} } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 068aea2..a481195 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -68,7 +68,7 @@ public class Shooter { public void setFlywheelVelocity(double input) { this.flywheelVelocity = input; } - + public double getFlywheelVelocity(){return this.flywheelVelocity;} public int getObeliskID() { return turr.getObeliskID(); } @@ -80,7 +80,6 @@ public class Shooter { case NOTHING: break; case MANUAL: - fly.manageFlywheel(flywheelVelocity); turr.manual(turretPosition); break; case TRACK_GOAL: @@ -104,7 +103,6 @@ public class Shooter { follow.getAcceleration().getYComponent() ); - fly.manageFlywheel(flywheelVelocity); break; case READ_OBELISK: turr.trackObelisk( @@ -122,7 +120,6 @@ public class Shooter { follow.getAcceleration().getYComponent() ); - fly.manageFlywheel(flywheelVelocity); break; case MANUAL_TURRET_TRACK_FLY: @@ -136,7 +133,6 @@ public class Shooter { follow.getAcceleration().getYComponent() ); - fly.manageFlywheel(flywheelVelocity); break; case MANUAL_FLYWHEEL_TRACK_TURR: @@ -150,7 +146,6 @@ public class Shooter { follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent() ); - fly.manageFlywheel(flywheelVelocity); break; } From e658ec044cace09853c054bca4f2c3231cb11887 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 3 Jun 2026 10:20:13 -0500 Subject: [PATCH 10/10] fixed issue - two flywheel instances created a conflict --- .../java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index a481195..b918f32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -7,7 +7,6 @@ import com.pedropathing.follower.Follower; public class Shooter { Robot robot; - Flywheel fly; Turret turr; VelocityCommander commander; @@ -23,7 +22,6 @@ public class Shooter { public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) { this.robot = rob; - this.fly = new Flywheel(rob); this.turr = new Turret(rob); this.follow = follower; this.commander = new VelocityCommander();