Skip to content

Commit

Permalink
Replace rear with back
Browse files Browse the repository at this point in the history
  • Loading branch information
rbrott committed Oct 18, 2023
1 parent 42246e0 commit a6b4dbe
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 16 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -103,20 +103,20 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();

public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftRear, rightRear, rightFront;
public final Encoder leftFront, leftBack, rightBack, rightFront;

private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos;
private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading;

public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
leftRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));

lastLeftFrontPos = leftFront.getPositionAndVelocity().position;
lastLeftRearPos = leftRear.getPositionAndVelocity().position;
lastRightRearPos = rightRear.getPositionAndVelocity().position;
lastLeftBackPos = leftBack.getPositionAndVelocity().position;
lastRightBackPos = rightBack.getPositionAndVelocity().position;
lastRightFrontPos = rightFront.getPositionAndVelocity().position;

lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
Expand All @@ -125,8 +125,8 @@ public DriveLocalizer() {
@Override
public Twist2dDual<Time> update() {
PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();

Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
Expand All @@ -138,12 +138,12 @@ public Twist2dDual<Time> update() {
leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(leftRearPosVel.position - lastLeftRearPos),
leftRearPosVel.velocity,
(leftBackPosVel.position - lastLeftBackPos),
leftBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightRearPosVel.position - lastRightRearPos),
rightRearPosVel.velocity,
(rightBackPosVel.position - lastRightBackPos),
rightBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos),
Expand All @@ -152,8 +152,8 @@ public Twist2dDual<Time> update() {
));

lastLeftFrontPos = leftFrontPosVel.position;
lastLeftRearPos = leftRearPosVel.position;
lastRightRearPos = rightRearPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;

lastHeading = heading;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,9 +60,9 @@ public static void register(OpModeManager manager) {
if (md.localizer instanceof MecanumDrive.DriveLocalizer) {
MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer;
leftEncs.add(dl.leftFront);
leftEncs.add(dl.leftRear);
leftEncs.add(dl.leftBack);
rightEncs.add(dl.rightFront);
rightEncs.add(dl.rightRear);
rightEncs.add(dl.rightBack);
} else if (md.localizer instanceof ThreeDeadWheelLocalizer) {
ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
parEncs.add(dl.par0);
Expand Down

0 comments on commit a6b4dbe

Please sign in to comment.