SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Replace rear with back

Browse Source
This commit is contained in:
Ryan Brott
2023-10-17 22:48:23 -07:00
parent 42246e0296
commit a6b4dbebc4
2 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -103,20 +103,20 @@ public final class MecanumDrive {
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));
@ -125,8 +125,8 @@ public final class MecanumDrive {
@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));
@ -138,12 +138,12 @@ public final class MecanumDrive {
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),
@ -152,8 +152,8 @@ public final class MecanumDrive {
));
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftRearPos = leftRearPosVel.position;
lastRightRearPos = rightRearPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
View File

@ -60,9 +60,9 @@ public final class TuningOpModes {
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);