From 7b5e646de4faee8f81358a2b5bebc8154c8fc502 Mon Sep 17 00:00:00 2001 From: suryatho Date: Sat, 20 Jan 2024 12:37:20 -0500 Subject: [PATCH] Fixed Odomety bug --- src/main/java/frc/robot/RobotContainer.java | 7 +- src/main/java/frc/robot/RobotState.java | 66 ++++--------- .../frc/robot/commands/DriveCommands.java | 3 +- .../frc/robot/subsystems/drive/Drive.java | 95 +++++++++++-------- .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/ModuleIO.java | 1 - .../subsystems/drive/ModuleIOKrakenFOC.java | 4 - .../robot/subsystems/drive/ModuleIOSim.java | 7 +- .../subsystems/drive/ModuleIOSparkMax.java | 3 - .../drive/PhoenixOdometryThread.java | 8 +- .../drive/SparkMaxOdometryThread.java | 13 +-- 11 files changed, 87 insertions(+), 122 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index afb32c6f..448896b7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.commands.FeedForwardCharacterization; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.kitbotshooter.*; +import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; import java.io.File; @@ -125,7 +126,9 @@ public RobotContainer() { Commands.runOnce( () -> robotState.resetPose( - traj.startPose(), drive.getWheelPositions(), drive.getGyroYaw()), + AllianceFlipUtil.apply(traj.startPose()), + drive.getWheelPositions(), + drive.getGyroYaw()), drive), new DriveTrajectory(drive, traj))); }; @@ -166,7 +169,7 @@ private void configureButtonBindings() { .resetPose( new Pose2d( robotState.getEstimatedPose().getTranslation(), - new Rotation2d()), + AllianceFlipUtil.apply(new Rotation2d())), drive.getWheelPositions(), drive.getGyroYaw()), drive) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index b6798421..6dc07959 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -17,10 +17,7 @@ public class RobotState { /** Odometry data from one main robot loop cycle */ public record OdometryObservation( - SwerveDriveWheelPositions[] wheelPositions, - Rotation2d[] gyroAngles, - double[] timestamps, - boolean gyroConnected) {} + SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {} public record VisionObservation(Pose2d visionPose, double timestamp, Matrix stdDevs) {} @@ -52,56 +49,29 @@ public static RobotState getInstance() { private Rotation2d lastGyroAngle = new Rotation2d(); private RobotState() { - odometryStdDevs.setColumn(0, DriveConstants.odometryStateStDevs.extractColumnVector(0)); + odometryStdDevs.setColumn(0, DriveConstants.odometryStateStdDevs.extractColumnVector(0)); kinematics = DriveConstants.kinematics; } public void addOdometryData(OdometryObservation observation) { Pose2d lastOdometryPose = odometryPose; - int minDelta = observation.wheelPositions().length; - if (observation.gyroConnected) minDelta = Math.min(minDelta, observation.gyroAngles().length); - for (int deltaIndex = 0; deltaIndex < minDelta; deltaIndex++) { - // double[] turnPositions = new double[4]; - // double[] lastTurnPositions = new double[4]; - // double[] turnPositionDeltas = new double[4]; - // for (int i = 0; i < 4; i++) { - // turnPositions[i] = - // observation.wheelPositions()[deltaIndex].positions[i].angle.getRadians(); - // lastTurnPositions[i] = lastWheelPositions.positions[i].angle.getRadians(); - // turnPositionDeltas[i] = - // observation - // .wheelPositions()[deltaIndex] - // .positions[i] - // .angle - // .minus(lastWheelPositions.positions[i].angle) - // .getRadians(); - // } - // Logger.recordOutput("Odometry/TurnPositions", turnPositions); - // Logger.recordOutput("Odometry/LastTurnPositions", lastTurnPositions); - // Logger.recordOutput("Odometry/TurnPositionDeltas", turnPositionDeltas); - Twist2d twist = - kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()[deltaIndex]); - lastWheelPositions = observation.wheelPositions()[deltaIndex]; - // Set gyro angle to null if gyro not connected (sim) - if (observation.gyroConnected()) { - // Update dtheta for twist if gyro connected - twist = - new Twist2d( - twist.dx, - twist.dy, - observation.gyroAngles()[deltaIndex].minus(lastGyroAngle).getRadians()); - lastGyroAngle = observation.gyroAngles()[deltaIndex]; - } - // Add twist to odometry pose - odometryPose = odometryPose.exp(twist); - // Add pose to buffer at timestamp - poseBuffer.addSample(observation.timestamps()[deltaIndex], odometryPose); - - // Calculate diff from last odom pose and add onto pose estimate - estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose)); - // Set last odometry pose to current - lastOdometryPose = odometryPose; + // take minimum + Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()); + lastWheelPositions = observation.wheelPositions(); + // Check gyro connected + if (observation.gyroAngle != null) { + // Update dtheta for twist if gyro connected + twist = + new Twist2d( + twist.dx, twist.dy, observation.gyroAngle().minus(lastGyroAngle).getRadians()); + lastGyroAngle = observation.gyroAngle(); } + // Add twist to odometry pose + odometryPose = odometryPose.exp(twist); + // Add pose to buffer at timestamp + poseBuffer.addSample(observation.timestamp(), odometryPose); + // Calculate diff from last odometry pose and add onto pose estimate + estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose)); } public void addVisionObservation(VisionObservation observation) { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index adfe80cc..204cc32f 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -21,6 +21,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.RobotState; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import java.util.function.DoubleSupplier; @@ -64,7 +65,7 @@ public static Command joystickDrive( linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(), linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(), omega * DriveConstants.drivetrainConfig.maxLinearVelocity(), - drive.getGyroYaw())); + RobotState.getInstance().getEstimatedPose().getRotation())); }, drive); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 77ce0dd4..dd61ae77 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,24 +13,36 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import java.util.*; +import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.stream.IntStream; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); + public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); + private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final ModuleIO[] modules = new ModuleIO[4]; // FL, FR, BL, BR - private final ModuleIOInputsAutoLogged[] moduleInputs = new ModuleIOInputsAutoLogged[4]; + private final ModuleIOInputsAutoLogged[] moduleInputs = + new ModuleIOInputsAutoLogged[] { + new ModuleIOInputsAutoLogged(), + new ModuleIOInputsAutoLogged(), + new ModuleIOInputsAutoLogged(), + new ModuleIOInputsAutoLogged() + }; private ChassisSpeeds robotVelocity = new ChassisSpeeds(); private ChassisSpeeds fieldVelocity = new ChassisSpeeds(); @@ -45,62 +57,63 @@ public Drive( modules[1] = frModuleIO; modules[2] = blModuleIO; modules[3] = brModuleIO; - Arrays.fill(moduleInputs, new ModuleIOInputsAutoLogged()); } public void periodic() { odometryLock.lock(); + double[] timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); + // Sim will not have any timstamps + if (timestamps.length == 0) { + timestamps = new double[] {Timer.getFPGATimestamp()}; + } + timestampQueue.clear(); gyroIO.updateInputs(gyroInputs); for (int i = 0; i < modules.length; i++) { modules[i].updateInputs(moduleInputs[i]); } odometryLock.unlock(); + Logger.processInputs("Drive/Gyro", gyroInputs); for (int i = 0; i < modules.length; i++) { Logger.processInputs("Drive/Module" + i, moduleInputs[i]); modules[i].periodic(); } + // Calculate the min odometry position updates across all modules + int minOdometryUpdates = + IntStream.of( + timestamps.length, + Arrays.stream(moduleInputs) + .mapToInt( + moduleInput -> + Math.min( + moduleInput.odometryDrivePositionsMeters.length, + moduleInput.odometryTurnPositions.length)) + .min() + .orElse(0)) + .min() + .orElse(0); + if (gyroInputs.connected) { + minOdometryUpdates = Math.min(gyroInputs.odometryYawPositions.length, minOdometryUpdates); + } // Pass odometry data to robot state - // Calculate the min odometry position count across all modules - OptionalInt minOdometryPositionsCount = - Arrays.stream(moduleInputs) - .mapToInt( - moduleInput -> - Math.min( - moduleInput.odometryDrivePositionsMeters.length, - moduleInput.odometryTurnPositions.length)) - .min(); - // Add observation to robot state if a minimum odometry positions count is found - minOdometryPositionsCount.ifPresent( - minCount -> { - SwerveDriveWheelPositions[] wheelPositions = new SwerveDriveWheelPositions[minCount]; - for (int i = 0; i < minCount; i++) { - int odometryPositionsIndex = i; - // Get all four swerve module positions at that odometry positions count - wheelPositions[i] = - new SwerveDriveWheelPositions( - Arrays.stream(moduleInputs) - .map( - moduleInput -> - new SwerveModulePosition( - moduleInput - .odometryDrivePositionsMeters[odometryPositionsIndex], - moduleInput.odometryTurnPositions[odometryPositionsIndex])) - .toArray(SwerveModulePosition[]::new)); - } - double[] timestamps = new double[minCount]; - // Since timestamps are all synced we can use the timestamps from any module - System.arraycopy(moduleInputs[0].odometryTimestamps, 0, timestamps, 0, minCount); - // Add observation to robot state - RobotState.getInstance() - .addOdometryData( - new RobotState.OdometryObservation( - wheelPositions, - gyroInputs.odometryYawPositions, - timestamps, - gyroInputs.connected)); - }); + for (int i = 0; i < minOdometryUpdates; i++) { + int odometryIndex = i; + Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null; + // Get all four swerve module positions at that odometry update + // and store in SwerveDriveWheelPositions object + SwerveDriveWheelPositions wheelPositions = + new SwerveDriveWheelPositions( + Arrays.stream(moduleInputs) + .map( + moduleInput -> + new SwerveModulePosition( + moduleInput.odometryDrivePositionsMeters[odometryIndex], + moduleInput.odometryTurnPositions[odometryIndex])) + .toArray(SwerveModulePosition[]::new)); + RobotState.getInstance() + .addOdometryData(new RobotState.OdometryObservation(wheelPositions, yaw, timestamps[i])); + } // Stop moving when disabled if (DriverStation.isDisabled()) { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index eb0f5cf2..a212f4f7 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -33,7 +33,7 @@ public final class DriveConstants { case COMPBOT -> 250.0; }; - public static final Matrix odometryStateStDevs = + public static final Matrix odometryStateStdDevs = switch (Constants.getRobot()) { default -> new Matrix<>(VecBuilder.fill(0.1, 0.1, 0.1)); }; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index da0e9a6d..b165ab1b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -34,7 +34,6 @@ class ModuleIOInputs { public double[] odometryDrivePositionsMeters = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - public double[] odometryTimestamps = new double[] {}; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java index 32e2ad71..8d38da01 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -53,7 +53,6 @@ public class ModuleIOKrakenFOC implements ModuleIO { private final boolean isTurnMotorInverted = true; // queues - private final Queue timestampsQueue; private final Queue drivePositionQueue; private final Queue turnPositionQueue; @@ -139,7 +138,6 @@ public ModuleIOKrakenFOC(ModuleConfig config) { turnCurrent = turnTalon.getStatorCurrent(); // 250hz signals - timestampsQueue = PhoenixOdometryThread.getInstance().getTimestampQueue(); drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); turnPositionQueue = @@ -199,10 +197,8 @@ public void updateInputs(ModuleIOInputs inputs) { turnPositionQueue.stream() .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction())) .toArray(Rotation2d[]::new); - inputs.odometryTimestamps = timestampsQueue.stream().mapToDouble(d -> d).toArray(); drivePositionQueue.clear(); turnPositionQueue.clear(); - timestampsQueue.clear(); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index dffe02c2..6186756f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -20,7 +20,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; /** @@ -69,15 +68,15 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; inputs.odometryDrivePositionsMeters = new double[] {driveSim.getAngularPositionRad() * wheelRadius}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryTurnPositions = + new Rotation2d[] {Rotation2d.fromRadians(turnSim.getAngularPositionRad())}; } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index c0d4f29b..e093ce4e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -43,7 +43,6 @@ public class ModuleIOSparkMax implements ModuleIO { // Queues private final Queue drivePositionQueue; private final Queue turnPositionQueue; - private final Queue timestampQueue; private final Rotation2d absoluteEncoderOffset; private int currentIteration = reSeedIterations; @@ -117,7 +116,6 @@ public ModuleIOSparkMax(ModuleConfig config) { turnPositionQueue = SparkMaxOdometryThread.getInstance() .registerSignal(() -> absoluteEncoderValue.get().getRadians()); - timestampQueue = SparkMaxOdometryThread.getInstance().getTimestampQueue(); driveFeedforward = new SimpleMotorFeedforward(moduleConstants.ffKs(), moduleConstants.ffKv()); driveFeedback = new PIDController(moduleConstants.driveKp(), 0.0, moduleConstants.drivekD()); @@ -142,7 +140,6 @@ public void updateInputs(ModuleIOInputs inputs) { drivePositionQueue.stream().mapToDouble(rads -> rads * wheelRadius).toArray(); inputs.odometryTurnPositions = turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); - inputs.odometryTimestamps = timestampQueue.stream().mapToDouble(d -> d).toArray(); drivePositionQueue.clear(); turnPositionQueue.clear(); } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 36c7d511..231829d0 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -35,7 +35,6 @@ public class PhoenixOdometryThread extends Thread { new ReentrantLock(); // Prevents conflicts when registering signals private BaseStatusSignal[] signals = new BaseStatusSignal[0]; private final List> queues = new ArrayList<>(); - private final Queue timestampQueue; private boolean isCANFD = false; private static PhoenixOdometryThread instance = null; @@ -50,7 +49,6 @@ public static PhoenixOdometryThread getInstance() { private PhoenixOdometryThread() { setName("PhoenixOdometryThread"); setDaemon(true); - timestampQueue = new ArrayDeque<>(100); start(); } @@ -97,14 +95,10 @@ public void run() { for (int i = 0; i < signals.length; i++) { queues.get(i).offer(signals[i].getValueAsDouble()); } - timestampQueue.offer(fpgaTimestamp); + Drive.timestampQueue.offer(fpgaTimestamp); } finally { Drive.odometryLock.unlock(); } } } - - public Queue getTimestampQueue() { - return timestampQueue; - } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index a3ea866a..eaaed519 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -15,10 +15,10 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.Timer; -import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; import java.util.function.DoubleSupplier; /** @@ -30,8 +30,6 @@ public class SparkMaxOdometryThread { private List signals = new ArrayList<>(); private List> queues = new ArrayList<>(); - private Queue timestampQueue; - private final Notifier notifier; private static SparkMaxOdometryThread instance = null; @@ -45,12 +43,11 @@ public static SparkMaxOdometryThread getInstance() { private SparkMaxOdometryThread() { notifier = new Notifier(this::periodic); notifier.setName("SparkMaxOdometryThread"); - timestampQueue = new ArrayDeque<>(100); notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); } public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayDeque<>(100); + Queue queue = new ArrayBlockingQueue<>(100); Drive.odometryLock.lock(); try { signals.add(signal); @@ -63,17 +60,13 @@ public Queue registerSignal(DoubleSupplier signal) { private void periodic() { Drive.odometryLock.lock(); + Drive.timestampQueue.offer(Timer.getFPGATimestamp()); try { for (int i = 0; i < signals.size(); i++) { queues.get(i).offer(signals.get(i).getAsDouble()); } - timestampQueue.offer(Timer.getFPGATimestamp()); } finally { Drive.odometryLock.unlock(); } } - - public Queue getTimestampQueue() { - return timestampQueue; - } }