diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index 2d1c4da..ebfc6a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -10,8 +10,6 @@ public class PedroConstants { /* Robot parameters */ - // Turn localizer - -0.003 - // Robot motor configurations public static final String FRONT_LEFT_MOTOR = "Drive front lt"; @@ -25,24 +23,11 @@ public class PedroConstants { public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; - // Robot IMU configuration - public static final String IMU = "imu"; - - // Robot IMU placement - public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION - = RevHubOrientationOnRobot.LogoFacingDirection.DOWN; - public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION - = RevHubOrientationOnRobot.UsbFacingDirection.LEFT; - - // Robot encoders - public static final String LEFT_ENCODER = "encoder left"; - public static final String RIGHT_ENCODER = "encoder right"; - public static final String BACK_ENCODER = "encoder back"; - // Robot encoder direction - public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD; - public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD; - public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD; + public static final double FRONT_LEFT_MOTOR_ENCODER = Encoder.FORWARD; + public static final double BACK_LEFT_MOTOR_ENCODER = Encoder.FORWARD; + public static final double FRONT_RIGHT_MOTOR_ENCODER = Encoder.FORWARD; + public static final double BACK_RIGHT_MOTOR_ENCODER = Encoder.FORWARD; /* Pedro's parameters 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 fe5b223..ad74928 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 @@ -70,8 +70,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)); + this(hardwareMap, new DriveEncoderLocalizer(hardwareMap)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index 94077a7..e0aa739 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -1,5 +1,13 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_ENCODER; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; @@ -64,16 +72,16 @@ public class DriveEncoderLocalizer extends Localizer { public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { hardwareMap = map; - leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName)); - leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName)); - rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName)); - rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName)); + leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR)); + leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR)); + rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR)); + rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR)); // TODO: reverse any encoders necessary - leftFront.setDirection(Encoder.REVERSE); - rightRear.setDirection(Encoder.REVERSE); - leftRear.setDirection(Encoder.FORWARD); - rightRear.setDirection(Encoder.FORWARD); + leftFront.setDirection(FRONT_LEFT_MOTOR_ENCODER); + rightFront.setDirection(FRONT_RIGHT_MOTOR_ENCODER); + leftRear.setDirection(BACK_LEFT_MOTOR_ENCODER); + rightRear.setDirection(BACK_RIGHT_MOTOR_ENCODER); setStartPose(setStartPose); timer = new NanoTimer();