From 4ed1c8c615da14b3cc0a86dbeb406f84d0ba9a79 Mon Sep 17 00:00:00 2001 From: Carlos Date: Tue, 17 Sep 2024 17:16:31 -0700 Subject: [PATCH] Tuned localizer for Competition Robot --- .../pedroPathing/localization/PoseUpdater.java | 2 +- .../localizers/ThreeWheelIMULocalizer.java | 18 +++++++++--------- .../localizers/ThreeWheelLocalizer.java | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index ce2a8f2..6bd60f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -69,7 +69,7 @@ public class PoseUpdater { */ public PoseUpdater(HardwareMap hardwareMap) { // TODO: replace the second argument with your preferred localizer - this(hardwareMap, new ThreeWheelLocalizer(hardwareMap)); + this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 9c2effc..e595cf6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -65,8 +65,8 @@ public class ThreeWheelIMULocalizer extends Localizer { private double previousIMUOrientation; private double deltaRadians; private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double FORWARD_TICKS_TO_INCHES = 0.0038;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = -0.0033;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5; public static boolean useIMU = true; @@ -93,12 +93,12 @@ public class ThreeWheelIMULocalizer extends Localizer { imu = hardwareMap.get(IMU.class, IMU); // TODO: replace this with your IMU's orientation - imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP))); + imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.DOWN, RevHubOrientationOnRobot.UsbFacingDirection.LEFT))); // TODO: replace these with your encoder positions - leftEncoderPose = new Pose(-3, 5.7, 0); - rightEncoderPose = new Pose(-3, -5.7, 0); - strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90)); + leftEncoderPose = new Pose(-7.625, 6.19375, 0); + rightEncoderPose = new Pose(-7.625, -6.19375, 0); + strafeEncoderPose = new Pose(7, 1, Math.toRadians(90)); // TODO: replace these with your encoder ports leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); @@ -106,9 +106,9 @@ public class ThreeWheelIMULocalizer extends Localizer { strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); // TODO: reverse any encoders necessary - leftEncoder.setDirection(Encoder.REVERSE); - rightEncoder.setDirection(Encoder.FORWARD); - strafeEncoder.setDirection(Encoder.FORWARD); + //leftEncoder.setDirection(Encoder.REVERSE); + //rightEncoder.setDirection(Encoder.FORWARD); + //strafeEncoder.setDirection(Encoder.FORWARD); setStartPose(setStartPose); timer = new NanoTimer(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index a2416da..b1d5c54 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -92,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer { strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); // TODO: reverse any encoders necessary - leftEncoder.setDirection(Encoder.REVERSE); - rightEncoder.setDirection(Encoder.REVERSE); - strafeEncoder.setDirection(Encoder.FORWARD); + //leftEncoder.setDirection(Encoder.REVERSE); + // rightEncoder.setDirection(Encoder.REVERSE); + //strafeEncoder.setDirection(Encoder.FORWARD); setStartPose(setStartPose); timer = new NanoTimer();