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/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 fcc6570..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 @@ -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); + }); } 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(); }