Arkm/ftc-lib
Template
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Arkm20
2026-03-23 21:45:54 -05:00
parent 32feae48c0
commit d400db1b96
9 changed files with 265 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java
View File

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

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java Normal file
View File

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

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java
View File

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

48
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
View File

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

74
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java
View File

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

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/BlueTeleOp.java Normal file
View File

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

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java
View File

@@ -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;

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/RedTeleOp.java Normal file
View File

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

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java
View File

@@ -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;
}
}
}
}