diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java new file mode 100644 index 0000000..c05b5f8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.Robot; + + +@Config +@Autonomous +public class Red extends LinearOpMode { + + Robot robot; + + MultipleTelemetry TELE; + + MecanumDrive drive; + + + + @Override + public void runOpMode() throws InterruptedException { + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()){ + + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java index 804276f..9c168b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java @@ -90,6 +90,7 @@ public class Spindexer implements Subsystem{ } public void checkForBalls() { + if (p0.getState()){ ball0 = true; green0 = p1.getState(); @@ -141,25 +142,34 @@ public class Spindexer implements Subsystem{ position = spindexer_outtakeBall1; } - public void outtakeGreen() { - if (green0) { - outtake1(); - } else if (green1){ - outtake2(); - } else if (green2) { + + public int outtakeGreen(int secLast, int Last) { + if (green2 && (secLast!=3) && (Last!=3)) { outtake3(); + return 3; + } else if (green1 && (secLast!=2) && (Last!=2)){ + outtake2(); + return 2; + } else if (green0 && (secLast!=1) && (Last!=1)) { + outtake1(); + return 1; + } else { + + if (secLast!=1 && Last!= 1){ + outtake1(); + return 1; + } else if (secLast!=2 && Last!=2){ + outtake2(); + return 2; + } else { + outtake3(); + return 3; + } + } } - public void outtakePurpleFs() { - if (!green0 && ball0) { - outtake1(); - } else if (!green1 && ball1){ - outtake2(); - } else if (!green2 && ball2) { - outtake3(); - } - } + public void outtakeGreenFs() { if (green0 && ball0) { @@ -171,13 +181,46 @@ public class Spindexer implements Subsystem{ } } - public void outtakePurple() { - if (!green0) { - outtake1(); - } else if (!green1){ - outtake2(); - } else if (!green2) { + public int greens() { + int num = 0; + + if (green0){num++;} + + if (green1){num++;} + + + if (green2){num++;} + + return num; + + + } + + + public int outtakePurple(int secLast, int Last) { + if (!green2 && (secLast!=3) && (Last!=3)) { outtake3(); + return 3; + } else if (!green1 && (secLast!=2) && (Last!=2)){ + outtake2(); + return 2; + } else if (!green0 && (secLast!=1) && (Last!=1)) { + outtake1(); + return 1; + } else { + + if (secLast!=1 && Last!= 1){ + outtake1(); + return 1; + } else if (secLast!=2 && Last!=2){ + outtake2(); + return 2; + } else { + outtake3(); + return 3; + } + + } } @@ -198,7 +241,7 @@ public class Spindexer implements Subsystem{ colorSensorTelemetry(); } - checkForBalls(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 0f716b2..d648c40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -49,6 +49,10 @@ public class TeleopV1 extends LinearOpMode { public static double pos = 0.54; + public boolean all = false; + + public int ticker = 0; + ToggleButtonReader g1RightBumper; ToggleButtonReader g2Circle; @@ -60,6 +64,8 @@ public class TeleopV1 extends LinearOpMode { ToggleButtonReader g2RightBumper; + ToggleButtonReader g1LeftBumper; + ToggleButtonReader g2LeftBumper; ToggleButtonReader g2DpadUp; @@ -69,12 +75,19 @@ public class TeleopV1 extends LinearOpMode { 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; @@ -83,10 +96,23 @@ public class TeleopV1 extends LinearOpMode { public boolean autotrack = true; + 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 @@ -113,6 +139,10 @@ public class TeleopV1 extends LinearOpMode { g2 = new GamepadEx(gamepad2); + g1LeftBumper = new ToggleButtonReader( + g1, GamepadKeys.Button.LEFT_BUMPER + ); + g2Circle = new ToggleButtonReader( g2, GamepadKeys.Button.B ); @@ -176,6 +206,8 @@ public class TeleopV1 extends LinearOpMode { shooter.setShooterMode("MANUAL"); + robot.rejecter.setPosition(rIn); + @@ -196,8 +228,30 @@ public class TeleopV1 extends LinearOpMode { TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); + TELE.addData("off", offset); + + robot.hood.setPosition(pos); + g1LeftBumper.readValue(); + + if (g1LeftBumper.wasJustPressed()){ + g2LeftBumperStamp = getRuntime(); + + leftBumper = true; + } + + if (leftBumper){ + double time = getRuntime() - g2LeftBumperStamp; + + if (time < 1.0){ + robot.rejecter.setPosition(rOut); + } else { + robot.rejecter.setPosition(rIn); + } + + } + intake(); @@ -215,6 +269,10 @@ public class TeleopV1 extends LinearOpMode { g2DpadUp.readValue(); + if (!scoreAll){ + spindexer.checkForBalls(); + } + if(g2DpadUp.wasJustPressed()){ pos -=0.02; } @@ -235,7 +293,7 @@ public class TeleopV1 extends LinearOpMode { offset +=0.02; } - if (gamepad2.right_trigger > 0.5){ + if (gamepad2.right_stick_x > 0.5){ pos = shooter.getAngleByDist( shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset) ); @@ -306,35 +364,422 @@ public class TeleopV1 extends LinearOpMode { 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) { - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1.5){ - spindexer.outtake3(); - } else if (time < 2.0){ - transfer.transferIn(); - } else if (time < 3){ - transfer.transferOut(); - spindexer.outtake2(); - } else if (time < 3.5){ - transfer.transferIn(); - } else if (time < 4.5){ - transfer.transferOut(); - spindexer.outtake1(); - } else if (time < 5){ - transfer.transferIn(); - } else { + 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); + + } - scoreAll = false; - transfer.transferOut(); - shooter.setManualPower(0); } - - - } @@ -403,41 +848,34 @@ public class TeleopV1 extends LinearOpMode { spindexer.intakeShake(getRuntime()); - } else - - { - - + } else { if (g2Circle.wasJustPressed()){ - spindexer.outtake3(); + circle = true; + tri = false; + square = false; + - } if (g2Triangle.wasJustPressed()){ - spindexer.outtake2(); + circle = false; + tri = true; + square = false; } if (g2Square.wasJustPressed()){ - spindexer.outtake1(); + circle = false; + tri = false; + square = true; } - - if (g2Circle.wasJustReleased()){ - transfer.setTransferPower(1); - + if (gamepad2.x){ + circle = false; + tri = false; + square = false; } - if (g2Triangle.wasJustReleased()){ - transfer.setTransferPower(1); - } - - if (g2Square.wasJustReleased()){ - transfer.setTransferPower(1); - } - - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index 22c434e..6427919 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java @@ -1,13 +1,22 @@ package org.firstinspires.ftc.teamcode.utils; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; - +@Config @TeleOp public class ConfigureColorRangefinder extends LinearOpMode { + + + + public static double lowerBound = 80; + public static double higherBound = 120; + + public static int led = 0; + @Override public void runOpMode() throws InterruptedException { ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color")); @@ -15,10 +24,10 @@ public class ConfigureColorRangefinder extends LinearOpMode { /* Using this example configuration, you can detect both artifact colors based on which pin is reading true: pin0 --> purple pin1 --> green */ - crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0 / 360.0 * 255, 360 / 360.0 * 255); // purple - crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement - crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green - crf.setLedBrightness(2); + crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 3, 20); + crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, lowerBound, higherBound); // green + crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer + crf.setLedBrightness(led); } }