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

Correct velocity in ManualFeedforwardTuner

Browse Source
This commit is contained in:
Ryan Brott
2023-03-05 18:32:16 -08:00
parent 98cbe1e950
commit 47151cbebd
2 changed files with 22 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
View File

@ -37,6 +37,8 @@ final class DriveView {
public final List<RawEncoder> leftEncs, rightEncs, parEncs, perpEncs;
public final List<RawEncoder> forwardEncs;
public final List<Encoder> forwardEncsWrapped;
public final IMU imu;
public final VoltageSensor voltageSensor;
@ -88,24 +90,38 @@ final class DriveView {
throw new AssertionError();
}
forwardEncsWrapped = new ArrayList<>();
if (localizer instanceof TwoDeadWheelLocalizer) {
TwoDeadWheelLocalizer l2 = (TwoDeadWheelLocalizer) localizer;
parEncs = Collections.singletonList(unwrap(l2.par));
perpEncs = Collections.singletonList(unwrap(l2.perp));
leftEncs = Collections.emptyList();
rightEncs = Collections.emptyList();
forwardEncsWrapped.add(l2.par);
forwardEncsWrapped.add(l2.perp);
} else if (localizer instanceof ThreeDeadWheelLocalizer) {
ThreeDeadWheelLocalizer l3 = (ThreeDeadWheelLocalizer) localizer;
parEncs = Arrays.asList(unwrap(l3.par0), unwrap(l3.par1));
perpEncs = Collections.singletonList(unwrap(l3.perp));
leftEncs = Collections.emptyList();
rightEncs = Collections.emptyList();
forwardEncsWrapped.add(l3.par0);
forwardEncsWrapped.add(l3.par1);
forwardEncsWrapped.add(l3.perp);
} else if (localizer instanceof MecanumDrive.DriveLocalizer) {
MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) localizer;
parEncs = Collections.emptyList();
perpEncs = Collections.emptyList();
leftEncs = Arrays.asList(unwrap(dl.leftFront), unwrap(dl.leftRear));
rightEncs = Arrays.asList(unwrap(dl.rightFront), unwrap(dl.rightRear));
forwardEncsWrapped.add(dl.leftFront);
forwardEncsWrapped.add(dl.leftRear);
forwardEncsWrapped.add(dl.rightFront);
forwardEncsWrapped.add(dl.rightRear);
} else if (localizer instanceof TankDrive.DriveLocalizer) {
TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) localizer;
parEncs = Collections.emptyList();
@ -113,10 +129,14 @@ final class DriveView {
leftEncs = new ArrayList<>();
for (Encoder e : dl.leftEncs) {
leftEncs.add(unwrap(e));
forwardEncsWrapped.add(e);
}
rightEncs = new ArrayList<>();
for (Encoder e : dl.rightEncs) {
rightEncs.add(unwrap(e));
forwardEncsWrapped.add(e);
}
} else {
throw new AssertionError();

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

@ -53,8 +53,8 @@ public class ManualFeedforwardTuner extends LinearOpMode {
mode = Mode.DRIVER_MODE;
}
for (int i = 0; i < view.forwardEncs.size(); i++) {
double v = view.forwardEncs.get(i).getPositionAndVelocity().velocity;
for (int i = 0; i < view.forwardEncsWrapped.size(); i++) {
double v = view.forwardEncsWrapped.get(i).getPositionAndVelocity().velocity;
telemetry.addData("v" + i, view.inPerTick * v);
}