diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index 7bb38ee..352becc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; + import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDController; @@ -20,8 +22,6 @@ import java.util.Objects; public class Shooter implements Subsystem { private final DcMotorEx fly1; private final DcMotorEx fly2; - - private final DcMotorEx encoder; private final Servo hoodServo; private final Servo turret1; @@ -50,9 +50,9 @@ public class Shooter implements Subsystem { - private String shooterMode = "AUTO"; + private String shooterMode = "MANUAL"; - private String turretMode = "AUTO"; + private String turretMode = "MANUAL"; public Shooter(Robot robot, MultipleTelemetry TELE) { @@ -78,8 +78,6 @@ public class Shooter implements Subsystem { this.turret2 = robot.turr2; - this.encoder = robot.shooterEncoder; - @@ -172,7 +170,7 @@ public class Shooter implements Subsystem { public void setTurretMode(String mode){ turretMode = mode;} - public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){ + public void trackGoal(Pose2d robotPose, Pose2d goalPose, boolean shooterOn){ fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -182,7 +180,7 @@ public class Shooter implements Subsystem { Pose2d deltaPose = new Pose2d( goalPose.position.x - robotPose.position.x, goalPose.position.y - robotPose.position.y, - goalPose.heading.toDouble() - (robotPose.heading.toDouble()) + goalPose.heading.toDouble() - robotPose.heading.toDouble() ); double distance = Math.sqrt( @@ -191,65 +189,42 @@ public class Shooter implements Subsystem { + Poses.relativeGoalHeight * Poses.relativeGoalHeight ); - telemetry.addData("dst", distance); - double shooterPow = getPowerByDist(distance); double hoodAngle = getAngleByDist(distance); + if (shooterOn){ -// hoodServo.setPosition(hoodAngle); + fly1.setVelocity(shooterPow); + fly2.setPower(fly1.getPower()); - moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); + } else { + fly1.setPower(0); + fly2.setPower(0); + } - return distance; - - - - - - //0.9974 * 355 + hoodServo.setPosition(hoodAngle); + moveTurret(getTurretPosByDeltaPose(deltaPose)); } - public double getTurretPosByDeltaPose (Pose2d dPose, double offset){ + public double getTurretPosByDeltaPose (Pose2d dPose){ double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); - double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x)); - - telemetry.addData("deltaAngle", deltaAngle); - - - - if (deltaAngle > 90) { - deltaAngle -=360; + if (deltaAngle < -180) { + deltaAngle +=360; } + deltaAngle /= (turret_GearRatio*turret_Range); - -// deltaAngle += aTanAngle; - - deltaAngle /= (335); - - telemetry.addData("dAngle", deltaAngle); - - telemetry.addData("AtanAngle", aTanAngle); - - - return ((0.30-deltaAngle) + offset); - + return (0.5+deltaAngle) ; } - //62, 0.44 - - //56.5, 0.5 - - public double getPowerByDist(double dist){ //TODO: ADD LOGIC @@ -258,20 +233,11 @@ public class Shooter implements Subsystem { public double getAngleByDist(double dist){ - - double newDist = dist - 56.5; - - double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46; - - - - - return pos; + //TODO: ADD LOGIC + return dist; } - - public void setTelemetryOn(boolean state){telemetryOn = state;} public void moveTurret(double pos){ @@ -296,16 +262,9 @@ public class Shooter implements Subsystem { fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - fly1.setVelocity(velocity); - - fly2.setPower(fly1.getPower()); - - - - - - + fly1.setVelocity(velocity, AngleUnit.DEGREES); + fly2.setVelocity(velocity, AngleUnit.DEGREES); } else if (Objects.equals(shooterMode, "POS")){ @@ -320,7 +279,7 @@ public class Shooter implements Subsystem { } if (Objects.equals(turretMode, "MANUAL")){ -// hoodServo.setPosition(hoodPos); + hoodServo.setPosition(hoodPos); moveTurret(turretPos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java index bd373f5..2cc3bae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java @@ -41,16 +41,6 @@ public class Transfer implements Subsystem{ this.motorPow = 0; } - public void transferOut(){ - this.setTransferPosition(transferServo_out); - } - - public void transferIn(){ - this.setTransferPosition(transferServo_in); - } - - - @Override 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 c00f792..97026e9 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 @@ -1,20 +1,14 @@ package org.firstinspires.ftc.teamcode.teleop; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.Rejecter; @@ -43,6 +37,8 @@ public class TeleopV1 extends LinearOpMode { Transfer transfer; + Shooter shooter; + MultipleTelemetry TELE; GamepadEx g1; @@ -53,85 +49,34 @@ public class TeleopV1 extends LinearOpMode { public static double slowMoSpeed = 0.4; - public static double power = 0.0; + public static double shooterPower = 0.0; - public static double pos = hoodDefault; + public static double turretPosition = 0.501; - public boolean all = false; - - public int ticker = 0; + public static double hoodPosition = 0.501; ToggleButtonReader g1RightBumper; + ToggleButtonReader g1LeftBumper; + ToggleButtonReader g2Circle; ToggleButtonReader g2Square; - - ToggleButtonReader g2Triangle; - - ToggleButtonReader g2RightBumper; - - ToggleButtonReader g1LeftBumper; - ToggleButtonReader g2LeftBumper; - ToggleButtonReader g2DpadUp; - - ToggleButtonReader g2DpadDown; - - ToggleButtonReader g2DpadRight; - - ToggleButtonReader g2DpadLeft; - - public boolean leftBumper = false; + ToggleButtonReader g2Triangle; 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; - - MecanumDrive drive ; - - public boolean autotrack = false; - - 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; + public double time = 0.0; @Override public void runOpMode() throws InterruptedException { - drive = new MecanumDrive(hardwareMap, teleStart); - - - - - robot = new Robot(hardwareMap); TELE = new MultipleTelemetry( @@ -145,12 +90,12 @@ public class TeleopV1 extends LinearOpMode { g1, GamepadKeys.Button.RIGHT_BUMPER ); - g2 = new GamepadEx(gamepad2); - g1LeftBumper = new ToggleButtonReader( g1, GamepadKeys.Button.LEFT_BUMPER ); + g2 = new GamepadEx(gamepad2); + g2Circle = new ToggleButtonReader( g2, GamepadKeys.Button.B ); @@ -163,33 +108,10 @@ public class TeleopV1 extends LinearOpMode { g2, GamepadKeys.Button.X ); - g2RightBumper = new ToggleButtonReader( - g2, GamepadKeys.Button.RIGHT_BUMPER - ); - - - g2LeftBumper = new ToggleButtonReader( + g2LeftBumper = new ToggleButtonReader( g2, GamepadKeys.Button.LEFT_BUMPER ); - g2DpadUp = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_UP - ); - - - g2DpadDown = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_DOWN - ); - - g2DpadLeft = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_LEFT - ); - - - g2DpadRight = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_RIGHT - ); - @@ -207,65 +129,19 @@ public class TeleopV1 extends LinearOpMode { spindexer = new Spindexer(robot, TELE); - spindexer.setTelemetryOn(true); - shooter = new Shooter(robot, TELE); - shooter.setShooterMode("MANUAL"); - - robot.rejecter.setPosition(rIn); - - + spindexer.setTelemetryOn(true); + time = getRuntime(); waitForStart(); if (isStopRequested()) return; - drive = new MecanumDrive(hardwareMap, teleStart); - - while(opModeIsActive()){ - drive.updatePoseEstimate(); - - TELE.addData("pose", drive.localizer.getPose()); - - TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); - - - TELE.addData("off", offset); - - - robot.hood.setPosition(pos); - - g1LeftBumper.readValue(); - - if (g1LeftBumper.wasJustPressed()){ - g2LeftBumperStamp = getRuntime(); - - - - spindexer.intakeShake(getRuntime()); - - leftBumper = true; - } - - if (leftBumper){ - double time = getRuntime() - g2LeftBumperStamp; - - if (time < 1.0){ - robot.rejecter.setPosition(rOut); - } else { - robot.rejecter.setPosition(rIn); - } - - } - - - - intake(); rejecter.rejecterPos(rpos); @@ -274,526 +150,13 @@ public class TeleopV1 extends LinearOpMode { TELE.update(); - transfer.update(); + transfer(); - g2RightBumper.readValue(); + shooter.setManualPower(shooterPower); - g2LeftBumper.readValue(); - - g2DpadDown.readValue(); - - g2DpadUp.readValue(); - - if (!scoreAll){ - spindexer.checkForBalls(); - } - - if(g2DpadUp.wasJustPressed()){ - pos -=0.02; - } - - if(g2DpadDown.wasJustPressed()){ - pos +=0.02; - } - - g2DpadLeft.readValue(); - - g2DpadRight.readValue(); - - if(g2DpadLeft.wasJustPressed()){ - offset -=0.02; - } - - if(g2DpadRight.wasJustPressed()){ - offset +=0.02; - } - - - - TELE.addData("hood", pos); - - - - - - - - - if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { - - - - - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); - - } else { - - autotrack = false; - - shooter.moveTurret(0.3+offset); - - } - - if (gamepad2.right_stick_button){ - autotrack = true; - } - - - - if (g2RightBumper.wasJustPressed()){ - transfer.setTransferPower(1); - transfer.transferIn(); - shooter.setManualPower(1); - - notShooting = false; - - } - - if (g2RightBumper.wasJustReleased()){ - transfer.setTransferPower(1); - transfer.transferOut(); - } - - if (gamepad2.left_stick_y>0.5){ - - shooter.setManualPower(0); - } else if (gamepad2.left_stick_y<-0.5){ - shooter.setManualPower(1); - } - - if (g2LeftBumper.wasJustPressed()){ - g2LeftBumperStamp = getRuntime(); - notShooting = false; - scoreAll = true; - } - - if (scoreAll) { - double time = getRuntime() - g2LeftBumperStamp; - - - 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 < 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); - - } - - - } - } - - - shooter.update(); + shooter.sethoodPosition(hoodPosition); + shooter.setTurretPosition(turretPosition); @@ -825,10 +188,6 @@ public class TeleopV1 extends LinearOpMode { if (g1RightBumper.wasJustPressed()){ - notShooting = true; - - - if (getRuntime() - g1RightBumperStamp < 0.3){ intake.reverse(); @@ -836,65 +195,47 @@ public class TeleopV1 extends LinearOpMode { intake.toggle(); } - if (intake.getIntakeState()==1){ - shooter.setManualPower(0); - } - - - - spindexer.intake(); - - transfer.transferOut(); g1RightBumperStamp = getRuntime(); } - - if (intake.getIntakeState()==1 && notShooting) { - + if (intake.getIntakeState()==1) { spindexer.intakeShake(getRuntime()); - + transfer.setTransferPowerOff(); + transfer.setTransferPositionOff(); } else { + + if (g2Circle.wasJustPressed()){ - circle = true; - tri = false; - square = false; - - - + transfer.setTransferPositionOff(); + intake.intakeMinPower(); + spindexer.outtake3(); + transfer.setTransferPowerOn(); } if (g2Triangle.wasJustPressed()){ - circle = false; - tri = true; - square = false; + transfer.setTransferPositionOff(); + intake.intakeMinPower(); + spindexer.outtake2(); + transfer.setTransferPowerOn(); } if (g2Square.wasJustPressed()){ - circle = false; - tri = false; - square = true; + transfer.setTransferPositionOff(); + intake.intakeMinPower(); + spindexer.outtake1(); + transfer.setTransferPowerOn(); } - if (gamepad2.x){ - circle = false; - tri = false; - square = false; - } - - - - } - intake.update(); - + transfer.update(); spindexer.update(); @@ -902,5 +243,17 @@ public class TeleopV1 extends LinearOpMode { + } + + public void transfer(){ + + g1LeftBumper.readValue(); + + if (g1LeftBumper.wasJustPressed()){ + transfer.setTransferPositionOn(); + } + + transfer.update(); + } } 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 6427919..7d59ecd 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,34 +1,41 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.config.Config; +import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; @Config -@TeleOp +@Autonomous + + 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")); - waitForStart(); - /* 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.DISTANCE, 3, 20); - crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, lowerBound, higherBound); // green - crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer - crf.setLedBrightness(led); + /* + Using this example configuration, you can detect all three sample colors based on which pin is reading true: + both --> yellow + only pin0 --> blue + only pin1 --> red + neither --> no object + */ + crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 80 / 360.0 * 255, 140 / 360.0 * 255); // green + crf.setPin1Saturation(175, 255); + crf.setPin1Value(100,200); + crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 20mm or closer requirement + + crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple + + crf.setLedBrightness(0); + + waitForStart(); + + stop(); } } @@ -37,9 +44,12 @@ public class ConfigureColorRangefinder extends LinearOpMode { * Online documentation: ... */ class ColorRangefinder { + public static int LED_VALUE = 15; + public final RevColorSensorV3 emulator; private final I2cDeviceSynchSimple i2c; public ColorRangefinder(RevColorSensorV3 emulator) { + this.emulator = emulator; this.i2c = emulator.getDeviceClient(); this.i2c.enableWriteCoalescing(true); } @@ -62,6 +72,22 @@ class ColorRangefinder { setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound); } + public void setPin0Saturation(double lowerBound, double higherBound) { + setDigital(PinNum.PIN0, DigitalMode.SATURATION, lowerBound, higherBound); + } + + public void setPin1Saturation(double lowerBound, double higherBound) { + setDigital(PinNum.PIN1, DigitalMode.SATURATION, lowerBound, higherBound); + } + + // Optional: Easy methods for value/brightness thresholding + public void setPin0Value(double lowerBound, double higherBound) { + setDigital(PinNum.PIN0, DigitalMode.VALUE, lowerBound, higherBound); + } + + public void setPin1Value(double lowerBound, double higherBound) { + setDigital(PinNum.PIN1, DigitalMode.VALUE, lowerBound, higherBound); + } /** * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger. * This is most useful when we want to know if an object is both close and the correct color. @@ -83,18 +109,13 @@ class ColorRangefinder { * This is useful if we want to threshold red; instead of having two thresholds we would invert * the color and look for blue. */ - public void setPin0InvertHue() { - setPin0DigitalMaxDistance(DigitalMode.HSV, 200); - } + /** * Invert the hue value before thresholding it, meaning that the colors become their opposite. * This is useful if we want to threshold red; instead of having two thresholds we would invert * the color and look for blue. */ - public void setPin1InvertHue() { - setPin1DigitalMaxDistance(DigitalMode.HSV, 200); - } /** * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog. @@ -161,7 +182,7 @@ class ColorRangefinder { if (lowerBound == higherBound) { lo = (int) lowerBound; hi = (int) higherBound; - } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255 + } else if (digitalMode.value <= DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range lo = (int) Math.round(lowerBound / 255.0 * 65535); hi = (int) Math.round(higherBound / 255.0 * 65535); } else { // distance in mm @@ -214,7 +235,7 @@ class ColorRangefinder { } public enum DigitalMode { - RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6); + RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6), SATURATION(7), VALUE(8); public final byte value; DigitalMode(int value) { @@ -231,3 +252,4 @@ class ColorRangefinder { } } } +