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

Move drive encoder reversal before first get

Browse Source 
Changing the direction of an encoder after reading it the first time
will create a potentially massive delta in the first odometry update.
This commit is contained in:
Ryan Brott
2023-12-09 09:17:20 -08:00
parent 01453c24d4
commit 02d8ea7615
2 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -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