Skip to content

Commit

Permalink
Stop reading localizer inputs on construction
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@rbrott
rbrott committed Feb 19, 2024
1 parent ecc49aa commit 9d2cd48
Show file tree
Hide file tree
Showing 11 changed files with 161 additions and 83 deletions.
2 changes: 1 addition & 1 deletion TeamCode/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 25 additions & 12 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,10 @@
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;
Expand Down Expand Up @@ -125,6 +126,7 @@ public class DriveLocalizer implements Localizer {

private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading;
private boolean initialized;

public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
Expand All @@ -134,13 +136,6 @@ public DriveLocalizer() {

// 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
Expand All @@ -150,12 +145,30 @@ public Twist2dDual<Time> update() {
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();

FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel));
YawPitchRollAngles angles = imu.get().getRobotYawPitchRollAngles();

Rotation2d heading = Rotation2d.exp(imu.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double headingDelta = heading.minus(lastHeading);
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)
);
}

double headingDelta = heading.minus(lastHeading);
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos),
Expand Down
29 changes: 16 additions & 13 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
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;
Expand Down Expand Up @@ -129,6 +129,7 @@ public class DriveLocalizer implements Localizer {
public final List<Encoder> leftEncs, rightEncs;

private double lastLeftPos, lastRightPos;
private boolean initialized;

public DriveLocalizer() {
{
Expand All @@ -151,16 +152,6 @@ public DriveLocalizer() {

// 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
Expand All @@ -186,8 +177,20 @@ public Twist2dDual<Time> update() {
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[] {
Expand Down
23 changes: 19 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
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;
Expand All @@ -14,6 +15,8 @@
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 {
Expand All @@ -29,6 +32,7 @@ public static class Params {
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)
Expand All @@ -41,10 +45,6 @@ public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
// 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);
Expand All @@ -55,6 +55,21 @@ public Twist2dDual<Time> update() {
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;
Expand Down
46 changes: 31 additions & 15 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
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;
Expand All @@ -17,6 +18,9 @@
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 {
Expand All @@ -36,6 +40,7 @@ public static class Params {
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)
Expand All @@ -49,36 +54,47 @@ public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick)

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[] {
Expand Down
19 changes: 0 additions & 19 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
View file

This file was deleted.

30 changes: 30 additions & 0 deletions .../src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumLocalizerInputsMessage.java
View file
Original file line number Diff line number Diff line change
@@ -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 changes: 2 additions & 2 deletions ...eamcode/messages/TankEncodersMessage.java → .../messages/TankLocalizerInputsMessage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@

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]);
Expand Down
4 changes: 2 additions & 2 deletions ...ssages/ThreeDeadWheelEncodersMessage.java → ...messages/ThreeDeadWheelInputsMessage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,13 @@

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;
Expand Down
15 changes: 0 additions & 15 deletions ...de/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
View file

This file was deleted.

Loading

0 comments on commit 9d2cd48

Please sign in to comment.