diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index cc7cad7..45f3215 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -271,42 +271,6 @@ public class TeleopV2 extends LinearOpMode { robot.hood.setPosition(hood); - //TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION - - //TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION - - //SHOOT ALL: - - if (gamepad2.rightBumperWasPressed()) { - shootAll = true; - } - - if (shootAll) { - intake = false; - reject = false; - - - - - if (shootA) { - shootA = shootTeleop(spindexer_outtakeBall3); - TELE.addData("shootA", shootA); - } else if (shootB) { - shootB = shootTeleop(spindexer_outtakeBall2); - TELE.addData("shootB", shootB); - } else if (shootC) { - shootC = shootTeleop(spindexer_outtakeBall1); - TELE.addData("shootC", shootC); - - } else { - robot.spin1.setPosition(spindexer_intakePos1); - robot.spin2.setPosition(1 - spindexer_intakePos1); - shootAll = false; - } - - - } - //TURRET: double offset; @@ -320,6 +284,8 @@ public class TeleopV2 extends LinearOpMode { double dx = goalX - robotX; // delta x from robot to goal double dy = goalY - robotY; // delta y from robot to goal + double distanceToGoal = Math.sqrt (dx*dx +dy*dy); + desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; @@ -350,6 +316,55 @@ public class TeleopV2 extends LinearOpMode { robot.turr1.setPosition(pos); robot.turr2.setPosition(1 - pos); + if (autoVel){ + vel = 2000 + distanceToGoal * 12; + } else { + + } + + + + + + + //TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION + + //SHOOT ALL: + + if (gamepad2.rightBumperWasPressed()) { + shootAll = true; + } + + if (shootAll) { + intake = false; + reject = false; + + + + + if (shootA) { + shootA = shootTeleop(spindexer_outtakeBall3); + TELE.addData("shootA", shootA); + } else if (shootB) { + shootB = shootTeleop(spindexer_outtakeBall2); + TELE.addData("shootB", shootB); + } else if (shootC) { + shootC = shootTeleop(spindexer_outtakeBall1); + TELE.addData("shootC", shootC); + + } else { + robot.spin1.setPosition(spindexer_intakePos1); + robot.spin2.setPosition(1 - spindexer_intakePos1); + shootA = true; + shootB = true; + shootC = true; + shootAll = false; + } + + + } + + //SPINDEXER: if (gamepad2.squareWasPressed()){ @@ -375,6 +390,8 @@ public class TeleopV2 extends LinearOpMode { TELE.addData("pose", drive.localizer.getPose()); TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); + TELE.addData("distanceToGoal", distanceToGoal); + TELE.update(); @@ -460,4 +477,5 @@ public class TeleopV2 extends LinearOpMode { + }