Pedro tuned for 20403

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

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<DcMotorEx> launcher;
     public Motor<DcMotorEx> 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);
         }

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,69 @@ 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);
+    });
+    // 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(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(
+            new RevHubIMU(
+                new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
+                    RevHubOrientationOnRobot.UsbFacingDirection.UP
+                )
+            )
+        );
+    });
+    public static ForesightConfig foresightConfig = new ForesightConfig(c -> {
+        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(
+                2.5,
+                primaryTranslationalForward
+            )
+        );
+        c.strafeTranslational.set(
+            Controller.piecewise(secondaryTranslationalLateral).put(
+                2.5,
+                primaryTranslationalLateral
+            )
+        );
+
+        c.coast.set(Controller.proportionalFeedforward(0.013777419288337794));
+        c.brake.set(Controller.proportionalFeedforward(0.011710806395087125));
+        // manually set this one
+        c.headingFeedback.set(Controller.proportional(6.24639824160929));
+        c.headingBrakeCoefficients.set(
+            Vector2D.cartesian(0.04756241922901685, 0.00551561844341756)
+        );
+
+        c.linearBrakeCoefficients.set(Matrix.diag(0.05735053908285078, 0.058135013547529826));
+        c.quadraticBrakeCoefficients.set(Matrix.diag(0.0022600197083254797, 0.0012621399803019945));
+
+        c.maxAchievableForwardVelocity.set(71.88837428192382);
+        c.maxAchievableStrafeVelocity.set(62.44050152401113);
+        c.naturalForwardDeceleration.set(27.50243954601577);
+        c.naturalStrafeDeceleration.set(59.156372270294554);
+    });
 }

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

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