intake test to be further tested into crowded balls

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java

@@ -0,0 +1,55 @@
+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;
+
+@Config
+@TeleOp
+public class ColorTest 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()){
+            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();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java

@@ -5,6 +5,7 @@ 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.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -12,25 +13,42 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 
+import java.util.ArrayList;
+import java.util.List;
+
 @Config
 @TeleOp
 public class IntakeTest extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
     Servos servo;
-
+    public boolean green1 = false;
+    public boolean green2 = false;
+    public boolean green3 = false;
+    List<Double> s1G = new ArrayList<>();
+    List<Double> s2G = new ArrayList<>();
+    List<Double> s3G = new ArrayList<>();
+    List<Double> s1T = new ArrayList<>();
+    List<Double> s2T = new ArrayList<>();
+    List<Double> s3T = new ArrayList<>();
+    List<Boolean> s1 = new ArrayList<>();
+    List<Boolean> s2 = new ArrayList<>();
+    List<Boolean> s3 = new ArrayList<>();
     public static int mode = 0; // 0 for teleop, 1 for auto
     public static double manualPow = 1.0;
     double stamp = 0;
     int ticker = 0;
-    boolean b1 = false;
-    boolean b2 = false;
-    boolean b3 = false;
     boolean steadySpin = false;
     double powPID = 0.0;
+    boolean intake = true;
     double spindexerPos = spindexer_intakePos1;
+    boolean wasMoving = false;
     @Override
     public void runOpMode() throws InterruptedException {
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
         robot = new Robot(hardwareMap);
         servo = new Servos(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -63,7 +81,7 @@ public class IntakeTest extends LinearOpMode {
             //TODO: test this monstrosity
             } else if (mode == 1) {
 
-                if (gamepad1.cross){
+                if (gamepad1.cross && intake){
                     robot.intake.setPower(1);
                 } else if (gamepad1.circle){
                     robot.intake.setPower(-1);
@@ -74,8 +92,8 @@ public class IntakeTest extends LinearOpMode {
                 colorDetect();
                 spindexer();
 
-                if (b1 && steadySpin && getRuntime() - stamp > 1.0){
+                if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){
-                    if (!b2){
+                    if (!ballIn(2)){
                         if (servo.spinEqual(spindexer_intakePos1)){
                             spindexerPos = spindexer_intakePos2;
                         } else if (servo.spinEqual(spindexer_intakePos2)){
@@ -83,7 +101,7 @@ public class IntakeTest extends LinearOpMode {
                         } else if (servo.spinEqual(spindexer_intakePos3)){
                             spindexerPos = spindexer_intakePos1;
                         }
-                    } else if (!b3){
+                    } else if (!ballIn(3)){
                         if (servo.spinEqual(spindexer_intakePos1)){
                             spindexerPos = spindexer_intakePos3;
                         } else if (servo.spinEqual(spindexer_intakePos2)){
@@ -100,64 +118,77 @@ public class IntakeTest extends LinearOpMode {
                 stamp = getRuntime();
                 ticker = 0;
                 spindexer();
+                intake = true;
+            }
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
             }
             TELE.addData("Manual Power", manualPow);
             TELE.addData("PID Power", powPID);
-            TELE.addData("B1", b1);
+            TELE.addData("B1", ballIn(1));
-            TELE.addData("B2", b2);
+            TELE.addData("B2", ballIn(2));
-            TELE.addData("B3", b3);
+            TELE.addData("B3", ballIn(3));
             TELE.addData("Spindex Pos", servo.getSpinPos());
+            TELE.update();
         }
     }
 
     public void colorDetect() {
-        // ----- COLOR 1 -----
+        double s1D = robot.color1.getDistance(DistanceUnit.MM);
-        double green1 = robot.color1.getNormalizedColors().green;
+        double s2D = robot.color2.getDistance(DistanceUnit.MM);
-        double blue1 = robot.color1.getNormalizedColors().blue;
+        double s3D = robot.color3.getDistance(DistanceUnit.MM);
-        double red1 = robot.color1.getNormalizedColors().red;
-
-        b1 = robot.color1.getDistance(DistanceUnit.MM) < 40;
-
-        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;
-
-        b2 = robot.color2.getDistance(DistanceUnit.MM) < 40;
-
-        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;
-
-        b3 = robot.color3.getDistance(DistanceUnit.MM) < 30;
-
-        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.addData("Color 1 Distance", s1D);
+        TELE.addData("Color 2 Distance", s2D);
+        TELE.addData("Color 3 Distance", s3D);
         TELE.update();
+
+        if (s1D < 40) {
+            s1T.add(getRuntime());
         }
 
-    public void spindexer(){
+        if (s2D < 40) {
-        if (!servo.spinEqual(spindexerPos)){
+            s2T.add(getRuntime());
+        }
+
+        if (s3D < 30) {
+            s3T.add(getRuntime());
+        }
+    }
+
+    public void spindexer() {
+        boolean atTarget = servo.spinEqual(spindexerPos);
+
+        if (!atTarget) {
             powPID = servo.setSpinPos(spindexerPos);
             robot.spin1.setPower(powPID);
             robot.spin2.setPower(-powPID);
+
             steadySpin = false;
+            wasMoving = true;   // remember we were moving
             stamp = getRuntime();
-        } else{
+        } else {
             robot.spin1.setPower(0);
             robot.spin2.setPower(0);
             steadySpin = true;
+            wasMoving = false;
         }
     }
+
+
+    boolean ballIn(int slot) {
+        List<Double> times;
+
+        if (slot == 1) {times = s1T;}
+        else if (slot == 2) {times = s2T;}
+        else if (slot == 3) {times = s3T;}
+        else return false;
+
+        if (!times.isEmpty()){
+            return times.get(times.size() - 1) > getRuntime() - 2;
+        } else {
+            return false;
+        }
+    }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -52,8 +52,8 @@ public class PositionalServoProgrammer extends LinearOpMode {
                 robot.turr1.setPower(turrHoldPow);
                 robot.turr2.setPower(turrHoldPow);
             } else {
-                robot.spin1.setPower(turretPow);
+                robot.turr1.setPower(turretPow);
-                robot.spin2.setPower(-turretPow);
+                robot.turr2.setPower(-turretPow);
             }
             if (transferPos != 0.501){
                 robot.transferServo.setPosition(transferPos);