Merge pull request #598 from XaverianTeamRobotics/feature/newscrimmag…

…ebot

Feature/newscrimmagebot

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/features/AirplaneLauncher.java

@@ -10,17 +10,16 @@
  * <p>
  * Connections: A servo in port 4 and a controller.
  * <p>
- * Controls: Press up on the dpad to launch the plane.
+ * Controls: Press triangle to launch the plane.
  */
 public class AirplaneLauncher extends Feature implements Buildable {
     @Override
     public void build() {
-        Devices.servo4.setPosition(0.0);
+        Devices.servo3.setPosition(50.0);
     }
 
     @Override
     public void loop() {
         if (Devices.controller1.getTriangle()) Devices.servo3.setPosition(100.0);
-        if (Devices.controller1.getSquare()) Devices.servo3.setPosition(100.0);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/features/ArmClaw.java

@@ -1,15 +1,11 @@
 package org.firstinspires.ftc.teamcode.features;
 
-import static org.firstinspires.ftc.teamcode.internals.hardware.Devices.motor2;
-import static org.firstinspires.ftc.teamcode.internals.hardware.Devices.motor6;
-import static org.firstinspires.ftc.teamcode.internals.hardware.Devices.servo0;//this is the left grabber
-import static org.firstinspires.ftc.teamcode.internals.hardware.Devices.servo1;//this is the right grabber
-import static org.firstinspires.ftc.teamcode.internals.hardware.Devices.servo2;//this is the servo for the claw mechanism
-
 import org.firstinspires.ftc.teamcode.internals.features.Buildable;
 import org.firstinspires.ftc.teamcode.internals.features.Feature;
 import org.firstinspires.ftc.teamcode.internals.hardware.Devices;//our two devices our the motors for the arm
 
+import static org.firstinspires.ftc.teamcode.internals.hardware.Devices.*;
+
 /**
  * this runs the grabber, intake, and claw pivot mechanism.  Dpad left is to close the left grabber,
  * Dpad right closes the right grabber, Dpad up opens both grabbers, Dpad down starts/stops the
@@ -22,12 +18,21 @@ public class ArmClaw extends Feature implements Buildable {
     public static final int R_CLOSED = 100, L_CLOSED = 5;
     private double counter = 0;
     private boolean dpadPressed = false;
+    private double counter2 = 0; //left servo
+    private double counter3 = 0; //right servo
+    private boolean dpadPressed2 = false; //left servo
+    private boolean dpadPressed3 = false; //right servo
     public void build(){
         servo0.setPosition(L_OPEN);
         servo1.setPosition(R_OPEN);
         servo2.setPosition(0);
+        motor0.setPower(0);
     }
-    public int intakeCount() {
+    /**
+     *this method counts how many times Dpad Down has been pressed so that we can use it to turn the
+     *intake on and off
+     **/
+    private int intakeCount() {
         if (Devices.controller2.getDpadDown() && !dpadPressed) {
             counter += 1;
             dpadPressed = true;
@@ -38,31 +43,55 @@ public int intakeCount() {
             return 0;
         }
     }
-    public void loop() {
-        double motorPower = -(Devices.controller2.getRightTrigger() - Devices.controller2.getLeftTrigger()) * 0.75;
-        Devices.motor0.setPower(motorPower);
-        Devices.motor1.setPower(motorPower);
 
-        //the motors must move in opposite directions
-        if (Devices.controller2.getDpadLeft()) {
-            servo0.setPosition(L_CLOSED);
-            //closes the left grabber
+    /**
+     * this method counts how many times Dpad Left has been pressed so that we can use it to open
+     * and close the left servo (left grabber)
+     */
+    private int leftButtonPressedCount() {
+        if (Devices.controller2.getDpadLeft() && !dpadPressed2) {
+            counter2 += 1;
+            dpadPressed2 = true;
         }
-        if (Devices.controller2.getDpadRight()) {
-            servo1.setPosition(R_CLOSED);
-            //closes the right grabber
+        else if (!Devices.controller2.getDpadLeft()) {
+            dpadPressed2 = false;
         }
-        if (Devices.controller2.getDpadUp()) {
-            servo0.setPosition(L_OPEN);
-            servo1.setPosition(R_OPEN);
-            //opens both grabbers
+        if (counter2 % 2 != 0) {
+            return L_CLOSED;
         }
-        if (Devices.controller2.getSquare()) {
-            servo0.setPosition(L_OPEN);
+        else {
+            return L_OPEN;
         }
-        if (Devices.controller2.getCircle()) {
-            servo1.setPosition(R_OPEN);
+    }
+
+    /**
+     * this method counts how many times Dpad Right has been pressed so that we can use it to open
+     * and close the right servo (right grabber)
+     */
+    private int rightButtonPressedCount() {
+        if (Devices.controller2.getDpadRight() && !dpadPressed3) {
+            counter3 += 1;
+            dpadPressed3 = true;
         }
+        else if (!Devices.controller2.getDpadRight()) {
+            dpadPressed3 = false;
+        }
+        if (counter3 % 2 != 0) {
+            return R_CLOSED;
+        }
+        else {
+            return R_OPEN;
+        }
+    }
+
+    public void loop() {
+        double motorPower = -(Devices.controller2.getRightTrigger() - Devices.controller2.getLeftTrigger()) * 0.75;
+        Devices.motor0.setPower(motorPower);
+        Devices.motor1.setPower(motorPower);
+
+        //the motors must move in opposite directions
+        servo0.setPosition(leftButtonPressedCount()); //this operates the left grabber
+        servo1.setPosition(rightButtonPressedCount()); //this operates the right grabber
         if (Devices.controller2.getLeftBumper()) {
             servo2.setPosition(servo2.getPosition() + 0.5);
             //this will rotate the entire claw mechanism so that it is in line with the backboard