Skip to content

Commit

Permalink
Migrate gyroscope to ADIS
Browse files Browse the repository at this point in the history
  • Loading branch information
@twangodev
twangodev committed Mar 5, 2024
1 parent 48a7182 commit cd46b92
Show file tree
Hide file tree
Showing 6 changed files with 72 additions and 153 deletions.
2 changes: 1 addition & 1 deletion src/main/java/org/rambots/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ object Robot : LoggedRobot() {

when (Constants.getMode()) {
Constants.Mode.REAL -> {
Logger.addDataReceiver(WPILOGWriter())
// Logger.addDataReceiver(WPILOGWriter())
Logger.addDataReceiver(NT4Publisher())
}

Expand Down
32 changes: 13 additions & 19 deletions src/main/java/org/rambots/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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()

Expand All @@ -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]),
Expand All @@ -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"))
}

Expand All @@ -93,7 +87,7 @@ object RobotContainer {
ModuleIOSim(),
ModuleIOSim()
)
flywheel = Flywheel(FlywheelIOSim())
// flywheel = Flywheel(FlywheelIOSim())
aprilTagVision =
AprilTagVision(
AprilTagVisionIOPhotonVisionSIM(
Expand All @@ -111,7 +105,7 @@ object RobotContainer {
object : ModuleIO {},
object : ModuleIO {},
object : ModuleIO {})
flywheel = Flywheel(object : FlywheelIO {})
// flywheel = Flywheel(object : FlywheelIO {})
aprilTagVision = AprilTagVision(object : AprilTagVisionIO {})
}
}
Expand Down Expand Up @@ -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()
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/rambots/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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(
Expand Down
19 changes: 8 additions & 11 deletions ...rambots/subsystems/drive/GyroIONavX2.java → ...rambots/subsystems/drive/GyroIO_ADIS.java
View file
Original file line number Diff line number Diff line change
@@ -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<Double> 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();
}
Expand All @@ -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);

Expand Down
63 changes: 49 additions & 14 deletions src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -42,7 +44,9 @@ public class PhoenixOdometryThread extends Thread {
new ReentrantLock(); // Prevents conflicts when registering signals
private final List<Queue<Double>> queues = new ArrayList<>();
private final List<Queue<Double>> timestampQueues = new ArrayList<>();
private BaseStatusSignal[] signals = new BaseStatusSignal[0];
private BaseStatusSignal[] ctreSignals = new BaseStatusSignal[0];
private List<Supplier<OptionalDouble>> otherSignals = new ArrayList<>();
private final List<Queue<Double>> otherQueues = new ArrayList<>();
private boolean isCANFD = false;

private PhoenixOdometryThread() {
Expand Down Expand Up @@ -70,10 +74,10 @@ public Queue<Double> registerSignal(ParentDevice device, StatusSignal<Double> 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();
Expand All @@ -82,6 +86,18 @@ public Queue<Double> registerSignal(ParentDevice device, StatusSignal<Double> si
return queue;
}

public Queue<Double> registerSignal(Supplier<OptionalDouble> signal) {
Queue<Double> queue = new ArrayBlockingQueue<>(10);
Drive.odometryLock.lock();
try {
otherSignals.add(signal);
otherQueues.add(queue);
} finally {
Drive.odometryLock.unlock();
}
return queue;
}

public Queue<Double> makeTimestampQueue() {
ArrayBlockingQueue<Double> queue = new ArrayBlockingQueue<>(10);
Drive.odometryLock.lock();
Expand All @@ -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();
Expand All @@ -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<Double> timestampQueue : timestampQueues) {
timestampQueue.offer(timestamp);
}
} finally {
Drive.odometryLock.unlock();
Expand Down
107 changes: 0 additions & 107 deletions src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java
View file

This file was deleted.

0 comments on commit cd46b92

Please sign in to comment.