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

Add localization test

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
View File

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

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
View File

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

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java Normal file
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();
}
}
}

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

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

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
View File

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