diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index a850a84..cb5a03e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -168,7 +168,7 @@ public class Shooter implements Subsystem { public void setTurretMode(String mode){ turretMode = mode;} - public double trackGoal(Pose2d robotPose, Pose2d goalPose){ + public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){ fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -196,7 +196,7 @@ public class Shooter implements Subsystem { // hoodServo.setPosition(hoodAngle); - moveTurret(getTurretPosByDeltaPose(deltaPose)); + moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); return distance; @@ -209,7 +209,7 @@ public class Shooter implements Subsystem { } - public double getTurretPosByDeltaPose (Pose2d dPose){ + public double getTurretPosByDeltaPose (Pose2d dPose, double offset){ double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); @@ -235,7 +235,7 @@ public class Shooter implements Subsystem { telemetry.addData("AtanAngle", aTanAngle); - return (0.30-deltaAngle) ; + return ((0.30-deltaAngle) + offset); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index b9df4cd..0f716b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -47,7 +47,7 @@ public class TeleopV1 extends LinearOpMode { public static double power = 0.0; - public static double pos = 0.5; + public static double pos = 0.54; ToggleButtonReader g1RightBumper; @@ -65,6 +65,10 @@ public class TeleopV1 extends LinearOpMode { ToggleButtonReader g2DpadUp; ToggleButtonReader g2DpadDown; + + ToggleButtonReader g2DpadRight; + + ToggleButtonReader g2DpadLeft; public double g1RightBumperStamp = 0.0; public double g2LeftBumperStamp = 0.0; @@ -79,6 +83,10 @@ public class TeleopV1 extends LinearOpMode { public boolean autotrack = true; + public double offset = 0.0; + + public boolean notShooting = true; + @Override @@ -89,6 +97,7 @@ public class TeleopV1 extends LinearOpMode { + robot = new Robot(hardwareMap); TELE = new MultipleTelemetry( @@ -134,6 +143,16 @@ public class TeleopV1 extends LinearOpMode { g2, GamepadKeys.Button.DPAD_DOWN ); + g2DpadLeft = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_LEFT + ); + + + g2DpadRight = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_RIGHT + ); + + drivetrain = new Drivetrain(robot, TELE, g1); @@ -204,9 +223,21 @@ public class TeleopV1 extends LinearOpMode { pos +=0.02; } - if (gamepad2.dpad_left){ + g2DpadLeft.readValue(); + + g2DpadRight.readValue(); + + if(g2DpadLeft.wasJustPressed()){ + offset -=0.02; + } + + if(g2DpadRight.wasJustPressed()){ + offset +=0.02; + } + + if (gamepad2.right_trigger > 0.5){ pos = shooter.getAngleByDist( - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0)) + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset) ); } @@ -218,18 +249,20 @@ public class TeleopV1 extends LinearOpMode { + + if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0)); + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); } else { autotrack = false; - shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.04); + shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.02); } @@ -244,6 +277,8 @@ public class TeleopV1 extends LinearOpMode { transfer.transferIn(); shooter.setManualPower(1); + notShooting = false; + } if (g2RightBumper.wasJustReleased()){ @@ -260,13 +295,14 @@ public class TeleopV1 extends LinearOpMode { if (g2LeftBumper.wasJustPressed()){ g2LeftBumperStamp = getRuntime(); + notShooting = false; scoreAll = true; } if (scoreAll) { double time = getRuntime() - g2LeftBumperStamp; - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-2, 0, 0)); + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); shooter.setManualPower(1); @@ -335,7 +371,9 @@ public class TeleopV1 extends LinearOpMode { if (g1RightBumper.wasJustPressed()){ - shooter.setManualPower(0); + notShooting = true; + + if (getRuntime() - g1RightBumperStamp < 0.3){ @@ -344,6 +382,13 @@ public class TeleopV1 extends LinearOpMode { intake.toggle(); } + if (intake.getIntakeState()==1){ + shooter.setManualPower(0); + } + + + + spindexer.intake(); transfer.transferOut(); @@ -353,9 +398,14 @@ public class TeleopV1 extends LinearOpMode { } - if (intake.getIntakeState()==1) { + + if (intake.getIntakeState()==1 && notShooting) { + spindexer.intakeShake(getRuntime()); - } else { + + } else + + { if (g2Circle.wasJustPressed()){