diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java index 2e10ddd..823371c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java @@ -4,6 +4,7 @@ import androidx.annotation.NonNull; +import com.qualcomm.robotcore.hardware.ColorRangeSensor; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -37,9 +38,9 @@ public class HWC { // ------ Declare Motors ------ // public DcMotorEx leftFront, rightFront, leftRear, rightRear, rightPulley, leftPulley, intakeMotor; // ------ Declare Servos ------ // - public Servo intakeArm, wrist, clawR, clawL, passoverArmLeft, passoverArmRight; + public Servo wrist, clawR, clawL, passoverArmLeft, passoverArmRight; // ------ Declare Sensors ------ // - public ColorSensor colorLeft, colorRight; + public ColorRangeSensor colorLeft, colorRight; // ------ Declare Gamepads ------ // public Gamepad currentGamepad1 = new Gamepad(); public Gamepad currentGamepad2 = new Gamepad(); @@ -90,7 +91,6 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { intakeMotor = hardwareMap.get(DcMotorEx.class, "intakeMotor"); // ------ Retrieve Servos ------ // - intakeArm = hardwareMap.get(Servo.class, "intakeArm"); clawL = hardwareMap.get(Servo.class, "clawL"); clawR = hardwareMap.get(Servo.class, "clawR"); wrist = hardwareMap.get(Servo.class, "wrist"); @@ -101,8 +101,8 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { // ------ Retrieve Sensors ------ // webcam = hardwareMap.get(WebcamName.class, "webcam"); - colorLeft = hardwareMap.get(ColorSensor.class, "colorL"); - colorRight = hardwareMap.get(ColorSensor.class, "colorR"); + colorLeft = hardwareMap.get(ColorRangeSensor.class, "colorL"); + colorRight = hardwareMap.get(ColorRangeSensor.class, "colorR"); // ------ Set Motor Directions ------ // leftFront.setDirection(DcMotorEx.Direction.FORWARD); @@ -129,26 +129,6 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { rightPulley.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); } - // ------ Function to Run Intake ------ // - // Runs intake at given power until color sensor detects a - // pixel - public void runIntake(double pwr) { - intakeMotor.setPower(pwr); - if (intakeDetection(colorLeft) == 0 && intakeDetection(colorRight) == 0) { - clawL.setPosition(1); - clawR.setPosition(1); - intakeMotor.setPower(0); - } else if (intakeDetection(colorLeft) == 1) { - clawL.setPosition(1); - - } else if (intakeDetection(colorRight) == 0) { - clawR.setPosition(1); - - - } - - } - // ------ Function to Toggle Claw (Open/Close) ------ // public void toggleClaw(char servo) { switch (servo) { @@ -159,39 +139,12 @@ public void toggleClaw(char servo) { clawR.setPosition(clawR.getPosition() == 0 ? 0.50 : 0); break; case 'C': - clawR.setPosition(clawR.getPosition() == 0 ? 0.85 : 0); - clawL.setPosition(clawL.getPosition() == 1 ? 0.15 : 1); + clawR.setPosition(clawR.getPosition() == 0 ? 0.5 : 0); + clawL.setPosition(clawL.getPosition() == 1 ? 0.5 : 1); break; } } - - // ------ Function to get Color ------ // - public String getColor(ColorSensor CS) { - int red = CS.red(); - int green = CS.green(); - int blue = CS.blue(); - String color; - - if (red > 200 && green > 200 && blue > 200) { - color = "white"; - } else if (210 < red && green > 200 & blue < 160) { - color = "yellow"; - } else if (green > red + blue) { - color = "green"; - } else { - color = "unknown"; - } - - return color; - } - - public int intakeDetection(ColorSensor CS) { - if (CS.argb() == 0) { - return 1; - } else return 0; - } - public void betterSleep(int sleep) { sleepTime.reset(); while (sleepTime.milliseconds() < sleep) { @@ -228,65 +181,6 @@ public void resetEncoders() { rightRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); } - // ------ Function to Drive Given Distance Using Odometry ------ // - public void odoDrive(int distance) { - leftRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // y axis - rightRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis - leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis - while (leftRear.getCurrentPosition() > distance) { - leftFront.setPower(0.3); - rightFront.setPower(0.3); - leftRear.setPower(0.3); - rightRear.setPower(0.3); - } - leftFront.setPower(0); - rightFront.setPower(0); - leftRear.setPower(0); - rightRear.setPower(0); - } - - // ------ Function to Strafe Given Distance Using Odometry ------ // - public void odoStrafeLeft(int distance) { - odoDrive(500); - leftRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // y axis - rightRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // x axis - leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis - - - while (leftRear.getCurrentPosition() < distance) { - leftFront.setPower(-0.3); - rightFront.setPower(0.3); - leftRear.setPower(0.3); - rightRear.setPower(-0.3); - } - } - - public void odoStrafeRight(int distance) { - odoDrive(500); - leftRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // y axis - rightRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // x axis - leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis - - - while (Math.abs(leftRear.getCurrentPosition()) < distance) { - leftFront.setPower(0.3); - rightFront.setPower(-0.3); - leftRear.setPower(-0.3); - rightRear.setPower(0.3); - } - } - - public void odoTurnRight() { - leftFront.setPower(0.3); - rightFront.setPower(-0.3); - leftRear.setPower(-0.3); - rightRear.setPower(0.3); - } - - // ------ Function to Turn Given Degrees Using Odometry ------ // - public void odoTurn(int degrees) { - } - public void initTFOD(String TFOD_MODEL_ASSET) { // Create the TensorFlow processor by using a builder. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java index bc23cd5..faeafc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.subsystems.HWC; import org.firstinspires.ftc.teamcode.teleop.enums.MultiplierSelection; @@ -161,14 +162,19 @@ public void loop() { // ------ (GAMEPAD 1) Intake Toggle Controls ------ // if (robot.currentGamepad1.right_bumper && !robot.previousGamepad1.right_bumper) { - if (intakeState == IntakeState.INTAKE || intakeState == IntakeState.OUTTAKE) { intakeState = IntakeState.OFF; } - else if (intakeState == IntakeState.OFF) { intakeState = IntakeState.INTAKE; } + if (intakeState == IntakeState.INTAKE || intakeState == IntakeState.OUTTAKE) { + intakeState = IntakeState.OFF; + } else if (intakeState == IntakeState.OFF) { + intakeState = IntakeState.INTAKE; + } } if (robot.currentGamepad1.left_bumper && !robot.previousGamepad1.left_bumper) { if (intakeState == IntakeState.INTAKE || intakeState == IntakeState.OUTTAKE) { intakeState = IntakeState.OFF; - } else if (intakeState == IntakeState.OFF) { intakeState = IntakeState.OUTTAKE; } + } else if (intakeState == IntakeState.OFF) { + intakeState = IntakeState.OUTTAKE; + } } // ------ (GAMEPAD 1) MANUAL Wrist Controls ------ // if (robot.currentGamepad1.dpad_up && !robot.previousGamepad1.dpad_up) { @@ -208,15 +214,35 @@ public void loop() { robot.wrist.setPosition(wristPosition); // -------- Check Intake State & Run Intake ------ // - switch(intakeState) { + switch (intakeState) { case INTAKE: + // Set Power robot.intakeMotor.setPower(-1); + + // Close Claws when Pixel Detected + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1) { + robot.clawL.setPosition(0.5); + } + + if (robot.colorRight.getDistance(DistanceUnit.CM) <= 1) { + robot.clawR.setPosition(0.5); + } + + // If both Pixels are Detected, Stop Intake + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) { + intakeState = IntakeState.OFF; + } + break; case OFF: + // Give Manual Control to Gamepad 2 robot.intakeMotor.setPower(robot.currentGamepad2.right_stick_x); + break; case OUTTAKE: + // Set Power robot.intakeMotor.setPower(1); + break; }