From e5cb48eefaf6a8c0459fb083a125a5e07ecd3158 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Fri, 30 Jan 2026 18:40:30 -0600 Subject: [PATCH] Code Cleanup --- .../autonomous/disabled/AutoClose_V3.java | 753 ----------------- .../autonomous/disabled/AutoFar_V1.java | 653 -------------- .../ftc/teamcode/disabled/blank.java | 4 - .../ftc/teamcode/teleop/TeleopV3.java | 310 +------ .../ftc/teamcode/teleop/blank.java | 4 - .../firstinspires/ftc/teamcode/teleop/old.txt | 793 ------------------ .../teamcode/tests/ActiveColorSensorTest.txt | 144 ---- .../ftc/teamcode/tests/ColorSensorTest.txt | 63 -- .../firstinspires/ftc/teamcode/tests/ed.txt | 210 ----- .../ftc/teamcode/utils/FlywheelV2.java | 110 --- 10 files changed, 4 insertions(+), 3040 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java deleted file mode 100644 index d38a711..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java +++ /dev/null @@ -1,753 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous.disabled; - -import static org.firstinspires.ftc.teamcode.constants.Color.*; -import static org.firstinspires.ftc.teamcode.constants.Poses.*; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.FlywheelV2; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; - -import java.util.List; - -@Disabled -@Config -@Autonomous(preselectTeleOp = "TeleopV3") -public class AutoClose_V3 extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - FlywheelV2 flywheel; - Servos servo; - - double velo = 0.0; - public static double intake1Time = 2.7; - public static double intake2Time = 3.0; - public static double colorDetect = 3.0; - boolean gpp = false; - boolean pgp = false; - boolean ppg = false; - double powPID = 0.0; - double bearing = 0.0; - int b1 = 0; // 0 = no ball, 1 = green, 2 = purple - int b2 = 0;// 0 = no ball, 1 = green, 2 = purple - int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - public static double holdTurrPow = 0.1; // power to hold turret in place - - public Action initShooter(int vel) { - return new Action() { - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - TELE.addData("Velocity", velo); - TELE.update(); - - return !flywheel.getSteady(); - } - }; - } - - public Action Obelisk() { - return new Action() { - int id = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - LLResult result = robot.limelight.getLatestResult(); - if (result != null && result.isValid()) { - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - id = fiducial.getFiducialId(); - TELE.addData("ID", id); - TELE.update(); - } - - } - - if (id == 21){ - gpp = true; - } else if (id == 22){ - pgp = true; - } else if (id == 23){ - ppg = true; - } - - TELE.addData("Velocity", velo); - TELE.addData("21", gpp); - TELE.addData("22", pgp); - TELE.addData("23", ppg); - TELE.update(); - - if (gpp || pgp || ppg) { - if (redAlliance){ - robot.limelight.pipelineSwitch(3); - double turretPID = turret_redClose; - robot.turr1.setPosition(turretPID); - robot.turr2.setPosition(-turretPID); - return false; - - } else { - robot.limelight.pipelineSwitch(2); - double turretPID = turret_blueClose; - robot.turr1.setPosition(turretPID); - robot.turr2.setPosition(-turretPID); - return false; - } - } else { - return true; - } - } - }; - } - - public Action spindex(double spindexer, int vel) { - return new Action() { - double spinPID = 0.0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - spinPID = servo.setSpinPos(spindexer); - robot.spin1.setPosition(spinPID); - robot.spin2.setPosition(-spinPID); - TELE.addData("Velocity", velo); - TELE.addLine("spindex"); - TELE.update(); - - drive.updatePoseEstimate(); - teleStart = drive.localizer.getPose(); - - if (servo.spinEqual(spindexer)){ - robot.spin1.setPosition(0); - robot.spin2.setPosition(0); - return false; - } else { - return true; - } - } - }; - } - - public Action Shoot(int vel) { - return new Action() { - double transferStamp = 0.0; - int ticker = 1; - boolean transferIn = false; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - drive.updatePoseEstimate(); - detectTag(); - - teleStart = drive.localizer.getPose(); - - if (ticker == 1) { - transferStamp = getRuntime(); - ticker++; - } - if (getRuntime() - transferStamp > waitTransfer && !transferIn) { - robot.transferServo.setPosition(transferServo_in); - TELE.addData("Velocity", velo); - TELE.addData("ticker", ticker); - TELE.update(); - transferIn = true; - return true; - } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { - robot.transferServo.setPosition(transferServo_out); - robot.turr1.setPosition(holdTurrPow); - robot.turr2.setPosition(holdTurrPow); - TELE.addData("Velocity", velo); - TELE.addLine("shot once"); - TELE.update(); - return false; - } else { - return true; - } - - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double position = spindexer_intakePos1; - double stamp = 0.0; - int ticker = 0; - double pow = 1.0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(pow); - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - if (!servo.spinEqual(position)){ - double spinPID = servo.setSpinPos(position); - robot.spin1.setPosition(spinPID); - robot.spin2.setPosition(-spinPID); - } - - if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ - if (s2D > 60){ - if (servo.spinEqual(spindexer_intakePos1)){ - position = spindexer_intakePos2; - } else if (servo.spinEqual(spindexer_intakePos2)){ - position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos3)){ - position = spindexer_intakePos1; - } - } else if (s3D > 33){ - if (servo.spinEqual(spindexer_intakePos1)){ - position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos2)){ - position = spindexer_intakePos1; - } else if (servo.spinEqual(spindexer_intakePos3)){ - position = spindexer_intakePos2; - } - } - stamp = getRuntime(); - } - - TELE.addData("Velocity", velo); - TELE.addLine("Intaking"); - TELE.update(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(1); - if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) { - robot.spin1.setPosition(0); - robot.spin2.setPosition(0); - if (getRuntime() - stamp - intakeTime < 1){ - pow = -2*(getRuntime() - stamp - intakeTime); - return true; - } else { - robot.intake.setPower(0); - return false; - } - } else { - return true; - } - } - }; - } - - public Action ColorDetect(int vel) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (s1D < 43) { - - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b1 = 2; - } else { - b1 = 1; - } - } - - if (s2D < 60) { - - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b2 = 2; - } else { - b2 = 1; - } - } - - if (s3D < 33) { - - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b3 = 2; - } else { - b3 = 1; - } - } - - TELE.addData("Velocity", velo); - TELE.addLine("Detecting"); - TELE.addData("Distance 1", s1D); - TELE.addData("Distance 2", s2D); - TELE.addData("Distance 3", s3D); - TELE.addData("B1", b1); - TELE.addData("B2", b2); - TELE.addData("B3", b3); - TELE.update(); - - return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect); - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - flywheel = new FlywheelV2(); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0, 0, 0 - )); - - robot.limelight.pipelineSwitch(1); - robot.limelight.start(); - - TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) - .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) - .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); - - TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) - .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) - .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); - - TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - while (opModeInInit()) { - - if (gamepad2.dpadUpWasPressed()) { - hoodAuto -= 0.01; - } - - if (gamepad2.dpadDownWasPressed()) { - hoodAuto += 0.01; - } - - if (gamepad2.crossWasPressed()){ - redAlliance = !redAlliance; - } - - double turrPID; - - if (redAlliance){ - turrPID = turret_detectRedClose; - - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); - - pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) - .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a) - .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b); - - shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); - - pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) - .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a) - .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b); - - shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); - } else { - turrPID = turret_detectBlueClose; - - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) - .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) - .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); - - shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) - .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) - .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); - - shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - } - - robot.turr1.setPosition(turrPID); - robot.turr2.setPosition(-turrPID); - - robot.hood.setPosition(hoodAuto); - - robot.transferServo.setPosition(transferServo_out); - - TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); - TELE.addData("Spin Pos", servo.getSpinPos()); - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - initShooter(AUTO_CLOSE_VEL), - Obelisk() - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.transfer.setPower(1); - - shootingSequence(); - - robot.transfer.setPower(0); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - intake(intake1Time) - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - shoot1.build(), - ColorDetect(AUTO_CLOSE_VEL) - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.transfer.setPower(1); - - shootingSequence(); - - robot.transfer.setPower(0); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - intake(intake2Time) - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - shoot2.build(), - ColorDetect(AUTO_CLOSE_VEL) - ) - ); - - robot.transfer.setPower(1); - - shootingSequence(); - - robot.transfer.setPower(0); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", velo); - TELE.addLine("finished"); - TELE.update(); - - sleep(2000); - - } - - } - //TODO: adjust this according to Teleop numbers - public void detectTag() { - LLResult result = robot.limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - bearing = result.getTx(); - } - } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = turretPos; - robot.turr1.setPosition(turretPID); - robot.turr2.setPosition(-turretPID); - } - - public void shootingSequence() { - TELE.addData("Velocity", velo); - if (gpp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (pgp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence4(); - TELE.addLine("sequence4"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2) { - sequence4(); - TELE.addLine("sequence4"); - } - } else { - sequence3(); - TELE.addLine("sequence3"); - } - } else if (ppg) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } - } else { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - TELE.update(); - } - - public void sequence1() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence2() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence3() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence4() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence5() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence6() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java deleted file mode 100644 index 7ed89b4..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java +++ /dev/null @@ -1,653 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous.disabled; - -import static org.firstinspires.ftc.teamcode.constants.Color.*; -import static org.firstinspires.ftc.teamcode.constants.Poses.*; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.FlywheelV2; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; - -import java.util.List; -@Disabled -@Config -@Autonomous - -public class AutoFar_V1 extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - FlywheelV2 flywheel; - Servos servo; - - double velo = 0.0; - public static double intake1Time = 2.7; - public static double intake2Time = 3.0; - public static double colorDetect = 3.0; - boolean gpp = false; - boolean pgp = false; - boolean ppg = false; - double powPID = 0.0; - double bearing = 0.0; - int b1 = 0; // 0 = no ball, 1 = green, 2 = purple - int b2 = 0;// 0 = no ball, 1 = green, 2 = purple - int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - public static double holdTurrPow = 0.1; // power to hold turret in place - - public Action initShooter(int vel) { - return new Action() { - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - TELE.addData("Velocity", velo); - TELE.update(); - - return !flywheel.getSteady(); - } - }; - } - - public Action Obelisk() { - return new Action() { - int id = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - LLResult result = robot.limelight.getLatestResult(); - if (result != null && result.isValid()) { - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - id = fiducial.getFiducialId(); - TELE.addData("ID", id); - TELE.update(); - } - - } - - if (id == 21){ - gpp = true; - } else if (id == 22){ - pgp = true; - } else if (id == 23){ - ppg = true; - } - - TELE.addData("Velocity", velo); - TELE.addData("21", gpp); - TELE.addData("22", pgp); - TELE.addData("23", ppg); - TELE.update(); - - if (gpp || pgp || ppg) { - if (redAlliance){ - robot.limelight.pipelineSwitch(3); - robot.turr1.setPosition(turret_redFar); - robot.turr2.setPosition(-turret_redFar); - - } else { - robot.limelight.pipelineSwitch(2); - - robot.turr1.setPosition(turret_blueFar); - robot.turr2.setPosition(-turret_blueFar); - - } - - return false; - } else { - return true; - } - } - }; - } - - public Action spindex(double spindexer, int vel) { - return new Action() { - double spinPID = 0.0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - spinPID = servo.setSpinPos(spindexer); - robot.spin1.setPosition(spinPID); - robot.spin2.setPosition(-spinPID); - TELE.addData("Velocity", velo); - TELE.addLine("spindex"); - TELE.update(); - - drive.updatePoseEstimate(); - teleStart = drive.localizer.getPose(); - - if (servo.spinEqual(spindexer)){ - robot.spin1.setPosition(0); - robot.spin2.setPosition(0); - return false; - } else { - return true; - } - } - }; - } - - public Action Shoot(int vel) { - return new Action() { - double transferStamp = 0.0; - int ticker = 1; - boolean transferIn = false; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - drive.updatePoseEstimate(); - detectTag(); - - teleStart = drive.localizer.getPose(); - - if (ticker == 1) { - transferStamp = getRuntime(); - ticker++; - } - if (getRuntime() - transferStamp > waitTransfer && !transferIn) { - robot.transferServo.setPosition(transferServo_in); - TELE.addData("Velocity", velo); - TELE.addData("ticker", ticker); - TELE.update(); - transferIn = true; - return true; - } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { - robot.transferServo.setPosition(transferServo_out); - robot.turr1.setPosition(holdTurrPow); - robot.turr2.setPosition(holdTurrPow); - TELE.addData("Velocity", velo); - TELE.addLine("shot once"); - TELE.update(); - return false; - } else { - return true; - } - - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double position = spindexer_intakePos1; - double stamp = 0.0; - int ticker = 0; - double pow = 1.0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(pow); - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - if (!servo.spinEqual(position)){ - double spinPID = servo.setSpinPos(position); - robot.spin1.setPosition(spinPID); - robot.spin2.setPosition(-spinPID); - } - - if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ - if (s2D > 60){ - if (servo.spinEqual(spindexer_intakePos1)){ - position = spindexer_intakePos2; - } else if (servo.spinEqual(spindexer_intakePos2)){ - position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos3)){ - position = spindexer_intakePos1; - } - } else if (s3D > 33){ - if (servo.spinEqual(spindexer_intakePos1)){ - position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos2)){ - position = spindexer_intakePos1; - } else if (servo.spinEqual(spindexer_intakePos3)){ - position = spindexer_intakePos2; - } - } - stamp = getRuntime(); - } - - TELE.addData("Velocity", velo); - TELE.addLine("Intaking"); - TELE.update(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(1); - if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) { - robot.spin1.setPosition(0); - robot.spin2.setPosition(0); - if (getRuntime() - stamp - intakeTime < 1){ - pow = -2*(getRuntime() - stamp - intakeTime); - return true; - } else { - robot.intake.setPower(0); - return false; - } - } else { - return true; - } - } - }; - } - - public Action ColorDetect(int vel) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (s1D < 43) { - - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b1 = 2; - } else { - b1 = 1; - } - } - - if (s2D < 60) { - - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b2 = 2; - } else { - b2 = 1; - } - } - - if (s3D < 33) { - - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b3 = 2; - } else { - b3 = 1; - } - } - - TELE.addData("Velocity", velo); - TELE.addLine("Detecting"); - TELE.addData("Distance 1", s1D); - TELE.addData("Distance 2", s2D); - TELE.addData("Distance 3", s3D); - TELE.addData("B1", b1); - TELE.addData("B2", b2); - TELE.addData("B3", b3); - TELE.update(); - - return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect); - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - flywheel = new FlywheelV2(); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0, 0, 0 - )); - - robot.limelight.pipelineSwitch(1); - robot.limelight.start(); - - //TODO: add positions to develop auto - - TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(0,0,0)) - .strafeToLinearHeading(new Vector2d(rfx1, rfy1), rfh1); - - while (opModeInInit()) { - - if (gamepad2.dpadUpWasPressed()) { - hoodAuto -= 0.01; - } - - if (gamepad2.dpadDownWasPressed()) { - hoodAuto += 0.01; - } - - if (gamepad2.crossWasPressed()){ - redAlliance = !redAlliance; - } - - double turrPID; - - if (redAlliance){ - turrPID = turret_detectRedClose; - } else { - turrPID = turret_detectBlueClose; - } - - robot.turr1.setPosition(turrPID); - robot.turr2.setPosition(-turrPID); - - robot.hood.setPosition(hoodAutoFar); - - robot.transferServo.setPosition(transferServo_out); - - TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); - TELE.addData("Spin Pos", servo.getSpinPos()); - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - Actions.runBlocking( - new ParallelAction( - initShooter(AUTO_FAR_VEL), - Obelisk() - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.transfer.setPower(1); - - shootingSequence(); - - robot.transfer.setPower(0); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking(park.build()); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", velo); - TELE.addLine("finished"); - TELE.update(); - - sleep(2000); - - } - - } - //TODO: adjust this according to Teleop numbers - public void detectTag() { - LLResult result = robot.limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - bearing = result.getTx(); - } - } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = turretPos; - robot.turr1.setPosition(turretPID); - robot.turr2.setPosition(-turretPID); - } - - public void shootingSequence() { - TELE.addData("Velocity", velo); - if (gpp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (pgp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence4(); - TELE.addLine("sequence4"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2) { - sequence4(); - TELE.addLine("sequence4"); - } - } else { - sequence3(); - TELE.addLine("sequence3"); - } - } else if (ppg) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } - } else { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - TELE.update(); - } - - public void sequence1() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL) - ) - ); - } - - public void sequence2() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL) - ) - ); - } - - public void sequence3() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL) - ) - ); - } - - public void sequence4() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL) - ) - ); - } - - public void sequence5() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL) - ) - ); - } - - public void sequence6() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL), - spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), - Shoot(AUTO_FAR_VEL) - ) - ); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java deleted file mode 100644 index 9258773..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.disabled; - -public class blank { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 99e1697..a2cdc2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -186,7 +186,7 @@ public class TeleopV3 extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); while (opModeIsActive()) { - //LIMELIGHT START + TELE.addData("Is limelight on?", robot.limelight.getStatus()); // LIGHT COLORS @@ -256,85 +256,6 @@ public class TeleopV3 extends LinearOpMode { } - //COLOR: - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - if (s1D < 43) { - - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - s1G.add(gP); - TELE.addData("gp1", gP); - - if (gP >= 0.36) { - s1.add(true); - } else { - s1.add(false); - } - - s1T.add(getRuntime()); - - } - - if (s2D < 60) { - - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - s2G.add(gP); - TELE.addData("gp2", gP); - - if (gP >= 0.43) { - s2.add(true); - } else { - s2.add(false); - } - - s2T.add(getRuntime()); - } - - if (s3D < 33) { - - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - TELE.addData("gp3", gP); - - s3G.add(gP); - - if (gP >= 0.43) { - s3.add(true); - } else { - s3.add(false); - } - - s3T.add(getRuntime()); - } - - if (!s1.isEmpty()) { - green1 = checkGreen(s1, s1T); - } - if (!s2.isEmpty()) { - green2 = checkGreen(s2, s2T); - - } - if (!s3.isEmpty()) { - green3 = checkGreen(s3, s3T); - } - robot.transfer.setPower(1); double offset; @@ -361,11 +282,9 @@ public class TeleopV3 extends LinearOpMode { turret.trackGoal(deltaPose); //VELOCITY AUTOMATIC - if (targetingVel) { + if (autoVel) { vel = targetingSettings.flywheelRPM; - } else if (autoVel) { - vel = velPrediction(distanceToGoal); - } else { + } else { vel = manualVel; } @@ -392,9 +311,7 @@ public class TeleopV3 extends LinearOpMode { if (targetingHood) { robot.hood.setPosition(targetingSettings.hoodAngle); - } else if (autoHood) { - robot.hood.setPosition(0.15 + hoodOffset); - } else { + } else { robot.hood.setPosition(hoodDefaultPos + hoodOffset); } @@ -431,18 +348,6 @@ public class TeleopV3 extends LinearOpMode { autoHood = true; fixedTurret = false; } -// -// if (gamepad2.left_stick_y < -0.5) { -// autoHood = true; -// } else if (gamepad2.left_stick_y > 0.5) { -// autoHood = false; -// hoodOffset = 0; -// if (gamepad2.left_bumper) { -// xOffset = robotX; -// yOffset = robotY; -// headingOffset = robotHeading; -// } -// } if (enableSpindexerManager) { //if (!shootAll) { @@ -471,50 +376,6 @@ public class TeleopV3 extends LinearOpMode { intake = false; reject = false; -// if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { -// -// if (shooterTicker == 0){ -// robot.transferServo.setPosition(transferServo_out); -// robot.spin1.setPosition(spinStartPos); -// robot.spin2.setPosition(1-spinStartPos); -// if (servo.spinEqual(spinStartPos)){ -// shooterTicker++; -// } -// TELE.addLine("starting to shoot"); -// } else { -// robot.transferServo.setPosition(transferServo_in); -// shooterTicker++; -// double prevSpinPos = servo.getSpinPos(); -// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); -// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); -// TELE.addLine("shooting"); -// } - -// //robot.intake.setPower(-0.3); -// if (getRuntime() - shootStamp < 3.0) { -// -// if (shooterTicker == 0 && !servo.spinEqual(ServoPositions.shootAllAutoSpinStartPos)) { -// robot.spin1.setPosition(ServoPositions.shootAllAutoSpinStartPos); -// robot.spin2.setPosition(1 - ServoPositions.shootAllAutoSpinStartPos); -// } else { -// shooterTicker++; -// //robot.intake.setPower(0.0); -// robot.transferServo.setPosition(transferServo_in); -// double prevSpinPos = robot.spin1.getPosition(); -// robot.spin1.setPosition(prevSpinPos + ServoPositions.shootAllSpindexerSpeedIncrease); -// robot.spin2.setPosition(1 - prevSpinPos - ServoPositions.shootAllAutoSpinStartPos); -// } -// -// } else { -// robot.transferServo.setPosition(transferServo_out); -// //spindexPos = spindexer_intakePos1; -// shootAll = false; -// spindexer.resetSpindexer(); -// spindexer.processIntake(); -// -// } - - if (shooterTicker == 0) { spindexer.prepareShootAllContinous(); TELE.addLine("preparing to shoot"); @@ -546,170 +407,7 @@ public class TeleopV3 extends LinearOpMode { } } -// -// if (shootAll) { -// -// TELE.addData("100% works", shootOrder); -// -// intake = false; -// reject = false; -// -// shooterTicker++; -// -// spindexPos = spindexer_intakePos1; -// -// if (getRuntime() - shootStamp < 1) { -// -// if (servo.spinEqual(spindexer_outtakeBall3) || ((getRuntime()-shootStamp)>0.4)){ -// robot.transferServo.setPosition(transferServo_in); -// -// } else { -// robot.transferServo.setPosition(transferServo_out); -// -// } -// -// -// autoSpintake = true; -// -// spindexPos = spindexer_outtakeBall3; -// robot.intake.setPower(0.5); -// -// } -// -// else if (getRuntime() - shootStamp < 1.8) { -// -// robot.transferServo.setPosition(transferServo_in); -// -// autoSpintake = true; -// robot.intake.setPower(0); -// -// spindexPos = spindexer_outtakeBall2; -// -// } -// else if (getRuntime() - shootStamp < 2.6) { -// -// robot.transferServo.setPosition(transferServo_in); -// -// autoSpintake = false; -// -// robot.spin1.setPower(1); -// robot.spin2.setPower(-1); -// -// } -// -// else { -// robot.transferServo.setPosition(transferServo_out); -// spindexPos = spindexer_intakePos1; -// -// shootAll = false; -// -// autoSpintake = true; -// -// robot.transferServo.setPosition(transferServo_out); -// } -// -// } -// if (gamepad1.squareWasPressed()) { -// square = true; -// shootStamp = getRuntime(); -// shootStamp2 = getRuntime(); -// outtake1 = false; -// outtake2 = false; -// outtake3 = false; -// } -// -// if (gamepad1.circleWasPressed()) { -// circle = true; -// shootStamp = getRuntime(); -// shootStamp2 = getRuntime(); -// -// outtake1 = false; -// outtake2 = false; -// outtake3 = false; -// -// } -// -// if (gamepad1.triangleWasPressed()) { -// triangle = true; -// shootStamp = getRuntime(); -// shootStamp2 = getRuntime(); -// -// outtake1 = false; -// outtake2 = false; -// outtake3 = false; -// -// } -// -// if (square || circle || triangle) { -// -// // Count green balls -// int greenCount = 0; -// if (green1) greenCount++; -// if (green2) greenCount++; -// if (green3) greenCount++; -// -// // Determine the odd ball color -// oddBallColor = greenCount < 2; // true = green, false = purple -// -// shootOrder.clear(); -// -// // Determine shooting order based on button pressed -// // square = odd ball first, triangle = odd ball second, circle = odd ball third -// if (square) { -// // Odd ball first -// addOddThenRest(shootOrder, oddBallColor); -// -// } else if (triangle) { -// // Odd ball second -// addOddInMiddle(shootOrder, oddBallColor); -// } else if (circle) { -// // Odd ball last -// addOddLast(shootOrder, oddBallColor); -// } -// -// circle = false; -// square = false; -// triangle = false; -// -// } -// -// // Right bumper shoots all balls fastest, ignoring colors -// if (gamepad1.crossWasPressed()) { -// shootOrder.clear(); -// shootStamp = getRuntime(); -// -// outtake1 = false; -// outtake2 = false; -// outtake3 = false; -// -// // Fastest order (example: slot 3 → 2 → 1) -// if (ballIn(3)) { -// shootOrder.add(3); -// } -// -// if (ballIn(2)) { -// shootOrder.add(2); -// } -// -// if (ballIn(1)) { -// shootOrder.add(1); -// } -// -// if (!shootOrder.contains(3)) { -// shootOrder.add(3); -// } -// -// if (!shootOrder.contains(2)) { -// shootOrder.add(2); -// } -// -// if (!shootOrder.contains(1)) { -// shootOrder.add(1); -// } -// -// shootAll = true; -// } //EXTRA STUFFINESS: drive.updatePoseEstimate(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java deleted file mode 100644 index dd32ec5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -public class blank { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt deleted file mode 100644 index 1ce3e14..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt +++ /dev/null @@ -1,793 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import static org.firstinspires.ftc.teamcode.constants.Poses.*; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Shooter; -import org.firstinspires.ftc.teamcode.subsystems.Spindexer; -import org.firstinspires.ftc.teamcode.subsystems.Transfer; -import org.firstinspires.ftc.teamcode.utils.Robot; - -@Config -@TeleOp -@Disabled -public class old extends LinearOpMode { - - Robot robot; - - Drivetrain drivetrain; - - Intake intake; - - Spindexer spindexer; - - Transfer transfer; - - MultipleTelemetry TELE; - - GamepadEx g1; - - GamepadEx g2; - - public static double defaultSpeed = 1; - - public static double slowMoSpeed = 0.4; - - public static double power = 0.0; - - public static double pos = hoodDefault; - - public boolean all = false; - - public int ticker = 0; - - ToggleButtonReader g1RightBumper; - - ToggleButtonReader g2Circle; - - ToggleButtonReader g2Square; - - ToggleButtonReader g2Triangle; - - ToggleButtonReader g2RightBumper; - - ToggleButtonReader g1LeftBumper; - - ToggleButtonReader g2LeftBumper; - - ToggleButtonReader g2DpadUp; - - ToggleButtonReader g2DpadDown; - - ToggleButtonReader g2DpadRight; - - ToggleButtonReader g2DpadLeft; - - public boolean leftBumper = false; - public double g1RightBumperStamp = 0.0; - - public double g1LeftBumperStamp = 0.0; - - public double g2LeftBumperStamp = 0.0; - - public static int spindexerPos = 0; - - public boolean green = false; - - Shooter shooter; - - public boolean scoreAll = false; - - MecanumDrive drive; - - public boolean autotrack = false; - - public int last = 0; - public int second = 0; - - public double offset = 0.0; - - public static double rIn = 0.59; - - public static double rOut = 0; - - public boolean notShooting = true; - - public boolean circle = false; - - public boolean square = false; - - public boolean tri = false; - - @Override - public void runOpMode() throws InterruptedException { - - drive = new MecanumDrive(hardwareMap, teleStart); - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - FtcDashboard.getInstance().getTelemetry(), - telemetry - ); - - g1 = new GamepadEx(gamepad1); - - g1RightBumper = new ToggleButtonReader( - g1, GamepadKeys.Button.RIGHT_BUMPER - ); - - g2 = new GamepadEx(gamepad2); - - g1LeftBumper = new ToggleButtonReader( - g1, GamepadKeys.Button.LEFT_BUMPER - ); - - g2Circle = new ToggleButtonReader( - g2, GamepadKeys.Button.B - ); - - g2Triangle = new ToggleButtonReader( - g2, GamepadKeys.Button.Y - ); - - g2Square = new ToggleButtonReader( - g2, GamepadKeys.Button.X - ); - - g2RightBumper = new ToggleButtonReader( - g2, GamepadKeys.Button.RIGHT_BUMPER - ); - - g2LeftBumper = new ToggleButtonReader( - g2, GamepadKeys.Button.LEFT_BUMPER - ); - - g2DpadUp = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_UP - ); - - g2DpadDown = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_DOWN - ); - - g2DpadLeft = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_LEFT - ); - - g2DpadRight = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_RIGHT - ); - - drivetrain = new Drivetrain(robot, TELE, g1); - - drivetrain.setMode("Default"); - - drivetrain.setDefaultSpeed(defaultSpeed); - - drivetrain.setSlowSpeed(slowMoSpeed); - - intake = new Intake(robot); - - transfer = new Transfer(robot); - - spindexer = new Spindexer(robot, TELE); - - spindexer.setTelemetryOn(true); - - shooter = new Shooter(robot, TELE); - - shooter.setShooterMode("MANUAL"); - - robot.rejecter.setPosition(rIn); - - waitForStart(); - - if (isStopRequested()) return; - - drive = new MecanumDrive(hardwareMap, teleStart); - - while (opModeIsActive()) { - - drive.updatePoseEstimate(); - - TELE.addData("pose", drive.localizer.getPose()); - - TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); - - TELE.addData("off", offset); - - robot.hood.setPosition(pos); - - g1LeftBumper.readValue(); - - if (g1LeftBumper.wasJustPressed()) { - g2LeftBumperStamp = getRuntime(); - - spindexer.intakeShake(getRuntime()); - - leftBumper = true; - } - - if (leftBumper) { - double time = getRuntime() - g2LeftBumperStamp; - - if (time < 1.0) { - robot.rejecter.setPosition(rOut); - } else { - robot.rejecter.setPosition(rIn); - } - - } - - intake(); - - drivetrain.update(); - - TELE.update(); - - transfer.update(); - - g2RightBumper.readValue(); - - g2LeftBumper.readValue(); - - g2DpadDown.readValue(); - - g2DpadUp.readValue(); - - if (!scoreAll) { - spindexer.checkForBalls(); - } - - if (g2DpadUp.wasJustPressed()) { - pos -= 0.02; - } - - if (g2DpadDown.wasJustPressed()) { - pos += 0.02; - } - - g2DpadLeft.readValue(); - - g2DpadRight.readValue(); - - if (g2DpadLeft.wasJustPressed()) { - offset -= 0.02; - } - - if (g2DpadRight.wasJustPressed()) { - offset += 0.02; - } - - TELE.addData("hood", pos); - - if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { - - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); - - } else { - - autotrack = false; - - shooter.moveTurret(0.3 + offset); - - } - - if (gamepad2.right_stick_button) { - autotrack = true; - } - - if (g2RightBumper.wasJustPressed()) { - transfer.setTransferPower(1); - transfer.transferIn(); - shooter.setManualPower(1); - - notShooting = false; - - } - - if (g2RightBumper.wasJustReleased()) { - transfer.setTransferPower(1); - transfer.transferOut(); - } - - if (gamepad2.left_stick_y > 0.5) { - - shooter.setManualPower(0); - } else if (gamepad2.left_stick_y < -0.5) { - shooter.setManualPower(1); - } - - if (g2LeftBumper.wasJustPressed()) { - g2LeftBumperStamp = getRuntime(); - notShooting = false; - scoreAll = true; - } - - if (scoreAll) { - double time = getRuntime() - g2LeftBumperStamp; - - shooter.setManualPower(1); - - TELE.addData("greenImportant", green); - - TELE.addData("last", last); - TELE.addData("2ndLast", second); - - int numGreen = spindexer.greens(); - - if (square) { - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakePurple(second, last); - second = last; - } else { - last = spindexer.outtakeGreen(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - transfer.transferIn(); - } else if (time < 6) { - - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - } else if (tri) { - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - second = last; - } else { - last = spindexer.outtakePurple(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakePurple(second, last); - } else { - last = spindexer.outtakeGreen(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - transfer.transferIn(); - } else if (time < 6) { - - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - } else if (circle) { - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - second = last; - } else { - last = spindexer.outtakePurple(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - transfer.transferIn(); - } else if (time < 6) { - - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakePurple(second, last); - } else { - last = spindexer.outtakeGreen(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - } else { - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - if (gamepad2.right_trigger > 0.5) { - green = false; - - all = gamepad2.left_trigger > 0.5; - - } else if (gamepad2.left_trigger > 0.5) { - green = true; - - all = false; - } else { - all = true; - } - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (all) { - spindexer.outtake3(); - last = 3; - second = 3; - } else if (green) { - last = spindexer.outtakeGreen(second, last); - second = last; - } else { - last = spindexer.outtakePurple(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - if (gamepad2.right_trigger > 0.5) { - green = false; - - all = gamepad2.left_trigger > 0.5; - - } else if (gamepad2.left_trigger > 0.5) { - green = true; - - all = false; - - } - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (all) { - spindexer.outtake2(); - - last = 2; - } else if (green) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - if (gamepad2.right_trigger > 0.5) { - green = false; - - all = gamepad2.left_trigger > 0.5; - - } else if (gamepad2.left_trigger > 0.5) { - green = true; - - all = false; - } - - transfer.transferIn(); - } else if (time < 6) { - - transfer.transferOut(); - - if (ticker == 0) { - - if (all) { - spindexer.outtake1(); - } else if (green) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - - } - } - - shooter.update(); - - } - - } - - public void intake() { - - g1RightBumper.readValue(); - - g2Circle.readValue(); - - g2Square.readValue(); - - g2Triangle.readValue(); - - if (g1RightBumper.wasJustPressed()) { - - notShooting = true; - - if (getRuntime() - g1RightBumperStamp < 0.3) { - intake.reverse(); - } else { - intake.toggle(); - } - - if (intake.getIntakeState() == 1) { - shooter.setManualPower(0); - } - - spindexer.intake(); - - transfer.transferOut(); - - g1RightBumperStamp = getRuntime(); - - } - - if (intake.getIntakeState() == 1 && notShooting) { - - spindexer.intakeShake(getRuntime()); - - } else { - if (g2Circle.wasJustPressed()) { - circle = true; - tri = false; - square = false; - - } - - if (g2Triangle.wasJustPressed()) { - circle = false; - tri = true; - square = false; - } - - if (g2Square.wasJustPressed()) { - circle = false; - tri = false; - square = true; - } - - if (gamepad2.x) { - circle = false; - tri = false; - square = false; - } - - } - - intake.update(); - - spindexer.update(); - - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt deleted file mode 100644 index 790f8f2..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt +++ /dev/null @@ -1,144 +0,0 @@ -package org.firstinspires.ftc.teamcode.tests; - -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.utils.Robot; - - -@TeleOp -@Config -public class ActiveColorSensorTest extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - - @Override - public void runOpMode() throws InterruptedException{ - robot = new Robot(hardwareMap); - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - int b1Purple = 1; - int b1Total = 1; - int b2Purple = 1; - int b2Total = 1; - int b3Purple = 1; - int b3Total = 1; - - double totalStamp1 = 0.0; - double purpleStamp1 = 0.0; - double totalStamp2 = 0.0; - double purpleStamp2 = 0.0; - double totalStamp3 = 0.0; - double purpleStamp3 = 0.0; - - String b1 = "none"; - String b2 = "none"; - String b3 = "none"; - - double position = 0.0; - - double stamp = getRuntime(); - - waitForStart(); - if (isStopRequested()) return; - - while (opModeIsActive()){ - - if ((getRuntime() % 0.3) >0.15) { - position = spindexer_intakePos1 + 0.015; - } else { - position = spindexer_intakePos1 - 0.015; - } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1-position); - - robot.intake.setPower(1); - - // Reset the counters after 1 second of not reading a ball. - final double ColorCounterResetDelay = 1.0; - // Number of times the loop needs to run before deciding on a color. - final int ColorCounterTotalMinCount = 20; - // If the color sensor reads a color this percentage of time - // out of the total, declare the color. - // Usage: (Color Count)/(Total Count) > ColorCounterThreshold - final double ColorCounterThreshold = 0.65; - - if (robot.pin1.getState()){ - if (robot.pin0.getState()){ - b1Purple ++; - } - b1Total++; - totalStamp1 = getRuntime(); - } - if (getRuntime() - totalStamp1 > ColorCounterResetDelay) { - // Too Much time has passed without detecting ball - b1 = "none"; - b1Total = 1; - b1Purple = 1; - }else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){ - // Enough Time has passed and we met the threshold - b1 = "Purple"; - }else if (b1Total > ColorCounterTotalMinCount) { - // Enough Time passed WITHOUT meeting the threshold - b1 = "Green"; - } - - if (robot.pin3.getState()){ - if (robot.pin2.getState()){ - b2Purple ++; - } - b2Total++; - totalStamp2 = getRuntime(); - } - if (getRuntime() - totalStamp2 > ColorCounterResetDelay) { - // Too Much time has passed without detecting ball - b2 = "none"; - b2Total = 1; - b2Purple = 1; - }else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){ - // Enough Time has passed and we met the threshold - b2 = "Purple"; - }else if (b2Total > ColorCounterTotalMinCount) { - // Enough Time passed WITHOUT meeting the threshold - b2 = "Green"; - } - - if (robot.pin5.getState()){ - if (robot.pin4.getState()){ - b3Purple ++; - } - b3Total++; - totalStamp3 = getRuntime(); - } - if (getRuntime() - totalStamp3 > ColorCounterResetDelay) { - // Too Much time has passed without detecting ball - b3 = "none"; - b3Total = 1; - b3Purple = 1; - }else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){ - // Enough Time has passed and we met the threshold - b3 = "Purple"; - }else if (b3Total > ColorCounterTotalMinCount) { - // Enough Time passed WITHOUT meeting the threshold - b3 = "Green"; - } - - TELE.addData("Green1:", robot.pin1.getState()); - TELE.addData("Purple1:", robot.pin0.getState()); - TELE.addData("Green2:", robot.pin3.getState()); - TELE.addData("Purple2:", robot.pin2.getState()); - TELE.addData("Green3:", robot.pin5.getState()); - TELE.addData("Purple3:", robot.pin4.getState()); - TELE.addData("1", b1); - TELE.addData("2",b2); - TELE.addData("3",b3); - - TELE.update(); - } - } - -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt deleted file mode 100644 index 61e2427..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt +++ /dev/null @@ -1,63 +0,0 @@ -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; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.utils.Robot; - -@TeleOp -@Config -public class ColorSensorTest extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - - @Override - public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - waitForStart(); - if (isStopRequested()) return; - - while (opModeIsActive()) { - -// ----- COLOR 1 ----- - double green1 = robot.color1.getNormalizedColors().green; - double blue1 = robot.color1.getNormalizedColors().blue; - double red1 = robot.color1.getNormalizedColors().red; - - TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor()); - TELE.addData("Color1 green", green1 / (green1 + blue1 + red1)); - TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM)); - - -// ----- COLOR 2 ----- - double green2 = robot.color2.getNormalizedColors().green; - double blue2 = robot.color2.getNormalizedColors().blue; - double red2 = robot.color2.getNormalizedColors().red; - - TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor()); - TELE.addData("Color2 green", green2 / (green2 + blue2 + red2)); - TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM)); - - -// ----- COLOR 3 ----- - double green3 = robot.color3.getNormalizedColors().green; - double blue3 = robot.color3.getNormalizedColors().blue; - double red3 = robot.color3.getNormalizedColors().red; - - TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor()); - TELE.addData("Color3 green", green3 / (green3 + blue3 + red3)); - TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM)); - - - TELE.update(); - } - } - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt deleted file mode 100644 index 91ec518..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt +++ /dev/null @@ -1,210 +0,0 @@ -package org.firstinspires.ftc.teamcode.tests; - -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Shooter; - -@TeleOp -@Config -public class ShooterTest extends LinearOpMode { - - Robot robot; - - public static double pow = 0.0; - public static double vel = 0.0; - public static double ecpr = 1024.0; // CPR of the encoder - public static double hoodPos = 0.5; - public static double turretPos = 0.9; - - public static String flyMode = "VEL"; - - public static boolean AutoTrack = false; - - double initPos = 0.0; - - double velo = 0.0; - double velo1 = 0.0; - double velo2 = 0.0; - double velo3 = 0.0; - double velo4 = 0.0; - double velo5 = 0.0; - - double stamp1 = 0.0; - - double initPos1 = 0.0; - - double powPID = 0.0; - - public static int maxVel = 4500; - - public static boolean shoot = false; - - public static int spindexPos = 1; - - public static boolean intake = true; - - public static int tolerance = 50; - - double stamp = 0.0; - - public static double kP = 0.001; // small proportional gain (tune this) - public static double maxStep = 0.06; // prevents sudden jumps - public static double distance = 50; - - MultipleTelemetry TELE; - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - Shooter shooter = new Shooter(robot, TELE); - - robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - shooter.setTelemetryOn(true); - - shooter.setShooterMode(flyMode); - - initPos = shooter.getECPRPosition(); - - int ticker = 0; - - waitForStart(); - - if (isStopRequested()) return; - - while (opModeIsActive()) { - - ticker++; - - if (AutoTrack){ - hoodPos = hoodAnglePrediction(distance); - vel = velPrediction(distance); - } - - - - shooter.setShooterMode(flyMode); - - shooter.setManualPower(pow); - - robot.hood.setPosition(hoodPos); - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(1 - turretPos); - if (intake) { - robot.transfer.setPower(0); - robot.intake.setPower(0.75); - robot.spin1.setPosition(spindexer_intakePos1); - robot.spin2.setPosition(1 - spindexer_intakePos1); - } else { - robot.transfer.setPower(.75 + (powPID/4)); - robot.intake.setPower(0); - if (spindexPos == 1) { - robot.spin1.setPosition(spindexer_outtakeBall1); - robot.spin2.setPosition(1 - spindexer_outtakeBall1); - } else if (spindexPos == 2) { - robot.spin1.setPosition(spindexer_outtakeBall2); - robot.spin2.setPosition(1 - spindexer_outtakeBall2); - } else if (spindexPos == 3) { - robot.spin1.setPosition(spindexer_outtakeBall3); - robot.spin2.setPosition(1 - spindexer_outtakeBall3); - } - } - - double penguin = 0; - if (ticker % 8 ==0){ - penguin = shooter.getECPRPosition(); - stamp = getRuntime(); - velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1)); - initPos1 = penguin; - stamp1 = stamp; - } - - - velo = velo1; - - double feed = vel / maxVel; // Example: vel=2500 → feed=0.5 - - if (vel > 500){ - feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; - } - - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo1; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - - if (vel - velo > 1000){ - powPID = 1; - } else if (velo - vel > 1000){ - powPID = 0; - } - - shooter.setVelocity(powPID); - - if (shoot) { - robot.transferServo.setPosition(transferServo_in); - } else { - robot.transferServo.setPosition(transferServo_out); - } - - shooter.update(); - - TELE.addData("Revolutions", shooter.getECPRPosition()); - TELE.addData("hoodPos", shooter.gethoodPosition()); - TELE.addData("turretPos", shooter.getTurretPosition()); - TELE.addData("Power Fly 1", robot.shooter1.getPower()); - TELE.addData("Power Fly 2", robot.shooter2.getPower()); - TELE.addData("powPID", shooter.getpowPID()); - TELE.addData("Velocity", velo); - TELE.update(); - - - } - - } - - public double hoodAnglePrediction(double distance) { - double L = 0.298317; - double A = 1.02124; - double k = 0.0157892; - double n = 3.39375; - - double dist = Math.sqrt(distance*distance+24*24); - - return L + A * Math.exp(-Math.pow(k * dist, n)); - } - public static double velPrediction(double distance) { - - double x = Math.sqrt(distance*distance+24*24); - - - - double A = -211149.992; - double B = -1.19943; - double C = 3720.15909; - - return A * Math.pow(x, B) + C; - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java deleted file mode 100644 index a85c8ea..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java +++ /dev/null @@ -1,110 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils; - -import com.acmerobotics.dashboard.config.Config; - -@Config -public class FlywheelV2 { - public static double kP = 0.001; // small proportional gain (tune this) - public static double maxStep = 0.06; // prevents sudden jumps - double initPos1 = 0.0; - double initPos2 = 0.0; - double stamp = 0.0; - double stamp1 = 0.0; - double ticker = 0.0; - double currentPos1 = 0.0; - double currentPos2 = 0.0; - double velo = 0.0; - double velo1 = 0.0; - double velo1a = 0.0; - double velo1b = 0.0; - double velo2 = 0.0; - double velo3 = 0.0; - double velo4 = 0.0; - double velo5 = 0.0; - double targetVelocity = 0.0; - double powPID = 0.0; - boolean steady = false; - - public FlywheelV2() { - //robot = new Robot(hardwareMap); - } - - public double getVelo(double shooter1CurPos, double shooter2CurPos) { - ticker++; - if (ticker % 2 == 0) { - velo5 = velo4; - velo4 = velo3; - velo3 = velo2; - velo2 = velo1; - - currentPos1 = shooter1CurPos / 28; - currentPos2 = shooter2CurPos / 28; - stamp = getTimeSeconds(); //getRuntime(); - velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1)); - velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1)); - initPos1 = currentPos1; - initPos2 = currentPos2; - stamp1 = stamp; - - if (velo1a < 200){ - velo1 = velo1b; - } else if (velo1b < 200){ - velo1 = velo1a; - } else { - velo1 = (velo1a + velo1b) / 2; - } - } - return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5); - } - - public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; } - - public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; } - - public boolean getSteady() { - return steady; - } - - private double getTimeSeconds() { - return (double) System.currentTimeMillis() / 1000.0; - } - - public double manageFlywheel(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) { - targetVelocity = commandedVelocity; - velo = getVelo(shooter1CurPos, shooter2CurPos); - // Flywheel PID code here - if (targetVelocity - velo > 4500) { - powPID = 1.0; - } else if (velo - targetVelocity > 4500) { - powPID = 0.0; - } else { - - double a = 2539.07863; - double c = 1967.6498; - double d = -0.289647; - double h = -1.1569; - - double feed = Math.log10((a / (targetVelocity + c)) + d) / h; - - // --- PROPORTIONAL CORRECTION --- - double error = targetVelocity - velo; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - } - - steady = (Math.abs(targetVelocity - velo) < 100.0); - - return powPID; - } - - public void update() { - } -} \ No newline at end of file