diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java index 014d8ce3..95ff602b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java @@ -14,6 +14,8 @@ @com.qualcomm.robotcore.eventloop.opmode.TeleOp public class TeleOp extends LinearOpMode { DcMotorEx barHangMotor; + DcMotorEx flipperMotor; + @Override public void runOpMode() throws InterruptedException { GlobalOpMode.opMode = this; @@ -34,8 +36,13 @@ public void runOpMode() throws InterruptedException { // cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0)); // cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(-70), Math.toRadians(45)); + flipperMotor = hardwareMap.get( DcMotorEx.class,"flipper"); + flipperMotor.setTargetPosition(flipperMotor.getCurrentPosition()); + flipperMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + waitForStart(); + while (!isStopRequested()) { drive.update(); // MUST be called every loop cycle so that RoadRunner calculates the pose correctly // Read pose @@ -63,8 +70,13 @@ public void runOpMode() throws InterruptedException { if (gamepad1.b) { // Move pixel hand servo to release 1 pixel // TODO: Do we need to debounce and count presses? Svit mentioned a "pixel cassette" that drops multiple pixels + throwPixel(); } else { // Close pixel hand servo + //resetFlipper(); + } + if (gamepad1.a) { + resetFlipper(); } if (gamepad1.right_bumper && gamepad1.left_bumper && getRuntime() >= 85) { // technically endgame is 90sec, we let them launch a little early just in case // Launch the airplane @@ -91,5 +103,31 @@ void barHang() { barHangMotor.setTargetPosition(538); barHangMotor.setPower(0.2); + } -} \ No newline at end of file + int startFlipperTicks=0; + + void throwPixel() { + int flipperTicks = flipperMotor.getCurrentPosition(); + if (startFlipperTicks==0) { + startFlipperTicks=flipperTicks; + } + flipperMotor.setTargetPosition(flipperTicks-475); + flipperMotor.setPower(0.1); + //flipperMotor. + telemetry.addData("flipper ticks", flipperTicks); + telemetry.update(); + + } + void resetFlipper() { + flipperMotor.setTargetPosition(startFlipperTicks); + } + //alelluia deus omnipotens confitor deo omnipotenti + + //in nomine patris, et fili, et spiritu sancti + + + +} + +