Added Pedro Support, Pedro Drive to Pose, and Megatag Support

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) {
+        for (LynxModule hub : allHubs) hub.clearBulkCache();
-            hub.clearBulkCache();
-        }
-
-
         g1.update();
         g2.update();
 
-        if (voltageTimer.milliseconds() >= Constants.PIDController.VOLTAGE_CHECK_INTERVAL_MS) {
+        follower.update();
-            BaseController.currentSystemVoltage = batterySensor.getVoltage();
+
-            voltageTimer.reset();
+        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();
         }

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();
+    }
+}

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();

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();
     }
 }

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));
+            frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FRONT_LEFT_MOTOR));
-            frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME));
+            frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FRONT_RIGHT_MOTOR));
-            backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME));
+            backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BACK_LEFT_MOTOR));
-            backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME));
+            backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BACK_RIGHT_MOTOR));
             hardwareExists = true;
         } catch (Exception e) {
             hardwareExists = false;
         }
 
-        frontRight.setDirection(DcMotor.Direction.REVERSE);
+        if (hardwareExists) {
-        backRight.setDirection(DcMotor.Direction.REVERSE);
+            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;
     }
 
     @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;
+            flPower /= max;
-            blPower /= max; brPower /= 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());
         }
     }
 }

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");
+    }
+}

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 FRONT_LEFT_MOTOR = "fl";
-        public static final String FR_NAME = "fr";
+        public static final String FRONT_RIGHT_MOTOR = "fr";
-        public static final String BL_NAME = "bl";
+        public static final String BACK_LEFT_MOTOR = "bl";
-        public static final String BR_NAME = "br";
+        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;

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");
+    }
+}

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 abstract class teleOp extends BaseOpMode {
-public 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.addLine("Running TeleOP");
-            unifiedTelemetry.addData("Loaded Alliance", AutoTransfer.getAllianceString());
-            unifiedTelemetry.update();
-        }
     }
 
     @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) {