Add localization test

Also shorten some longer method names.
This commit is contained in:
Ryan Brott
2023-05-20 22:59:50 -07:00
parent 491bb42123
commit 1ae2be3dcf
7 changed files with 75 additions and 14 deletions

View File

@ -126,7 +126,7 @@ public final class MecanumDrive {
} }
@Override @Override
public Twist2dIncrDual<Time> updateAndGetIncr() { public Twist2dIncrDual<Time> update() {
Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity(); Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity(); Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
@ -255,7 +255,7 @@ public final class MecanumDrive {
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t); Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel(); Twist2d robotVelRobot = updatePoseEstimate();
Twist2dDual<Time> command = new HolonomicController( Twist2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN, AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
@ -336,7 +336,7 @@ public final class MecanumDrive {
Pose2dDual<Time> txWorldTarget = turn.get(t); Pose2dDual<Time> txWorldTarget = turn.get(t);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel(); Twist2d robotVelRobot = updatePoseEstimate();
Twist2dDual<Time> command = new HolonomicController( Twist2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN, AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
@ -375,8 +375,8 @@ public final class MecanumDrive {
} }
} }
private Twist2d updatePoseEstimateAndGetActualVel() { public Twist2d updatePoseEstimate() {
Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr(); Twist2dIncrDual<Time> incr = localizer.update();
pose = pose.plus(incr.value()); pose = pose.plus(incr.value());
poseHistory.add(pose); poseHistory.add(pose);

View File

@ -135,7 +135,7 @@ public final class TankDrive {
} }
@Override @Override
public Twist2dIncrDual<Time> updateAndGetIncr() { public Twist2dIncrDual<Time> update() {
double meanLeftPos = 0.0, meanLeftVel = 0.0; double meanLeftPos = 0.0, meanLeftVel = 0.0;
for (Encoder e : leftEncs) { for (Encoder e : leftEncs) {
Encoder.PositionVelocityPair p = e.getPositionAndVelocity(); Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
@ -265,7 +265,7 @@ public final class TankDrive {
Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3); Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
updatePoseEstimateAndGetActualVel(); updatePoseEstimate();
Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR) Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
.compute(x, txWorldTarget, pose); .compute(x, txWorldTarget, pose);
@ -347,7 +347,7 @@ public final class TankDrive {
Pose2dDual<Time> txWorldTarget = turn.get(t); Pose2dDual<Time> txWorldTarget = turn.get(t);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel(); Twist2d robotVelRobot = updatePoseEstimate();
Twist2dDual<Time> command = new Twist2dDual<>( Twist2dDual<Time> command = new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0, 0), 3), Vector2dDual.constant(new Vector2d(0, 0), 3),
@ -390,8 +390,8 @@ public final class TankDrive {
} }
} }
public Twist2d updatePoseEstimateAndGetActualVel() { public Twist2d updatePoseEstimate() {
Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr(); Twist2dIncrDual<Time> incr = localizer.update();
pose = pose.plus(incr.value()); pose = pose.plus(incr.value());
poseHistory.add(pose); poseHistory.add(pose);

View File

@ -36,7 +36,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
this.inPerTick = inPerTick; this.inPerTick = inPerTick;
} }
public Twist2dIncrDual<Time> updateAndGetIncr() { public Twist2dIncrDual<Time> update() {
Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity(); Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity(); Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity(); Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();

View File

@ -40,7 +40,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
this.inPerTick = inPerTick; this.inPerTick = inPerTick;
} }
public Twist2dIncrDual<Time> updateAndGetIncr() { public Twist2dIncrDual<Time> update() {
Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity(); Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity(); Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));

View File

@ -0,0 +1,60 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
waitForStart();
while (opModeIsActive()) {
drive.setDrivePowers(new Twist2d(
new Vector2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x
),
-gamepad1.right_stick_x
));
drive.updatePoseEstimate();
telemetry.addData("x", drive.pose.trans.x);
telemetry.addData("y", drive.pose.trans.y);
telemetry.addData("heading", drive.pose.rot);
telemetry.update();
}
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
waitForStart();
while (opModeIsActive()) {
drive.setDrivePowers(new Twist2d(
new Vector2d(
-gamepad1.left_stick_y,
0.0
),
-gamepad1.right_stick_x
));
drive.updatePoseEstimate();
telemetry.addData("x", drive.pose.trans.x);
telemetry.addData("y", drive.pose.trans.y);
telemetry.addData("heading", drive.pose.rot);
telemetry.update();
}
} else {
throw new AssertionError();
}
}
}

View File

@ -32,7 +32,8 @@ public final class TuningOpModes {
ManualFeedbackTuner.class, ManualFeedbackTuner.class,
ManualFeedforwardTuner.class, ManualFeedforwardTuner.class,
SplineTest.class, SplineTest.class,
MecanumMotorDirectionDebugger.class MecanumMotorDirectionDebugger.class,
LocalizationTest.class
); );
for (Class<? extends OpMode> o : opModes) { for (Class<? extends OpMode> o : opModes) {

View File

@ -4,5 +4,5 @@ import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual; import com.acmerobotics.roadrunner.Twist2dIncrDual;
public interface Localizer { public interface Localizer {
Twist2dIncrDual<Time> updateAndGetIncr(); Twist2dIncrDual<Time> update();
} }