diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java new file mode 100644 index 00000000..e5ec6265 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.auton.colorSensor; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.ColorSensor; + +import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; +import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; + +@Autonomous(name = "Prop Dectection") +public class PropDetection extends OpMode { + + private Hardware robot; + int red; + int blue; + int colorTolerence = 30; + boolean teamPropDetected = false; + + @Override + public void init() { + robot = new Hardware(hardwareMap, telemetry); + robot.introduce(new HardwareElement<>(ColorSensor.class, hardwareMap, "colorSensor")); + + telemetry.addData("Status", "Init"); + } + + @Override + public void loop() { + red = robot.get("colorSensor").red(); + + if (red - colorTolerence < red && red < red + colorTolerence) { + teamPropDetected = true; + } + + if (blue - colorTolerence < blue && blue < blue + colorTolerence) { + teamPropDetected = true; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java index 0ba7d4f4..200ade59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; @@ -10,11 +11,12 @@ import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; -// TODO: Outer intake @com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "TeleOp", group = "OpMode") public class TeleOp extends OpMode { private Hardware robot; + + // Drive Variables double leftFPower; double rightFPower; double leftBPower; @@ -22,7 +24,12 @@ public class TeleOp extends OpMode { double drive; double turn; double strafe; + + // Intake Variables double intakePower = 0.5; + boolean intakeToggle = false; + + // Scoring Algorithm Variables private final double armStartPos = 0.0; private final double armTargetPos = 0.0; private final double armTolerance = 2.5; @@ -43,6 +50,13 @@ public class TeleOp extends OpMode { ElapsedTime viperSlidePositionChange = new ElapsedTime(); ElapsedTime armPositionChange = new ElapsedTime(); + // Gamepads + Gamepad currentGamepad1 = new Gamepad(); + Gamepad currentGamepad2 = new Gamepad(); + + Gamepad previousActionGamepad1 = new Gamepad(); + Gamepad previousActionGamepad2 = new Gamepad(); + @Override public void init() { telemetry.addData("Status", "Initializing"); @@ -84,6 +98,7 @@ public void start() { @Override public void loop() { listControls(); + gamepadUpdate(); // automaticScoring(); clawLock(); @@ -170,7 +185,11 @@ public void clawJoint() { public void intake() { // Activating Intake via gamepad a. - if (gamepad1.a) { + if (currentGamepad1.a && !previousActionGamepad1.a) { + intakeToggle = !intakeToggle; + } + + if (intakeToggle) { robot.get("intakeTube").setPower(intakePower); robot.get("intakeGeckoWheels").setPower(0.2); robot.get("outerIntakeTube").setPower(0.5); @@ -380,4 +399,12 @@ public void listControls() { " * D Pad Up = Outer Intake Down\n" + " * D Pad Down = Outer Intake Up"); } + + public void gamepadUpdate() { + previousActionGamepad1.copy(currentGamepad1); + previousActionGamepad2.copy(currentGamepad2); + + currentGamepad1.copy(gamepad1); + currentGamepad2.copy(gamepad2); + } }