Merge pull request #79 from rh-robotics/george-jensen-prop-detection

Team Prop Detection

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.<ColorSensor>get("colorSensor").red();
+
+        if (red - colorTolerence < red && red < red + colorTolerence) {
+            teamPropDetected = true;
+        }
+
+        if (blue - colorTolerence < blue && blue < blue + colorTolerence) {
+            teamPropDetected = true;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java

@@ -3,26 +3,33 @@
 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;
 
 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;
     double rightBPower;
     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.<CRServo>get("intakeTube").setPower(intakePower);
             robot.<CRServo>get("intakeGeckoWheels").setPower(0.2);
             robot.<CRServo>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);
+    }
 }