diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index db06bbf..01dbe5d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -133,15 +133,15 @@ public final class MecanumDrive { rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); + // TODO: reverse encoders if needed + // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + lastLeftFrontPos = leftFront.getPositionAndVelocity().position; lastLeftBackPos = leftBack.getPositionAndVelocity().position; lastRightBackPos = rightBack.getPositionAndVelocity().position; lastRightFrontPos = rightFront.getPositionAndVelocity().position; lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - - // TODO: reverse encoders if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java index ae2b28e..9989d93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java @@ -136,9 +136,7 @@ public final class TankDrive { for (DcMotorEx m : leftMotors) { Encoder e = new OverflowEncoder(new RawEncoder(m)); leftEncs.add(e); - lastLeftPos += e.getPositionAndVelocity().position; } - lastLeftPos /= leftEncs.size(); this.leftEncs = Collections.unmodifiableList(leftEncs); } @@ -147,14 +145,22 @@ public final class TankDrive { for (DcMotorEx m : rightMotors) { Encoder e = new OverflowEncoder(new RawEncoder(m)); rightEncs.add(e); - lastRightPos += e.getPositionAndVelocity().position; } - lastRightPos /= rightEncs.size(); this.rightEncs = Collections.unmodifiableList(rightEncs); } // TODO: reverse encoder directions if needed // leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE); + + for (Encoder e : leftEncs) { + lastLeftPos += e.getPositionAndVelocity().position; + } + lastLeftPos /= leftEncs.size(); + + for (Encoder e : rightEncs) { + lastRightPos += e.getPositionAndVelocity().position; + } + lastRightPos /= rightEncs.size(); } @Override