diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index c280bd7..b86eb4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -1,11 +1,19 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utilsv2.*; @TeleOp @@ -13,7 +21,10 @@ import org.firstinspires.ftc.teamcode.utilsv2.*; public class TeleopV4 extends LinearOpMode { Robot robot; Drivetrain drivetrain; + Shooter shooter; MultipleTelemetry TELE; + Follower follower; + SpindexerTransferIntake spindexerTransferIntake; @Override public void runOpMode() throws InterruptedException { @@ -24,6 +35,15 @@ public class TeleopV4 extends LinearOpMode { ); drivetrain = new Drivetrain(robot, TELE); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); + + shooter = new Shooter(robot, TELE, follower, Color.redAlliance); + shooter.setState(Shooter.ShooterState.TRACK_GOAL); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE); + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + waitForStart(); @@ -39,6 +59,41 @@ public class TeleopV4 extends LinearOpMode { gamepad1.left_stick_x ); + shooter.update(); + spindexerTransferIntake.update(); + + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); + + if (gamepad1.xWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { + + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + } + + if (gamepad1.aWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + + if (gamepad1.yWasPressed() + && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.INTAKE + ); + } + + + TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 9c60f8f..068aea2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -45,7 +45,7 @@ public class Shooter { private double flywheelVelocity = 0.0; private double turretPosition = 0.5; - enum ShooterState { + public enum ShooterState { READ_OBELISK, TRACK_GOAL, MANUAL_FLYWHEEL_TRACK_TURR, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index fefb67d..0dc6cb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -51,6 +51,10 @@ public class SpindexerTransferIntake { this.mode = spindexerMode; } + public RapidMode getRapidState(){ + return this.rapidMode; + } + private long stateTime() { return System.currentTimeMillis() - stateStartTime; }