From 514b02acd480f009577d5cc5750a864a79177a8e Mon Sep 17 00:00:00 2001
From: sihan <idioticplayer0402@outlook.com>
Date: Fri, 10 Nov 2023 23:43:03 -0600
Subject: [PATCH] League meet one Code, not tuned for practice fields or
 anything

---
 .../ftc/teamcode/hardware/Robot.java          | 25 +++++++++++++++++++
 .../ftc/teamcode/opmodes/MainTeleOp.java      | 11 ++++++--
 .../ftc/teamcode/util/Configurables.java      |  2 ++
 .../ftc/teamcode/util/Constants.java          |  1 +
 4 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
index 371a31d..b86e86e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
@@ -9,6 +9,8 @@ import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK;
 import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED;
 import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN;
 import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP;
+import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH;
+import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK;
 import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCK;
 import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED;
 import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP;
@@ -16,6 +18,7 @@ import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE;
 import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
 import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM;
 import static org.firstinspires.ftc.teamcode.util.Constants.LEFTHANG;
+import static org.firstinspires.ftc.teamcode.util.Constants.PLANE;
 import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM;
 import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTHANG;
 import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
@@ -45,6 +48,8 @@ public class Robot {
     private Hang hang;
     @Getter
     private Camera camera;
+    @Getter
+    private Plane plane;
 
     public Robot init(HardwareMap hardwareMap) {
         this.drive = new MecanumDrive(hardwareMap);
@@ -54,6 +59,7 @@ public class Robot {
         this.wrist = new Wrist().init(hardwareMap);
         this.claw = new Claw().init(hardwareMap);
         this.camera = new Camera(hardwareMap);
+        this.plane = new Plane().init(hardwareMap);
         return this;
     }
 
@@ -178,4 +184,23 @@ public class Robot {
             this.claw.setPosition(BIGOPEN);
         }
     }
+
+    public static class Plane {
+        private Servo plane;
+
+        public Plane init(HardwareMap hardwareMap) {
+            this.plane = hardwareMap.get(Servo.class, PLANE);
+            this.plane.setPosition(PLANELOCK);
+            return this;
+        }
+
+        public void planeLock() {
+            this.plane.setPosition(PLANELOCK);
+        }
+
+        public void planeLaunch() {
+            this.plane.setPosition(PLANELAUNCH);
+        }
+
+    }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
index 8a69760..54cf8b3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
@@ -23,8 +23,9 @@ public class MainTeleOp extends OpMode {
         boolean scoreArm = gamepad2.dpad_left || gamepad2.a;
         boolean accurateScoreArm = gamepad2.right_bumper;
         boolean claw = gamepad2.b;
-        boolean pickupWrist = gamepad2.left_bumper || gamepad2.x;
-        boolean scoreWrist = gamepad2.a;
+        boolean pickupWrist = gamepad2.x;
+        boolean scoreWrist = gamepad2.a || gamepad2.left_bumper;
+        boolean launch = gamepad2.y;
 //Drive
         robot.getDrive().setInput(gamepad1, gamepad2);
 //Hang
@@ -53,5 +54,11 @@ public class MainTeleOp extends OpMode {
         } else if (scoreWrist) {
             this.robot.getWrist().wristScore();
         }
+//Plane
+        if (launch) {
+            this.robot.getPlane().planeLaunch();
+        } else {
+            this.robot.getPlane().planeLock();
+        }
     }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
index 6c1a893..db43889 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
@@ -27,6 +27,8 @@ public class Configurables {
     public static double OPEN = 0.53;
     public static double BIGOPEN = 0.45;
     public static double CLOSE = 0.6;
+    public static double PLANELOCK = 0.1;
+    public static double PLANELAUNCH = 0.9;
 
     //Drive Speed
     public static double SPEED = 1;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
index a2cb636..811a874 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
@@ -56,4 +56,5 @@ public class Constants {
     public static final String LEFTARM = "leftArm";
     public static final String RIGHTARM = "rightArm";
     public static final String WRIST = "wrist";
+    public static final String PLANE = "plane";
 }
\ No newline at end of file