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<>(); private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
public class DriveLocalizer implements Localizer { 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; private Rotation2d lastHeading;
public DriveLocalizer() { public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
leftRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
lastLeftFrontPos = leftFront.getPositionAndVelocity().position; lastLeftFrontPos = leftFront.getPositionAndVelocity().position;
lastLeftRearPos = leftRear.getPositionAndVelocity().position; lastLeftBackPos = leftBack.getPositionAndVelocity().position;
lastRightRearPos = rightRear.getPositionAndVelocity().position; lastRightBackPos = rightBack.getPositionAndVelocity().position;
lastRightFrontPos = rightFront.getPositionAndVelocity().position; lastRightFrontPos = rightFront.getPositionAndVelocity().position;
lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
@ -125,8 +125,8 @@ public final class MecanumDrive {
@Override @Override
public Twist2dDual<Time> update() { public Twist2dDual<Time> update() {
PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity(); PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity(); PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
@ -138,12 +138,12 @@ public final class MecanumDrive {
leftFrontPosVel.velocity, leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick), }).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(leftRearPosVel.position - lastLeftRearPos), (leftBackPosVel.position - lastLeftBackPos),
leftRearPosVel.velocity, leftBackPosVel.velocity,
}).times(PARAMS.inPerTick), }).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(rightRearPosVel.position - lastRightRearPos), (rightBackPosVel.position - lastRightBackPos),
rightRearPosVel.velocity, rightBackPosVel.velocity,
}).times(PARAMS.inPerTick), }).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos), (rightFrontPosVel.position - lastRightFrontPos),
@ -152,8 +152,8 @@ public final class MecanumDrive {
)); ));
lastLeftFrontPos = leftFrontPosVel.position; lastLeftFrontPos = leftFrontPosVel.position;
lastLeftRearPos = leftRearPosVel.position; lastLeftBackPos = leftBackPosVel.position;
lastRightRearPos = rightRearPosVel.position; lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position; lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading; 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) { if (md.localizer instanceof MecanumDrive.DriveLocalizer) {
MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer; MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer;
leftEncs.add(dl.leftFront); leftEncs.add(dl.leftFront);
leftEncs.add(dl.leftRear); leftEncs.add(dl.leftBack);
rightEncs.add(dl.rightFront); rightEncs.add(dl.rightFront);
rightEncs.add(dl.rightRear); rightEncs.add(dl.rightBack);
} else if (md.localizer instanceof ThreeDeadWheelLocalizer) { } else if (md.localizer instanceof ThreeDeadWheelLocalizer) {
ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer; ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
parEncs.add(dl.par0); parEncs.add(dl.par0);