detect block, intake until speci, claw intake power, and teleop action

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java

@@ -145,7 +145,16 @@ public void configureCommands() {
         );
     }
 
-    // TODO: Configure controls for gamepad (talk with driveteam)
+    public void senseBlock() {
+        Claw.BlockCases status = this.claw.hasValidBlock(Claw.Color.BLUE);
+
+        if (status == Claw.BlockCases.ACCEPT) {
+            this.claw.setClawPower(0);
+            this.ready.schedule();
+        } else if (status == Claw.BlockCases.REJECT) this.claw.setClawPower(-1);
+    }
+
+    // TODO: Configure controls for gamepad (talk with drive team)
     public void configureControls() {
         this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
                 .whenPressed(this.accepting);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawIntakePower.java

@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.claw;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Claw;
+
+public class ClawIntakePower extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Claw claw;
+    private final double power;
+
+    public ClawIntakePower(final MultipleTelemetry telemetry, final Claw claw, final double power) {
+        this.telemetry = telemetry;
+        this.power = power;
+        this.claw = claw;
+
+        super.addRequirements(claw);
+    }
+
+    @Override
+    public void initialize() {
+        this.claw.setClawPower(power);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/IntakeUntilSpecimen.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.commands.claw;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.Claw;
+
+public class IntakeUntilSpecimen extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Claw claw;
+    private final Claw.Color color;
+    private final int duration;
+    private final ElapsedTime timer;
+
+    public IntakeUntilSpecimen(final MultipleTelemetry telemetry, final Claw claw, final Claw.Color color, final int duration) {
+        this.telemetry = telemetry;
+        this.claw = claw;
+        this.color = color;
+        this.duration = duration;
+        this.timer = new ElapsedTime();
+
+        super.addRequirements(claw);
+    }
+
+    @Override
+    public void initialize() {
+        this.claw.setClawPower(1);
+        this.timer.reset();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return this.claw.hasValidBlock(color) != Claw.BlockCases.WAIT || this.timer.seconds() >= duration;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java

@@ -38,6 +38,8 @@ public void runOpMode() {
             super.resetCycle();
             CommandScheduler.getInstance().run();
 
+            this.robot.senseBlock();
+
             super.logCycles();
             super.telemetry.update();
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java

@@ -15,6 +15,10 @@ public class Claw extends SubsystemBase {
     public static double READY = 0.5;
     public static double DOWN = 0.0;
 
+    // TODO: Calculate appropriate values for these thresholds
+    public static int COLOR_THRESHOLD = 100;
+    public static int DISTANCE_THRESHOLD = 100;
+
     private final MultipleTelemetry telemetry;
 
     private final Servo leftPivot, rightPivot;
@@ -37,6 +41,21 @@ public void startThreads(OpModeCore opMode) {
         this.sensor.startThread(opMode);
     }
 
+    public BlockCases hasValidBlock(Claw.Color color) {
+        if (this.sensor.getDistance() < DISTANCE_THRESHOLD) {
+            if (this.sensor.getBlue() < COLOR_THRESHOLD && this.sensor.getRed() < COLOR_THRESHOLD)
+                return BlockCases.ACCEPT;
+
+            else if (color == Color.BLUE)
+                return this.sensor.getBlue() > COLOR_THRESHOLD ? BlockCases.ACCEPT : BlockCases.REJECT;
+
+            else
+                return this.sensor.getRed() > COLOR_THRESHOLD ? BlockCases.ACCEPT : BlockCases.REJECT;
+        }
+
+        return BlockCases.WAIT;
+    }
+
     public void setPosition(double position) {
         this.leftPivot.setPosition(position);
         this.rightPivot.setPosition(1 - position);
@@ -46,4 +65,15 @@ public void setClawPower(double power) {
         this.rightCRServo.setPower(power);
         this.leftCRServo.setPower(-power);
     }
+
+    public enum Color {
+        RED,
+        BLUE
+    }
+
+    public enum BlockCases {
+        ACCEPT,
+        REJECT,
+        WAIT
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedColorSensor.java

@@ -7,7 +7,10 @@
 
 public class EnhancedColorSensor {
     private final ColorRangeSensor sensor;
+
     private double distance;
+    private int red;
+    private int blue;
 
     public EnhancedColorSensor(ColorRangeSensor sensor) {
         this.sensor = sensor;
@@ -19,6 +22,8 @@ public void startThread(CommandOpMode opMode) {
                 try {
                     synchronized (this.sensor) {
                         this.distance = this.sensor.getDistance(DistanceUnit.MM);
+                        this.red = this.sensor.red();
+                        this.blue = this.sensor.blue();
                     }
                     Thread.sleep(50);
                 } catch (InterruptedException e) {
@@ -32,6 +37,13 @@ public double getDistance() {
         return this.distance;
     }
 
+    public double getRed() {
+        return this.red;
+    }
+
+    public double getBlue() {
+        return this.blue;
+    }
 
     public void enableLed(boolean enable) {
         this.sensor.enableLed(enable);