diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java index 414b9cde..de33269c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java +++ b/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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/auton/ClawIntakePower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/auton/ClawIntakePower.java deleted file mode 100644 index 60ba702b..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/auton/ClawIntakePower.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.firstinspires.ftc.teamcode.commands.auton; - -public class ClawIntakePower { - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/auton/IntakeUntilSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/auton/IntakeUntilSpecimen.java deleted file mode 100644 index e48b9aca..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/auton/IntakeUntilSpecimen.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.commands.auton; - -public class IntakeUntilSpecimen { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawIntakePower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawIntakePower.java new file mode 100644 index 00000000..f1485865 --- /dev/null +++ b/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); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/IntakeUntilSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/IntakeUntilSpecimen.java new file mode 100644 index 00000000..e639c396 --- /dev/null +++ b/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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java index 17634559..ad9ac669 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java +++ b/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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java index eb35e8f3..229d776e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java +++ b/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 + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedColorSensor.java index 0dbcecdc..07ca81b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedColorSensor.java +++ b/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);