sus 2+1

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java

@@ -39,8 +39,8 @@ public class Arm {
     public static double armScore = 0.95;
 
     public static double armScore_highMacro = 0.95;
-    public static double wristStart = 0.29;
-    public static double wristScore = 0.5;
+    public static double wristStart = 0.8;
+    public static double wristScore = 0.31;
 
     public static double elbow_mid = 0.5;
     public static double elbow_right = 0.29;
@@ -48,6 +48,8 @@ public class Arm {
 
     public static double wristScore_highMacro = 0.3;
 
+    public double counter;
+
     public enum Position {
         INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT
     }
@@ -56,6 +58,10 @@ public class Arm {
         OPEN, CLOSE
     }
 
+    public enum Elbowpos{
+
+    }
+
     public Arm(HardwareMap hardwareMap) {
         wristServo = hardwareMap.get(Servo.class, "Wrist");
         doorServo = hardwareMap.get(Servo.class, "Door");
@@ -76,6 +82,7 @@ public class Arm {
 
         setWristPos(Position.INTAKE);
         setDoor(DoorPosition.CLOSE);
+        setElbowPos(1);
     }
 
     public void setArmPos(Position pos) {
@@ -104,16 +111,15 @@ public class Arm {
         }
     }
 
-    public void setElbowPos (Position pos){
-        if (pos == Position.INTAKE) {
+    public void setElbowPos (int counter){
+        if (counter == 1) {
             elbowPos = elbow_mid;
-        } else if (pos == Position.SCORE) {
-            wristPos = elbow_mid;
-        } else if (pos == Position.SCORELEFT) {
-            wristPos = elbow_left;
-        } else if (pos == Position.SCORERIGHT) {
-            wristPos = elbow_right;
-
+        } else if (counter == 2) {
+            elbowPos = elbow_mid;
+        } else if (counter == 3) {
+            elbowPos = elbow_left;
+        } else if (counter == 4) {
+            elbowPos = elbow_right;
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java

@@ -50,12 +50,13 @@ public class BlueTeleop extends OpMode {
 
         this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
         this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
-        this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
+        this.servos[2] = this.hardwareMap.get(Servo.class, "Elbow");
         this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
         this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
         this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
         this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
 
+
         controller1 = new Controller(gamepad1);
         controller2 = new Controller(gamepad2);
 
@@ -179,10 +180,14 @@ public class BlueTeleop extends OpMode {
         }
 
 
-        //Elbowpos
-
-
-
+        //ElbowPos
+        if (controller2.getDRight().isPressed()){
+            robot.arm.setElbowPos(4);
+        } else if (controller2.getDLeft().isPressed()) {
+            robot.arm.setElbowPos(3);
+        }else {
+            robot.arm.setElbowPos(1);
+        }
 
 
         // macros
@@ -190,16 +195,28 @@ public class BlueTeleop extends OpMode {
             case (0): // manual mode
 
                 robot.slides.increaseTarget(controller2.getLeftStick().getY());
+
+//                if (robot.intake.getPower() >= 0.01) {
+//                    robot.arm.setDoor(OPEN);
+//                } else if (robot.intake.getPower() <= -0.01) {
+//                    robot.arm.setDoor(OPEN);
+//                } else if (controller2.getLeftBumper().isPressed()) {
+//                    robot.arm.setDoor(Arm.DoorPosition.OPEN);
+//                } else {
+//                    robot.arm.setDoor(CLOSE);
+//                }
+
                 if (robot.intake.getPower() >= 0.01) {
-                    robot.arm.setDoor(OPEN);
+                    robot.intake.wheel_swallow();
                 } else if (robot.intake.getPower() <= -0.01) {
-                    robot.arm.setDoor(OPEN);
+                    robot.intake.wheel_swallow();
                 } else if (controller2.getLeftBumper().isPressed()) {
-                    robot.arm.setDoor(Arm.DoorPosition.OPEN);
+                    robot.intake.wheel_swallow();
                 } else {
-                    robot.arm.setDoor(CLOSE);
+                    robot.intake.wheel_stop();
                 }
 
+                //Start A/B stuff
                 if (controller2.getX().isJustPressed()) {
                     robot.runningMacro = 1;
                 } else if (controller2.getY().isJustPressed()) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java

@@ -188,6 +188,7 @@ public class RedTeleop extends OpMode {
             case (0): // manual mode
 
                 robot.slides.increaseTarget(controller2.getLeftStick().getY());
+
                 if (robot.intake.getPower() >= 0.01) {
                     robot.arm.setDoor(OPEN);
                 } else if (robot.intake.getPower() <= -0.01) {
@@ -197,6 +198,15 @@ public class RedTeleop extends OpMode {
                 } else {
                     robot.arm.setDoor(CLOSE);
                 }
+                //Elbowpos
+                if (controller2.getDRight().isPressed()){
+                    robot.arm.setElbowPos(4);
+                } else if (controller2.getDLeft().isPressed()) {
+                    robot.arm.setElbowPos(3);
+                }else {
+                    robot.arm.setElbowPos(1);
+                }
+
 
                 if (controller2.getX().isJustPressed()) {
                     robot.runningMacro = 1;