diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index ea3e619..f0427d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/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; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java index 954c0b0..0168741 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java +++ b/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()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java index be45263..e6b4620 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java +++ b/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;