From 58e7289c7b690e321139ee601d846e1818f39e14 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Mon, 12 Jan 2026 20:55:09 -0600 Subject: [PATCH] new auton that is very simple --- .../{Auto_V3.java => AutoClose_V3.java} | 36 +- .../ftc/teamcode/autonomous/AutoFar_V1.java | 651 ++++++++++++++++++ .../ftc/teamcode/autonomous/Blue_V2.java | 8 +- .../ftc/teamcode/autonomous/Red_V2.java | 4 +- .../ftc/teamcode/constants/Poses.java | 10 +- .../teamcode/constants/ServoPositions.java | 16 +- .../ftc/teamcode/constants/ShooterVars.java | 1 + 7 files changed, 696 insertions(+), 30 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_V3.java => AutoClose_V3.java} (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java index ec66770..b6ab1c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java @@ -33,7 +33,7 @@ import java.util.List; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_V3 extends LinearOpMode { +public class AutoClose_V3 extends LinearOpMode { Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -101,15 +101,20 @@ public class Auto_V3 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg) { - double turretPID = servo.setTurrPos(turret_red); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); if (redAlliance){ robot.limelight.pipelineSwitch(3); + double turretPID = servo.setTurrPos(turret_redClose); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_redClose); + } else { robot.limelight.pipelineSwitch(2); + double turretPID = servo.setTurrPos(turret_blueClose); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_blueClose); } - return !servo.turretEqual(turret_red); } else { return true; } @@ -256,7 +261,13 @@ public class Auto_V3 extends LinearOpMode { if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) { robot.spin1.setPower(0); robot.spin2.setPower(0); - return false; + if (getRuntime() - stamp - intakeTime < 1){ + pow = -2*(getRuntime() - stamp - intakeTime); + return true; + } else { + robot.intake.setPower(0); + return false; + } } else { return true; } @@ -397,7 +408,11 @@ public class Auto_V3 extends LinearOpMode { redAlliance = !redAlliance; } + double turrPID; + if (redAlliance){ + turrPID = servo.setTurrPos(turret_detectRedClose); + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); @@ -415,6 +430,8 @@ public class Auto_V3 extends LinearOpMode { shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); } else { + turrPID = servo.setTurrPos(turret_detectBlueClose); + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); @@ -433,6 +450,9 @@ public class Auto_V3 extends LinearOpMode { .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); } + robot.turr1.setPower(turrPID); + robot.turr2.setPower(-turrPID); + robot.hood.setPosition(hoodAuto); robot.transferServo.setPosition(transferServo_out); @@ -449,8 +469,6 @@ public class Auto_V3 extends LinearOpMode { if (opModeIsActive()) { - robot.hood.setPosition(hoodAuto); - Actions.runBlocking( new ParallelAction( shoot0.build(), @@ -468,8 +486,6 @@ public class Auto_V3 extends LinearOpMode { robot.transfer.setPower(0); - robot.hood.setPosition(hoodAuto); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java new file mode 100644 index 0000000..88dd86e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java @@ -0,0 +1,651 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +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.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; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +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); + double turretPID = servo.setTurrPos(turret_redFar); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_redFar); + + } else { + robot.limelight.pipelineSwitch(2); + double turretPID = servo.setTurrPos(turret_blueFar); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_blueFar); + } + } 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.setPower(spinPID); + robot.spin2.setPower(-spinPID); + TELE.addData("Velocity", velo); + TELE.addLine("spindex"); + TELE.update(); + + drive.updatePoseEstimate(); + teleStart = drive.localizer.getPose(); + + if (servo.spinEqual(spindexer)){ + robot.spin1.setPower(0); + robot.spin2.setPower(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.setPower(holdTurrPow); + robot.turr2.setPower(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.setPower(spinPID); + robot.spin2.setPower(-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.setPower(0); + robot.spin2.setPower(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 = servo.setTurrPos(turret_detectRedClose); + } else { + turrPID = servo.setTurrPos(turret_detectBlueClose); + } + + robot.turr1.setPower(turrPID); + robot.turr2.setPower(-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 = servo.setTurrPos(turretPos); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-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/autonomous/Blue_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java index 518c249..792b6db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java @@ -178,8 +178,8 @@ public class Blue_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - robot.turr1.setPower(turret_blue); - robot.turr2.setPower(1 - turret_blue); + robot.turr1.setPower(turret_blueClose); + robot.turr2.setPower(1 - turret_blueClose); return false; } else { return true; @@ -541,8 +541,8 @@ public class Blue_V2 extends LinearOpMode { robot.hood.setPosition(hoodAuto); - robot.turr1.setPower(turret_detectBlue); - robot.turr2.setPower(1 - turret_detectBlue); + robot.turr1.setPower(turret_detectBlueClose); + robot.turr2.setPower(1 - turret_detectBlueClose); robot.transferServo.setPosition(transferServo_out); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index bcf89a0..00ce9a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java @@ -169,10 +169,10 @@ public class Red_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - double turretPID = servo.setTurrPos(turret_red); + double turretPID = servo.setTurrPos(turret_redClose); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_red); + return !servo.turretEqual(turret_redClose); } else { return true; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index b9dfead..6deb4ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -15,25 +15,19 @@ public class Poses { public static Pose2d goalPose = new Pose2d(-15, 0, 0); public static double rx1 = 45, ry1 = -7, rh1 = 0; - public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); - public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140); - public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140); - public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); public static double bx1 = 45, by1 = 6, bh1 = 0; - public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); - public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); - public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); - public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140); + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this + public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index f1b49bf..fe47cfa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -26,16 +26,20 @@ public class ServoPositions { public static double hoodAuto = 0.55; - public static double hoodHigh = 0.21; + public static double hoodAutoFar = 0.5; //TODO: change this; - public static double hoodLow = 1.0; + public static double hoodHigh = 0.21; //TODO: change this; - public static double turret_red = 0.42; - public static double turret_blue = 0.38; + public static double hoodLow = 1.0; //TODO: change this; - public static double turret_detectRed = 0.2; + public static double turret_redClose = 0.42; + public static double turret_blueClose = 0.38; + public static double turret_redFar = 0.5; //TODO: change this + public static double turret_blueFar = 0.5; // TODO: change this - public static double turret_detectBlue = 0.6; + public static double turret_detectRedClose = 0.2; + + public static double turret_detectBlueClose = 0.6; public static double turrDefault = 0.40; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index 4494544..3216fcf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -20,4 +20,5 @@ public class ShooterVars { // VELOCITY CONSTANTS public static int AUTO_CLOSE_VEL = 3025; //3300; + public static int AUTO_FAR_VEL = 4000; //TODO: test this } \ No newline at end of file