More tele...almost done ig lol

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()){
+
+        }
+
+    }
+}

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) {
+    public int outtakeGreen(int secLast, int Last) {
-            outtake1();
+        if (green2 && (secLast!=3) && (Last!=3)) {
-        } else if (green1){
-            outtake2();
-        } else if (green2) {
             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() {
+    public int greens() {
-        if (!green0) {
+        int num = 0;
-            outtake1();
+
-        } else if (!green1){
+        if (green0){num++;}
-            outtake2();
+
-        } else if (!green2) {
+        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();
+
 
     }
 }

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) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
                         transfer.transferOut();
                         transfer.setTransferPower(1);
-                } else if (time < 1.5){
+                    } else if (time < 2) {
-                    spindexer.outtake3();
+
-                } else  if (time < 2.0){
+                        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 < 3){
+                    } else if (time < 4) {
                         transfer.transferOut();
-                    spindexer.outtake2();
+
-                } else  if (time < 3.5){
+                        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 < 4.5){
+                    } else if (time < 6) {
+
+
                         transfer.transferOut();
-                    spindexer.outtake1();
+
-                }  else  if (time < 5){
+                        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);
+
+                    }
+
+
+                }
             }
 
 
@@ -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 (gamepad2.x){
-            if (g2Circle.wasJustReleased()){
+                circle = false;
-                transfer.setTransferPower(1);
+                tri = false;
-
+                square = false;
             }
 
-            if (g2Triangle.wasJustReleased()){
-                transfer.setTransferPower(1);
-            }
-
-            if (g2Square.wasJustReleased()){
-                transfer.setTransferPower(1);
-            }
-
-
 
 
 

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.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 3,  20);
-        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, lowerBound, higherBound); // green
-        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green
+        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer
-        crf.setLedBrightness(2);
+        crf.setLedBrightness(led);
 
     }
 }