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

Stop reading localizer inputs on construction

Browse Source 
This ensures that any motion between construction and first update
will be ignored (this is usually the period between init and start
of an op mode). If teams want to track pose during that period,
they can call `updatePoseEstimate()` explicitly. This matches the
behavior of the 0.5.x localizers (without the annoying reset on
every pose estimate set).

The localizers also now log much more data to help troubleshoot
localization issues in the future.
This commit is contained in:
Ryan Brott
2024-02-19 14:23:43 -08:00
parent ecc49aa778
commit 9d2cd48287
11 changed files with 161 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/build.gradle
View File

@ -33,6 +33,6 @@ dependencies {
implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation "com.acmerobotics.roadrunner:ftc:0.1.12"
implementation "com.acmerobotics.roadrunner:ftc:0.1.13"
implementation "com.acmerobotics.dashboard:dashboard:0.4.14"
}

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

@ -39,9 +39,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;
import java.lang.Math;
@ -125,6 +126,7 @@ public final class MecanumDrive {
private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading;
private boolean initialized;
public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
@ -134,13 +136,6 @@ public final class MecanumDrive {
// 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.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
}
@Override
@ -150,12 +145,30 @@ public final class MecanumDrive {
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel));
YawPitchRollAngles angles = imu.get().getRobotYawPitchRollAngles();
FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
if (!initialized) {
initialized = true;
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
Rotation2d heading = Rotation2d.exp(imu.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double headingDelta = heading.minus(lastHeading);
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos),

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

@ -48,7 +48,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;
import org.firstinspires.ftc.teamcode.messages.TankCommandMessage;
import org.firstinspires.ftc.teamcode.messages.TankEncodersMessage;
import org.firstinspires.ftc.teamcode.messages.TankLocalizerInputsMessage;
import java.util.ArrayList;
import java.util.Arrays;
@ -129,6 +129,7 @@ public final class TankDrive {
public final List<Encoder> leftEncs, rightEncs;
private double lastLeftPos, lastRightPos;
private boolean initialized;
public DriveLocalizer() {
{
@ -151,16 +152,6 @@ public final class TankDrive {
// 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
@ -186,8 +177,20 @@ public final class TankDrive {
meanRightPos /= rightEncs.size();
meanRightVel /= rightEncs.size();
FlightRecorder.write("TANK_ENCODERS",
new TankEncodersMessage(leftReadings, rightReadings));
FlightRecorder.write("TANK_LOCALIZER_INPUTS",
new TankLocalizerInputsMessage(leftReadings, rightReadings));
if (!initialized) {
initialized = true;
lastLeftPos = meanLeftPos;
lastRightPos = meanRightPos;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
new DualNum<Time>(new double[] {

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

@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
@ -14,6 +15,8 @@ import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.messages.ThreeDeadWheelInputsMessage;
@Config
public final class ThreeDeadWheelLocalizer implements Localizer {
public static class Params {
@ -29,6 +32,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
public final double inPerTick;
private int lastPar0Pos, lastPar1Pos, lastPerpPos;
private boolean initialized;
public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
// TODO: make sure your config has **motors** with these names (or change them)
@ -41,10 +45,6 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
// TODO: reverse encoder directions if needed
// par0.setDirection(DcMotorSimple.Direction.REVERSE);
lastPar0Pos = par0.getPositionAndVelocity().position;
lastPar1Pos = par1.getPositionAndVelocity().position;
lastPerpPos = perp.getPositionAndVelocity().position;
this.inPerTick = inPerTick;
FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
@ -55,6 +55,21 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
FlightRecorder.write("THREE_DEAD_WHEEL_INPUTS", new ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel));
if (!initialized) {
initialized = true;
lastPar0Pos = par0PosVel.position;
lastPar1Pos = par1PosVel.position;
lastPerpPos = perpPosVel.position;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
int par0PosDelta = par0PosVel.position - lastPar0Pos;
int par1PosDelta = par1PosVel.position - lastPar1Pos;
int perpPosDelta = perpPosVel.position - lastPerpPos;

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

@ -5,6 +5,7 @@ import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
@ -17,6 +18,9 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.messages.TwoDeadWheelInputsMessage;
@Config
public final class TwoDeadWheelLocalizer implements Localizer {
@ -36,6 +40,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
private final double inPerTick;
private double lastRawHeadingVel, headingVelOffset;
private boolean initialized;
public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
// TODO: make sure your config has **motors** with these names (or change them)
@ -49,36 +54,47 @@ public final class TwoDeadWheelLocalizer implements Localizer {
this.imu = imu;
lastParPos = par.getPositionAndVelocity().position;
lastPerpPos = perp.getPositionAndVelocity().position;
lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
this.inPerTick = inPerTick;
FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
}
// see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
private double getHeadingVelocity() {
double rawHeadingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
public Twist2dDual<Time> update() {
PositionVelocityPair parPosVel = par.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.RADIANS);
FlightRecorder.write("TWO_DEAD_WHEEL_INPUTS", new TwoDeadWheelInputsMessage(parPosVel, perpPosVel, angles, angularVelocity));
Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
// see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
double rawHeadingVel = angularVelocity.zRotationRate;
if (Math.abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
headingVelOffset -= Math.signum(rawHeadingVel) * 2 * Math.PI;
}
lastRawHeadingVel = rawHeadingVel;
return headingVelOffset + rawHeadingVel;
}
double headingVel = headingVelOffset + rawHeadingVel;
public Twist2dDual<Time> update() {
PositionVelocityPair parPosVel = par.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
if (!initialized) {
initialized = true;
lastParPos = parPosVel.position;
lastPerpPos = perpPosVel.position;
lastHeading = heading;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
int parPosDelta = parPosVel.position - lastParPos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
double headingDelta = heading.minus(lastHeading);
double headingVel = getHeadingVelocity();
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
View File

@ -1,19 +0,0 @@
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
public final class MecanumEncodersMessage {
public long timestamp;
public PositionVelocityPair leftFront;
public PositionVelocityPair leftBack;
public PositionVelocityPair rightBack;
public PositionVelocityPair rightFront;
public MecanumEncodersMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront) {
this.timestamp = System.nanoTime();
this.leftFront = leftFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.rightFront = rightFront;
}
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumLocalizerInputsMessage.java Normal file
View File

@ -0,0 +1,30 @@
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
public final class MecanumLocalizerInputsMessage {
public long timestamp;
public PositionVelocityPair leftFront;
public PositionVelocityPair leftBack;
public PositionVelocityPair rightBack;
public PositionVelocityPair rightFront;
public double yaw;
public double pitch;
public double roll;
public MecanumLocalizerInputsMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront, YawPitchRollAngles angles) {
this.timestamp = System.nanoTime();
this.leftFront = leftFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.rightFront = rightFront;
{
this.yaw = angles.getYaw(AngleUnit.RADIANS);
this.pitch = angles.getPitch(AngleUnit.RADIANS);
this.roll = angles.getRoll(AngleUnit.RADIANS);
}
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankLocalizerInputsMessage.java
View File

@ -4,12 +4,12 @@ import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import java.util.List;
public final class TankEncodersMessage {
public final class TankLocalizerInputsMessage {
public long timestamp;
public PositionVelocityPair[] left;
public PositionVelocityPair[] right;
public TankEncodersMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
public TankLocalizerInputsMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
this.timestamp = System.nanoTime();
this.left = left.toArray(new PositionVelocityPair[0]);
this.right = right.toArray(new PositionVelocityPair[0]);

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelInputsMessage.java
View File

@ -2,13 +2,13 @@ package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
public final class ThreeDeadWheelEncodersMessage {
public final class ThreeDeadWheelInputsMessage {
public long timestamp;
public PositionVelocityPair par0;
public PositionVelocityPair par1;
public PositionVelocityPair perp;
public ThreeDeadWheelEncodersMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
public ThreeDeadWheelInputsMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
this.timestamp = System.nanoTime();
this.par0 = par0;
this.par1 = par1;

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
View File

@ -1,15 +0,0 @@
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
public final class TwoDeadWheelEncodersMessage {
public long timestamp;
public PositionVelocityPair par;
public PositionVelocityPair perp;
public TwoDeadWheelEncodersMessage(PositionVelocityPair par, PositionVelocityPair perp) {
this.timestamp = System.nanoTime();
this.par = par;
this.perp = perp;
}
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelInputsMessage.java Normal file
View File

@ -0,0 +1,35 @@
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
public final class TwoDeadWheelInputsMessage {
public long timestamp;
public PositionVelocityPair par;
public PositionVelocityPair perp;
public double yaw;
public double pitch;
public double roll;
public double xRotationRate;
public double yRotationRate;
public double zRotationRate;
public TwoDeadWheelInputsMessage(PositionVelocityPair par, PositionVelocityPair perp, YawPitchRollAngles angles, AngularVelocity angularVelocity) {
this.timestamp = System.nanoTime();
this.par = par;
this.perp = perp;
{
this.yaw = angles.getYaw(AngleUnit.RADIANS);
this.pitch = angles.getPitch(AngleUnit.RADIANS);
this.roll = angles.getRoll(AngleUnit.RADIANS);
}
{
this.xRotationRate = angularVelocity.xRotationRate;
this.yRotationRate = angularVelocity.yRotationRate;
this.zRotationRate = angularVelocity.zRotationRate;
}
}
}