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 0e79a38..70b8c34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -1,5 +1,9 @@ package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; + public class PedroConstants { /* @@ -12,6 +16,12 @@ public class PedroConstants { public static final String FRONT_RIGHT_MOTOR = "Drive front rt"; public static final String BACK_RIGHT_MOTOR = "Drive back rt"; + // Robot motor direction + public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE; + public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE; + 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"; @@ -20,6 +30,11 @@ public class PedroConstants { 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; + /* Pedro's parameters */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index fe9d153..649c126 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower; +import static org.firstinspires.ftc.teamcode.PedroConstants.*; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; @@ -168,8 +169,10 @@ public class Follower { rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); // TODO: Make sure that this is the direction your motors need to be reversed in. - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION); + leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION); + rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); + rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); 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 e595cf6..1cc8037 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 @@ -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(LEFT_ENCODER_DIRECTION); + rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION); + strafeEncoder.setDirection(BACK_ENCODER_DIRECTION); setStartPose(setStartPose); timer = new NanoTimer();