From 308471bf76c5f5538f47814907175bb067fc3b07 Mon Sep 17 00:00:00 2001 From: austinlaplante Date: Tue, 6 Oct 2026 18:28:42 -0700 Subject: [PATCH 1/3] Pedro is tuned. Foresight tests not yet run --- .../ftc/teamcode/pedro/Tuning.java | 5 +- .../teamcode/twenty403/PedroConstants.java | 74 ++++++++++++++++++- 2 files changed, 74 insertions(+), 5 deletions(-) diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java index 5d3804b..106e907 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.teamcode.pedro.procedures.MecanumTuner; import org.firstinspires.ftc.teamcode.pedro.procedures.OctoQuadTuner; import org.firstinspires.ftc.teamcode.pedro.procedures.Tests; +import org.firstinspires.ftc.teamcode.pedro.procedures.TwoWheelTuner; import org.firstinspires.ftc.teamcode.twenty403.PedroConstants; public class Tuning { @@ -20,8 +21,8 @@ public static Procedure mecanumTuner() { } @Tuner - public static Procedure octoquadTuner() { - return new OctoQuadTuner(); + public static Procedure TwoWheelTuner() { + return new TwoWheelTuner(); } @Tuner diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java index fcc6570..0346730 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java @@ -2,11 +2,18 @@ import com.pedropathing.algorithm.Foresight; import com.pedropathing.algorithm.ForesightConfig; +import com.pedropathing.controllers.Controller; import com.pedropathing.follower.Follower; +import com.pedropathing.math.Matrix; +import com.pedropathing.math.Vector2D; import com.pedropathing.revhub.drivetrains.Mecanum; import com.pedropathing.revhub.drivetrains.MecanumConfig; +import com.pedropathing.revhub.localizers.Encoder; +import com.pedropathing.revhub.localizers.RevHubIMU; import com.pedropathing.revhub.localizers.TwoWheelConfig; import com.pedropathing.revhub.localizers.TwoWheelLocalizer; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; public class PedroConstants { @@ -20,7 +27,68 @@ public static Follower create(HardwareMap h) { } // TODO: Run the tuners on the bot, fill these it with the results: - public static MecanumConfig drivetrainConfig = null; - public static TwoWheelConfig localizerConfig = null; - public static ForesightConfig foresightConfig = null; + public static MecanumConfig drivetrainConfig = new MecanumConfig(c -> { + c.frontLeftName.set("fl"); + c.frontRightName.set("fr"); + c.backLeftName.set("rl"); + c.backRightName.set("rr"); + c.frontLeftDirection.set(DcMotorSimple.Direction.REVERSE); + c.frontRightDirection.set(DcMotorSimple.Direction.REVERSE); + c.backLeftDirection.set(DcMotorSimple.Direction.REVERSE); + c.backRightDirection.set(DcMotorSimple.Direction.FORWARD); + }); + public static TwoWheelConfig localizerConfig = new TwoWheelConfig(c -> { + c.xPodName.set("fbodo"); + c.yPodName.set("strafeodo"); + c.imuName.set("imu"); + c.xPodOffset.set(-0.044544361213595896); + c.yPodOffset.set(-6.755979305146669); + c.forwardTicksToInches.set(0.0019973615715189914); + c.strafeTicksToInches.set(0.002071152863611527); + c.xPodDirection.set(Encoder.FORWARD); + c.yPodDirection.set(Encoder.FORWARD); + c.imu.set( + new RevHubIMU( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP + ) + ) + ); + }); + public static ForesightConfig foresightConfig = new ForesightConfig(c -> { + Controller primaryTranslationalForward = Controller.proportional(0.26316799030345517); + Controller secondaryTranslationalForward = Controller.proportional(0.09723354043891597); + Controller primaryTranslationalLateral = Controller.proportional(0.4333549343156143); + Controller secondaryTranslationalLateral = Controller.proportional(0.16011306877251263); + + c.forwardTranslational.set( + Controller.piecewise(secondaryTranslationalForward).put( + 2.5, + primaryTranslationalForward + ) + ); + c.strafeTranslational.set( + Controller.piecewise(secondaryTranslationalLateral).put( + 2.5, + primaryTranslationalLateral + ) + ); + + c.coast.set(Controller.proportionalFeedforward(0.013402344630944559)); + c.brake.set(Controller.proportionalFeedforward(0.011391992936302874)); + + c.headingFeedback.set(Controller.proportional(6.262921484299736)); + c.headingBrakeCoefficients.set( + Vector2D.cartesian(0.059888673975135163, 0.0037857270380500523) + ); + + c.linearBrakeCoefficients.set(Matrix.diag(0.09333249940119288, 0.0595602507219176)); + c.quadraticBrakeCoefficients.set(Matrix.diag(0.0012810241980275724, 0.0016948956191025326)); + + c.maxAchievableForwardVelocity.set(73.25426377980192); + c.maxAchievableStrafeVelocity.set(41.95581906838906); + c.naturalForwardDeceleration.set(34.01811460082023); + c.naturalStrafeDeceleration.set(83.19033430043984); + }); } From 46f45b7c167882c83b9f2539b229760c7a0f9846 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Fri, 9 Oct 2026 16:11:24 -0700 Subject: [PATCH 2/3] Initial Pedro Foresight values --- .../ftc/teamcode/twenty403/Hardware.java | 7 ++++ .../teamcode/twenty403/PedroConstants.java | 37 ++++++++++--------- .../ftc/teamcode/twenty403/Setup.java | 7 ++-- .../twenty403/opmodes/JustDrivingTeleOp.java | 19 +++++++--- 4 files changed, 44 insertions(+), 26 deletions(-) diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java index b1c9c86..3b9dc21 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java @@ -11,6 +11,7 @@ import com.technototes.library.hardware.sensor.AdafruitIMU; import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; +import com.technototes.library.hardware.sensor.encoder.MotorEncoder; import com.technototes.library.logger.Loggable; import java.util.List; import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; @@ -21,6 +22,8 @@ public class Hardware implements Loggable { public EncodedMotor launcher; public Motor inTake; + public MotorEncoder fbOdo; + public MotorEncoder strafeOdo; public Limelight3A limelight; public CRServo transferServo; public CRServo leftIntakeServo; @@ -34,6 +37,10 @@ public Hardware(HardwareMap hwmap) { if (Setup.Connected.DRIVEBASE) { } + if (Setup.Connected.ODO) { + fbOdo = new MotorEncoder(PedroConstants.localizerConfig.xPodName.get()); + strafeOdo = new MotorEncoder(PedroConstants.localizerConfig.yPodName.get()); + } if (Setup.Connected.LAUNCHER) { launcher = new EncodedMotor<>(Setup.HardwareNames.LAUNCHER); } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java index 0346730..2a29ea3 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java @@ -37,14 +37,15 @@ public static Follower create(HardwareMap h) { c.backLeftDirection.set(DcMotorSimple.Direction.REVERSE); c.backRightDirection.set(DcMotorSimple.Direction.FORWARD); }); + // Ran on Oct 9 2026 public static TwoWheelConfig localizerConfig = new TwoWheelConfig(c -> { c.xPodName.set("fbodo"); c.yPodName.set("strafeodo"); c.imuName.set("imu"); - c.xPodOffset.set(-0.044544361213595896); - c.yPodOffset.set(-6.755979305146669); - c.forwardTicksToInches.set(0.0019973615715189914); - c.strafeTicksToInches.set(0.002071152863611527); + c.xPodOffset.set(1.73); + c.yPodOffset.set(2.12); + c.forwardTicksToInches.set(0.00177); + c.strafeTicksToInches.set(0.00204); c.xPodDirection.set(Encoder.FORWARD); c.yPodDirection.set(Encoder.FORWARD); c.imu.set( @@ -57,10 +58,10 @@ public static Follower create(HardwareMap h) { ); }); public static ForesightConfig foresightConfig = new ForesightConfig(c -> { - Controller primaryTranslationalForward = Controller.proportional(0.26316799030345517); - Controller secondaryTranslationalForward = Controller.proportional(0.09723354043891597); - Controller primaryTranslationalLateral = Controller.proportional(0.4333549343156143); - Controller secondaryTranslationalLateral = Controller.proportional(0.16011306877251263); + Controller primaryTranslationalForward = Controller.proportional(0.27971881633553536); + Controller secondaryTranslationalForward = Controller.proportional(0.10334862841155307); + Controller primaryTranslationalLateral = Controller.proportional(0.48144662310086206); + Controller secondaryTranslationalLateral = Controller.proportional(0.17788166274507058); c.forwardTranslational.set( Controller.piecewise(secondaryTranslationalForward).put( @@ -75,20 +76,20 @@ public static Follower create(HardwareMap h) { ) ); - c.coast.set(Controller.proportionalFeedforward(0.013402344630944559)); - c.brake.set(Controller.proportionalFeedforward(0.011391992936302874)); + c.coast.set(Controller.proportionalFeedforward(0.013777419288337794)); + c.brake.set(Controller.proportionalFeedforward(0.011710806395087125)); - c.headingFeedback.set(Controller.proportional(6.262921484299736)); + c.headingFeedback.set(Controller.proportional(14.24639824160929)); c.headingBrakeCoefficients.set( - Vector2D.cartesian(0.059888673975135163, 0.0037857270380500523) + Vector2D.cartesian(0.04756241922901685, 0.00551561844341756) ); - c.linearBrakeCoefficients.set(Matrix.diag(0.09333249940119288, 0.0595602507219176)); - c.quadraticBrakeCoefficients.set(Matrix.diag(0.0012810241980275724, 0.0016948956191025326)); + c.linearBrakeCoefficients.set(Matrix.diag(0.05735053908285078, 0.058135013547529826)); + c.quadraticBrakeCoefficients.set(Matrix.diag(0.0022600197083254797, 0.0012621399803019945)); - c.maxAchievableForwardVelocity.set(73.25426377980192); - c.maxAchievableStrafeVelocity.set(41.95581906838906); - c.naturalForwardDeceleration.set(34.01811460082023); - c.naturalStrafeDeceleration.set(83.19033430043984); + c.maxAchievableForwardVelocity.set(71.88837428192382); + c.maxAchievableStrafeVelocity.set(62.44050152401113); + c.naturalForwardDeceleration.set(27.50243954601577); + c.naturalStrafeDeceleration.set(59.156372270294554); }); } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Setup.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Setup.java index 927f977..e47d404 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Setup.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Setup.java @@ -7,14 +7,15 @@ public class Setup { @Configurable public static class Connected { - public static boolean DRIVEBASE = true; + public static boolean DRIVEBASE = false; public static boolean TRANSFER = false; + public static boolean ODO = true; public static boolean SAFETYSUBSYSTEM = false; - public static boolean EXTERNALIMU = true; + public static boolean EXTERNALIMU = false; public static boolean LAUNCHER = true; public static boolean INTAKE = true; public static boolean LIMELIGHT = false; - public static boolean OTOS = true; + public static boolean OTOS = false; } @Configurable diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/opmodes/JustDrivingTeleOp.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/opmodes/JustDrivingTeleOp.java index 8a4ab73..fbb0fc9 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/opmodes/JustDrivingTeleOp.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/opmodes/JustDrivingTeleOp.java @@ -32,7 +32,7 @@ // unicode is moai emoji @Configurable -@TeleOp(name = "Two Controller Drive \uD83D\uDDFF") +@TeleOp(name = "2 Controller Drive \uD83D\uDDFF") @SuppressWarnings("unused") public class JustDrivingTeleOp extends CommandOpMode { @@ -63,8 +63,10 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.BLUE, StartingPosition.Unspecified); controlsOperator = new OperatorController(codriverGamepad, robot); - SparkFunOTOS otos = hardwareMap.get(SparkFunOTOS.class, Setup.HardwareNames.OTOS); - otos.calibrateImu(); + if (Setup.Connected.OTOS) { + SparkFunOTOS otos = hardwareMap.get(SparkFunOTOS.class, Setup.HardwareNames.OTOS); + otos.calibrateImu(); + } controlsDriver = new DriverController(driverGamepad, robot); if (Setup.Connected.LIMELIGHT) { limelight = hardwareMap.get(Limelight3A.class, LIMELIGHT); @@ -105,8 +107,9 @@ public void uponStart() { public void runLoop() { // telemetry.addData("imu ori", hardware.imu.getHeading(AngleUnit.DEGREES)); // telemetry.update(); - robot.follower.update(); - + if (Setup.Connected.DRIVEBASE) { + robot.follower.update(); + } LLStatus status = null; if (Setup.Connected.LIMELIGHT) { status = limelight.getStatus(); @@ -228,6 +231,12 @@ public void runLoop() { telemetry.addData("Limelight", "No data available"); } } + + // Pose location = robot.follower.pose(); + double fbLoc = hardware.fbOdo.getPosition(); + double strafeLoc = hardware.strafeOdo.getPosition(); + telemetry.addData("FB Odo", fbLoc); + telemetry.addData("Strafe Odo", strafeLoc); panelsTelemetry.update(telemetry); telemetry.update(); } From 8367b9d349d831ecb90c99cfaa5185ce5f591421 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Fri, 9 Oct 2026 16:28:26 -0700 Subject: [PATCH 3/3] Manually overrode the headingFeedback value, because the tuner went a little nuts --- .../firstinspires/ftc/teamcode/twenty403/PedroConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java index 2a29ea3..1a7e569 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/PedroConstants.java @@ -78,8 +78,8 @@ public static Follower create(HardwareMap h) { c.coast.set(Controller.proportionalFeedforward(0.013777419288337794)); c.brake.set(Controller.proportionalFeedforward(0.011710806395087125)); - - c.headingFeedback.set(Controller.proportional(14.24639824160929)); + // manually set this one + c.headingFeedback.set(Controller.proportional(6.24639824160929)); c.headingBrakeCoefficients.set( Vector2D.cartesian(0.04756241922901685, 0.00551561844341756) );