From cd46b92a3c9db0369f3c1f26dd9d6a937b0f47bd Mon Sep 17 00:00:00 2001 From: James Ding Date: Tue, 5 Mar 2024 09:15:24 -0800 Subject: [PATCH] Migrate gyroscope to ADIS --- src/main/java/org/rambots/Robot.kt | 2 +- src/main/java/org/rambots/RobotContainer.kt | 32 +++--- .../org/rambots/subsystems/drive/Drive.java | 2 +- .../{GyroIONavX2.java => GyroIO_ADIS.java} | 19 ++-- .../drive/PhoenixOdometryThread.java | 63 ++++++++--- .../drive/SparkMaxOdometryThread.java | 107 ------------------ 6 files changed, 72 insertions(+), 153 deletions(-) rename src/main/java/org/rambots/subsystems/drive/{GyroIONavX2.java => GyroIO_ADIS.java} (72%) delete mode 100644 src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java diff --git a/src/main/java/org/rambots/Robot.kt b/src/main/java/org/rambots/Robot.kt index c0b3d31..76539f0 100644 --- a/src/main/java/org/rambots/Robot.kt +++ b/src/main/java/org/rambots/Robot.kt @@ -52,7 +52,7 @@ object Robot : LoggedRobot() { when (Constants.getMode()) { Constants.Mode.REAL -> { - Logger.addDataReceiver(WPILOGWriter()) +// Logger.addDataReceiver(WPILOGWriter()) Logger.addDataReceiver(NT4Publisher()) } diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index d77b3a5..8f8fb8a 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -21,19 +21,13 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.networktables.LoggedDashboardChooser import org.rambots.commands.DriveCommands import org.rambots.commands.DriveToPoint -import org.rambots.commands.MultiDistanceShot import org.rambots.commands.PathFinderAndFollow import org.rambots.subsystems.drive.* import org.rambots.subsystems.drive.DriveConstants.moduleConfigs -import org.rambots.subsystems.flywheel.Flywheel -import org.rambots.subsystems.flywheel.FlywheelIO -import org.rambots.subsystems.flywheel.FlywheelIOSim -import org.rambots.subsystems.flywheel.FlywheelIOTalonFX import org.rambots.subsystems.vision.AprilTagVision import org.rambots.subsystems.vision.AprilTagVisionIO import org.rambots.subsystems.vision.AprilTagVisionIOLimelight import org.rambots.subsystems.vision.AprilTagVisionIOPhotonVisionSIM -import org.rambots.util.FieldConstants import org.rambots.util.VisionHelpers.TimestampedVisionUpdate import java.util.function.Consumer @@ -47,7 +41,7 @@ import java.util.function.Consumer object RobotContainer { // Subsystems private var drive: Drive - private var flywheel: Flywheel +// private var flywheel: Flywheel private var aprilTagVision: AprilTagVision private val driveMode = DriveController() @@ -65,7 +59,7 @@ object RobotContainer { Constants.Mode.REAL -> { // Real robot, instantiate hardware IO implementations drive = Drive( - GyroIONavX2(), + GyroIO_ADIS(), ModuleIOTalonFX(moduleConfigs[0]), ModuleIOTalonFX(moduleConfigs[1]), ModuleIOTalonFX(moduleConfigs[2]), @@ -79,7 +73,7 @@ object RobotContainer { // new ModuleIOTalonFX(1), // new ModuleIOTalonFX(2), // new ModuleIOTalonFX(3)); - flywheel = Flywheel(FlywheelIOTalonFX()) +// flywheel = Flywheel(FlywheelIOTalonFX()) aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight")) } @@ -93,7 +87,7 @@ object RobotContainer { ModuleIOSim(), ModuleIOSim() ) - flywheel = Flywheel(FlywheelIOSim()) +// flywheel = Flywheel(FlywheelIOSim()) aprilTagVision = AprilTagVision( AprilTagVisionIOPhotonVisionSIM( @@ -111,7 +105,7 @@ object RobotContainer { object : ModuleIO {}, object : ModuleIO {}, object : ModuleIO {}) - flywheel = Flywheel(object : FlywheelIO {}) +// flywheel = Flywheel(object : FlywheelIO {}) aprilTagVision = AprilTagVision(object : AprilTagVisionIO {}) } } @@ -202,14 +196,14 @@ object RobotContainer { drive, Pose2d(Translation2d(2.954, 3.621), Rotation2d.fromRadians(2.617)) ) ) - - controller - .povUp() - .whileTrue( - MultiDistanceShot( - { drive!!.pose }, FieldConstants.Speaker.centerSpeakerOpening, flywheel - ) - ) +// +// controller +// .povUp() +// .whileTrue( +// MultiDistanceShot( +// { drive!!.pose }, FieldConstants.Speaker.centerSpeakerOpening, flywheel +// ) +// ) // controller // .b() diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java index d77e751..a544f1f 100644 --- a/src/main/java/org/rambots/subsystems/drive/Drive.java +++ b/src/main/java/org/rambots/subsystems/drive/Drive.java @@ -43,6 +43,7 @@ import org.rambots.util.VisionHelpers; import java.util.List; +import java.util.Queue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -97,7 +98,6 @@ public Drive( // Start threads (no-op for each if no signals have been created) PhoenixOdometryThread.getInstance().start(); - SparkMaxOdometryThread.getInstance().start(); // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( diff --git a/src/main/java/org/rambots/subsystems/drive/GyroIONavX2.java b/src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java similarity index 72% rename from src/main/java/org/rambots/subsystems/drive/GyroIONavX2.java rename to src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java index 3456e77..7f6977e 100644 --- a/src/main/java/org/rambots/subsystems/drive/GyroIONavX2.java +++ b/src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java @@ -1,29 +1,26 @@ package org.rambots.subsystems.drive; -import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.ADIS16470_IMU; import java.util.OptionalDouble; import java.util.Queue; -public class GyroIONavX2 implements GyroIO { - private final AHRS navx = new AHRS(SPI.Port.kMXP); +public class GyroIO_ADIS implements GyroIO { + private final ADIS16470_IMU navx = new ADIS16470_IMU(); private final Queue yawPositionQueue; - public GyroIONavX2() { + public GyroIO_ADIS() { navx.reset(); - navx.resetDisplacement(); - navx.zeroYaw(); yawPositionQueue = - SparkMaxOdometryThread.getInstance() + PhoenixOdometryThread.getInstance() .registerSignal( () -> { boolean valid = navx.isConnected(); if (valid) { - return OptionalDouble.of(navx.getYaw()); + return OptionalDouble.of(navx.getAngle()); } else { return OptionalDouble.empty(); } @@ -33,8 +30,8 @@ public GyroIONavX2() { @Override public void updateInputs(GyroIOInputs inputs) { inputs.connected = navx.isConnected(); - inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRawGyroZ()); + inputs.yawPosition = Rotation2d.fromDegrees(navx.getAngle()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate()); inputs.odometryYawPositions = yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); diff --git a/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java index 1333ae2..600b009 100644 --- a/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java @@ -21,10 +21,12 @@ import java.util.ArrayList; import java.util.List; +import java.util.OptionalDouble; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Supplier; import static org.rambots.subsystems.drive.DriveConstants.odometryFrequency; @@ -42,7 +44,9 @@ public class PhoenixOdometryThread extends Thread { new ReentrantLock(); // Prevents conflicts when registering signals private final List> queues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private BaseStatusSignal[] ctreSignals = new BaseStatusSignal[0]; + private List> otherSignals = new ArrayList<>(); + private final List> otherQueues = new ArrayList<>(); private boolean isCANFD = false; private PhoenixOdometryThread() { @@ -70,10 +74,10 @@ public Queue registerSignal(ParentDevice device, StatusSignal si Drive.odometryLock.lock(); try { isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; + BaseStatusSignal[] newSignals = new BaseStatusSignal[ctreSignals.length + 1]; + System.arraycopy(ctreSignals, 0, newSignals, 0, ctreSignals.length); + newSignals[ctreSignals.length] = signal; + ctreSignals = newSignals; queues.add(queue); } finally { signalsLock.unlock(); @@ -82,6 +86,18 @@ public Queue registerSignal(ParentDevice device, StatusSignal si return queue; } + public Queue registerSignal(Supplier signal) { + Queue queue = new ArrayBlockingQueue<>(10); + Drive.odometryLock.lock(); + try { + otherSignals.add(signal); + otherQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + public Queue makeTimestampQueue() { ArrayBlockingQueue queue = new ArrayBlockingQueue<>(10); Drive.odometryLock.lock(); @@ -100,14 +116,14 @@ public void run() { signalsLock.lock(); try { if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / odometryFrequency, signals); + BaseStatusSignal.waitForAll(2.0 / odometryFrequency, ctreSignals); } else { // "waitForAll" does not support blocking on multiple // signals with a bus that is not CAN FD, regardless // of Pro licensing. No reasoning for this behavior // is provided by the documentation. Thread.sleep((long) (1000.0 / odometryFrequency)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + if (ctreSignals.length > 0) BaseStatusSignal.refreshAll(ctreSignals); } } catch (InterruptedException e) { e.printStackTrace(); @@ -120,17 +136,36 @@ public void run() { try { double timestamp = Logger.getRealTimestamp() / 1e6; double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { + for (BaseStatusSignal signal : ctreSignals) { totalLatency += signal.getTimestamp().getLatency(); } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; + if (ctreSignals.length > 0) { + timestamp -= totalLatency / ctreSignals.length; + } + for (int i = 0; i < ctreSignals.length; i++) { + queues.get(i).offer(ctreSignals[i].getValueAsDouble()); + } + + double[] otherValues = new double[otherSignals.size()]; + boolean isValid = true; + for (int i = 0; i < otherSignals.size(); i++) { + OptionalDouble value = otherSignals.get(i).get(); + if (value.isPresent()) { + otherValues[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); + if (isValid) { + for (int i = 0; i < otherQueues.size(); i++) { + otherQueues.get(i).offer(otherValues[i]); + } + } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); + + for (Queue timestampQueue : timestampQueues) { + timestampQueue.offer(timestamp); } } finally { Drive.odometryLock.unlock(); diff --git a/src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java deleted file mode 100644 index 56dc83a..0000000 --- a/src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.drive; - -import edu.wpi.first.wpilibj.Notifier; -import org.littletonrobotics.junction.Logger; - -import java.util.ArrayList; -import java.util.List; -import java.util.OptionalDouble; -import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; -import java.util.function.Supplier; - -import static org.rambots.subsystems.drive.DriveConstants.odometryFrequency; - -/** - * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. - * - *

This version is intended for devices like the SparkMax that require polling rather than a - * blocking thread. A Notifier thread is used to gather samples with consistent timing. - */ -public class SparkMaxOdometryThread { - private static SparkMaxOdometryThread instance = null; - private final Notifier notifier; - private List> signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - private List> timestampQueues = new ArrayList<>(); - - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - } - - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; - } - - public void start() { - if (timestampQueues.size() > 0) { - notifier.startPeriodic(1.0 / odometryFrequency); - } - } - - public Queue registerSignal(Supplier signal) { - Queue queue = new ArrayBlockingQueue<>(20); - Drive.odometryLock.lock(); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } - - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(20); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } - - private void periodic() { - Drive.odometryLock.lock(); - double timestamp = Logger.getRealTimestamp() / 1e6; - try { - double[] values = new double[signals.size()]; - boolean isValid = true; - for (int i = 0; i < signals.size(); i++) { - OptionalDouble value = signals.get(i).get(); - if (value.isPresent()) { - values[i] = value.getAsDouble(); - } else { - isValid = false; - break; - } - } - if (isValid) { - for (int i = 0; i < signals.size(); i++) { - queues.get(i).offer(values[i]); - timestampQueues.get(i).offer(timestamp); - } - } - } finally { - Drive.odometryLock.unlock(); - } - } -}