From d400db1b966187b968c0a4d85db3f95671c9f1e9 Mon Sep 17 00:00:00 2001 From: Arkm20 Date: Mon, 23 Mar 2026 21:45:54 -0500 Subject: [PATCH] Added Pedro Support, Pedro Drive to Pose, and Megatag Support --- .../ftc/teamcode/lib/BaseOpMode.java | 74 ++++++++++++++----- .../lib/actions/lib/PedroDriveAction.java | 35 +++++++++ .../ftc/teamcode/lib/util/LLUtil.java | 11 ++- .../ftc/teamcode/pedroPathing/Constants.java | 48 +++++++++++- .../ftc/teamcode/subsys/Drivetrain.java | 74 ++++++++++--------- .../ftc/teamcode/teleOp/BlueTeleOp.java | 16 ++++ .../ftc/teamcode/teleOp/Constants.java | 23 +++++- .../ftc/teamcode/teleOp/RedTeleOp.java | 16 ++++ .../ftc/teamcode/teleOp/teleOp.java | 38 ++++++---- 9 files changed, 265 insertions(+), 70 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/BlueTeleOp.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/RedTeleOp.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java index d0a38e6..fcbf7b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java @@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode.lib; import android.os.SystemClock; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.util.ElapsedTime; @@ -10,16 +12,28 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import com.bylazar.telemetry.JoinedTelemetry; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.lib.actions.RoutineManager; import org.firstinspires.ftc.teamcode.lib.pid.BaseController; import org.firstinspires.ftc.teamcode.lib.util.EnhancedGamepad; +import org.firstinspires.ftc.teamcode.lib.util.LLUtil; import org.firstinspires.ftc.teamcode.teleOp.Constants; +import org.firstinspires.ftc.teamcode.util.AutoTransfer; import org.firstinspires.ftc.teamcode.util.FPSCounter; import java.util.List; public abstract class BaseOpMode extends OpMode { + + public enum Alliance { + RED, BLUE + } + + protected final Alliance alliance; + + protected Follower follower; protected final SubsysManager manager = new SubsysManager(); protected final RoutineManager routines = new RoutineManager(); @@ -35,21 +49,39 @@ public abstract class BaseOpMode extends OpMode { protected Telemetry unifiedTelemetry; + protected LLUtil limelight; + protected abstract void setupSubsystems(); protected abstract void stateMachineUpdate(); protected void onInit() {} + public BaseOpMode(Alliance alliance) { + this.alliance = alliance; + } + @Override public final void init() { if (manager.isRobotHealthy()) { - unifiedTelemetry.addData("SYSTEM STATUS", "READY / HEALTHY"); + unifiedTelemetry.addData("SYSTEM STATUS", "OK"); } else { unifiedTelemetry.addData("SYSTEM STATUS", "!!! HARDWARE FAILURE !!!"); manager.health(unifiedTelemetry); } unifiedTelemetry = new JoinedTelemetry(telemetry); + follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap); + + if (AutoTransfer.isAutonRan) { + follower.setStartingPose(AutoTransfer.transferPose); + } else { + if (alliance == Alliance.RED) { + follower.setStartingPose(Constants.GLOBAL.DEFAULT_RED_POSE); + } else { + follower.setStartingPose(Constants.GLOBAL.DEFAULT_BLUE_POSE); + } + } + batterySensor = hardwareMap.voltageSensor.iterator().next(); BaseController.currentSystemVoltage = batterySensor.getVoltage(); @@ -59,6 +91,9 @@ public abstract class BaseOpMode extends OpMode { } + limelight = new LLUtil(hardwareMap); + + g1 = new EnhancedGamepad(gamepad1); g2 = new EnhancedGamepad(gamepad2); @@ -78,31 +113,36 @@ public abstract class BaseOpMode extends OpMode { @Override public final void loop() { fps.startLoop(); - for (LynxModule hub : allHubs) { - hub.clearBulkCache(); - } - - + for (LynxModule hub : allHubs) hub.clearBulkCache(); g1.update(); g2.update(); - if (voltageTimer.milliseconds() >= Constants.PIDController.VOLTAGE_CHECK_INTERVAL_MS) { - BaseController.currentSystemVoltage = batterySensor.getVoltage(); - voltageTimer.reset(); + follower.update(); + + limelight.updateHeading(Math.toDegrees(follower.getPose().getHeading())); + limelight.update(); + + if (Constants.GLOBAL.ENABLE_POSE_HEALING && limelight.hasTarget()) { + Pose3D mt2Pose = limelight.getMegaTagPose(); + if (mt2Pose != null) { + double llX = mt2Pose.getPosition().x * Constants.LIMELIGHT.METERS_TO_INCHES; + double llY = mt2Pose.getPosition().y * Constants.LIMELIGHT.METERS_TO_INCHES; + + Pose currentPose = follower.getPose(); + double healedX = currentPose.getX() + (llX - currentPose.getX()) * Constants.LIMELIGHT.POSE_HEALING_FACTOR; + double healedY = currentPose.getY() + (llY - currentPose.getY()) * Constants.LIMELIGHT.POSE_HEALING_FACTOR; + + follower.setPose(new Pose(healedX, healedY, currentPose.getHeading())); + } } + + routines.updateAll(); stateMachineUpdate(); manager.updateAll(); - routines.updateAll(); if (telemetryTimer.milliseconds() >= Constants.GLOBAL.TELEMETRY_DELAY_MS) { + unifiedTelemetry.addData("Pose", follower.getPose().toString()); manager.publishTelemetryAll(unifiedTelemetry); - - if (Constants.GLOBAL.DEBUG_MODE) { - unifiedTelemetry.addData("System Voltage", BaseController.currentSystemVoltage); - unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime()); - unifiedTelemetry.addData("FPS", fps.getAverageFps()); - } - unifiedTelemetry.update(); telemetryTimer.reset(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java new file mode 100644 index 0000000..b79533b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.lib.actions.lib; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; + +import org.firstinspires.ftc.teamcode.lib.actions.Action; + +public class PedroDriveAction implements Action { + private final Follower follower; + private final Pose targetPose; + private final boolean holdEnd; + + public PedroDriveAction(Follower follower, Pose targetPose, boolean holdEnd) { + this.follower = follower; + this.targetPose = targetPose; + this.holdEnd = holdEnd; + } + + @Override + public void start() { + follower.followPath( + follower.pathBuilder() + .addPath(new BezierLine(follower::getPose, targetPose)) + .setLinearHeadingInterpolation(follower.getPose().getHeading(), targetPose.getHeading()) + .build(), + holdEnd + ); + } + + @Override + public boolean update() { + return !follower.isBusy(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java index daceab9..ca12ed2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.teleOp.Constants; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; public class LLUtil { private Limelight3A limelight; @@ -55,7 +56,6 @@ public class LLUtil { return heightDiff / Math.tan(angleToTargetRad); } - public double getAreaDistance() { // TA Regr if (!hasTarget()) return lastKnownDistance; return (Constants.LIMELIGHT.AREA_COEFF * Math.pow(getTa(), Constants.LIMELIGHT.AREA_EXPONENT)) @@ -65,6 +65,15 @@ public class LLUtil { public void setPipeline(int index) { limelight.pipelineSwitch(index); } + public Pose3D getMegaTagPose() { + if (hasTarget()) { + return lastResult.getBotpose_MT2(); + } + return null; + } + public void updateHeading(double yawDegrees) { + limelight.updateRobotOrientation(yawDegrees); + } public void stop() { limelight.stop(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fa30420..99e204d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,19 +1,65 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.drivetrains.MecanumConstants; import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.localization.Encoder; +import com.pedropathing.ftc.localization.constants.ThreeWheelConstants; +import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants; +import com.pedropathing.ftc.localization.constants.TwoWheelConstants; import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.teleOp.Constants.DRIVE; + public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(12) + .translationalPIDFCoefficients(new PIDFCoefficients(0.26, 0, 0.01, 0.03)) + .headingPIDFCoefficients(new PIDFCoefficients(1.8, 0, 0.01, 0.03)) + .forwardZeroPowerAcceleration(-39.64668514000648) + .lateralZeroPowerAcceleration(-76.57271150137258); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(0.7) + .xVelocity(68.10320673181904) + .yVelocity(57.52038399624941) + .rightFrontMotorName(DRIVE.FRONT_RIGHT_MOTOR) + .rightRearMotorName(DRIVE.BACK_RIGHT_MOTOR) + .leftRearMotorName(DRIVE.BACK_LEFT_MOTOR) + .leftFrontMotorName(DRIVE.FRONT_LEFT_MOTOR) + .leftFrontMotorDirection(DRIVE.FRONT_LEFT_DIRECTION) + .leftRearMotorDirection(DRIVE.BACK_LEFT_DIRECTION) + .rightFrontMotorDirection(DRIVE.FRONT_RIGHT_DIRECTION) + .rightRearMotorDirection(DRIVE.BACK_RIGHT_DIRECTION); public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + + public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants() + .forwardTicksToInches(0.0020041908950982315) + .strafeTicksToInches(0.002004094712407555) + .turnTicksToInches(0.0022824233082645137) + .leftPodY(3.75) + .rightPodY(-3.25) + .strafePodX(-6.25) + .leftEncoder_HardwareMapName("intake") + .rightEncoder_HardwareMapName("fl") + .strafeEncoder_HardwareMapName("fr") + .leftEncoderDirection(Encoder.FORWARD) + .rightEncoderDirection(Encoder.FORWARD) + .strafeEncoderDirection(Encoder.FORWARD); + + public static Follower createFollower(HardwareMap hardwareMap) { return new FollowerBuilder(followerConstants, hardwareMap) .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .threeWheelLocalizer(localizerConstants) .build(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java index 8a62f06..5527ae2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java @@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.subsys; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.pedropathing.follower.Follower; import org.firstinspires.ftc.robotcore.external.Telemetry; - import org.firstinspires.ftc.teamcode.teleOp.Constants; import org.firstinspires.ftc.teamcode.lib.Subsystem; -import org.firstinspires.ftc.teamcode.lib.hardware.CMotor; // Your caching wrapper +import org.firstinspires.ftc.teamcode.lib.hardware.CMotor; public class Drivetrain extends Subsystem { @@ -17,55 +17,45 @@ public class Drivetrain extends Subsystem { } private DriveState currentState = DriveState.NORMAL; - - // Use the Cached Wrapper instead of raw DcMotor private CMotor frontLeft, frontRight, backLeft, backRight; - + private final Follower follower; private boolean hardwareExists = false; private double driveY = 0.0; private double driveX = 0.0; private double driveTurn = 0.0; + public Drivetrain(Follower follower) { + this.follower = follower; + } + @Override public void init(HardwareMap hwMap) { try { - frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME)); - frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME)); - backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME)); - backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME)); + frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FRONT_LEFT_MOTOR)); + frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FRONT_RIGHT_MOTOR)); + backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BACK_LEFT_MOTOR)); + backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BACK_RIGHT_MOTOR)); hardwareExists = true; } catch (Exception e) { hardwareExists = false; } - frontRight.setDirection(DcMotor.Direction.REVERSE); - backRight.setDirection(DcMotor.Direction.REVERSE); + if (hardwareExists) { + frontLeft.setDirection(DcMotor.Direction.REVERSE); + backLeft.setDirection(DcMotor.Direction.REVERSE); - frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - - @Override - public boolean isHealthy() { - return hardwareExists && frontLeft != null && frontRight != null - && backLeft != null && backRight != null; - } - - public void setTargetState(DriveState newState) { - this.currentState = newState; - } - - public void setDriveVectors(double y, double x, double turn) { - this.driveY = y; - this.driveX = x; - this.driveTurn = turn; + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } } @Override public void update() { + if (follower.isBusy()) return; + if (currentState == DriveState.LOCKED) { stopMotors(); return; @@ -85,8 +75,10 @@ public class Drivetrain extends Subsystem { max = Math.max(max, Math.abs(brPower)); if (max > 1.0) { - flPower /= max; frPower /= max; - blPower /= max; brPower /= max; + flPower /= max; + frPower /= max; + blPower /= max; + brPower /= max; } frontLeft.setPower(flPower); @@ -95,6 +87,16 @@ public class Drivetrain extends Subsystem { backRight.setPower(brPower); } + public void setDriveVectors(double y, double x, double turn) { + this.driveY = y; + this.driveX = x; + this.driveTurn = turn; + } + + public void setTargetState(DriveState newState) { + this.currentState = newState; + } + private void stopMotors() { frontLeft.setPower(0); frontRight.setPower(0); @@ -102,11 +104,17 @@ public class Drivetrain extends Subsystem { backRight.setPower(0); } + @Override + public boolean isHealthy() { + return hardwareExists; + } + @Override public void publishTelemetry(Telemetry telemetry) { if (Constants.GLOBAL.DEBUG_MODE) { telemetry.addData("Drive State", currentState.toString()); telemetry.addData("Drive Vectors", String.format("Y:%.2f X:%.2f T:%.2f", driveY, driveX, driveTurn)); + telemetry.addData("Pedro Busy", follower.isBusy()); } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/BlueTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/BlueTeleOp.java new file mode 100644 index 0000000..2893831 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/BlueTeleOp.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.teleOp; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp(name = "Blue TeleOp", group = "Main") +public class BlueTeleOp extends teleOp { + public BlueTeleOp() { + super(Alliance.BLUE); + } + + @Override + protected void onInit() { + super.onInit(); + unifiedTelemetry.addLine("Alliance: BLUE"); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java index 0f31bcc..d8799cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.teleOp; import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.pedropathing.geometry.Pose; @Configurable public class Constants { @@ -11,6 +13,10 @@ public class Constants { public static boolean DEBUG_MODE = true; public static int TARGET_FPS = 0; // No target FPS (UNCAPPED) + public static boolean ENABLE_POSE_HEALING = true; + + public static Pose DEFAULT_RED_POSE = new Pose(72,72,0); + public static Pose DEFAULT_BLUE_POSE = new Pose(0,0,0); } @@ -31,6 +37,8 @@ public class Constants { public static String NAME = "limelight"; public static int POLL_RATE = 100; + public static double METERS_TO_INCHES = 39.3701; + // Trig Distance public static double MOUNT_HEIGHT_IN = 0.0; @@ -41,16 +49,23 @@ public class Constants { public static double AREA_EXPONENT = -0.518935; public static double AREA_OFFSET = 4.0; + public static double POSE_HEALING_FACTOR = 0.05; public static double STALE_DATA_TIMEOUT_SEC = 0.5; } @Configurable public static class DRIVE { // Hardware Names - public static final String FL_NAME = "fl"; - public static final String FR_NAME = "fr"; - public static final String BL_NAME = "bl"; - public static final String BR_NAME = "br"; + public static final String FRONT_LEFT_MOTOR = "fl"; + public static final String FRONT_RIGHT_MOTOR = "fr"; + public static final String BACK_LEFT_MOTOR = "bl"; + public static final String BACK_RIGHT_MOTOR = "br"; + + + public static final DcMotorSimple.Direction FRONT_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction FRONT_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction BACK_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction BACK_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD; // Speed Constants public static double NORMAL_SPEED = 0.6; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/RedTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/RedTeleOp.java new file mode 100644 index 0000000..114a44c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/RedTeleOp.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.teleOp; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp(name = "Red TeleOp", group = "Main") +public class RedTeleOp extends teleOp { + public RedTeleOp() { + super(Alliance.RED); + } + + @Override + protected void onInit() { + super.onInit(); + unifiedTelemetry.addLine("Alliance: RED"); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java index 0042460..0254a97 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java @@ -1,13 +1,14 @@ package org.firstinspires.ftc.teamcode.teleOp; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.pedropathing.geometry.Pose; + import org.firstinspires.ftc.teamcode.lib.BaseOpMode; +import org.firstinspires.ftc.teamcode.lib.actions.Routine; +import org.firstinspires.ftc.teamcode.lib.actions.lib.PedroDriveAction; import org.firstinspires.ftc.teamcode.subsys.Drivetrain; -import org.firstinspires.ftc.teamcode.util.AutoTransfer; -@TeleOp(name = "Basic TeleOp", group = "Main") -public class teleOp extends BaseOpMode { +public abstract class teleOp extends BaseOpMode { public enum GlobalState { IDLE, @@ -17,32 +18,41 @@ public class teleOp extends BaseOpMode { private GlobalState robotState = GlobalState.IDLE; private Drivetrain drive; + public teleOp(Alliance alliance) { + super(alliance); + } + @Override protected void setupSubsystems() { - drive = new Drivetrain(); + drive = new Drivetrain(follower); manager.register(drive); } @Override protected void onInit() { - if (AutoTransfer.hasValidData()) { - unifiedTelemetry.addData("Loaded Alliance", AutoTransfer.getAllianceString()); - unifiedTelemetry.update(); - } + unifiedTelemetry.addLine("Running TeleOP"); } @Override protected void stateMachineUpdate() { if (g1.backWasPressed()) { - routines.cancelAll(); + follower.breakFollowing(); robotState = GlobalState.IDLE; - - drive.setTargetState(Drivetrain.DriveState.NORMAL); - g1.rumbleBlips(2); } + + if (g1.aWasPressed()) { + Pose scorePose = new Pose(72, 72, Math.toRadians(90)); + + routines.run( + Routine.sequence( + new PedroDriveAction(follower, scorePose, true), + Routine.instant(() -> g1.rumble(500)) + ) + ); + } drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_y()); switch (robotState) { @@ -64,4 +74,4 @@ public class teleOp extends BaseOpMode { break; } } -} \ No newline at end of file +}