diff --git a/src/main/java/frc/robot/subsystems/GyroSubsystem.java b/src/main/java/frc/robot/subsystems/GyroSubsystem.java new file mode 100644 index 0000000..70f7156 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/GyroSubsystem.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.logging.io.gyro.GyroIo; +import frc.robot.utils.logging.io.gyro.MockGyroIo; +import frc.robot.utils.logging.io.gyro.RealGyroIo; +import frc.robot.utils.simulation.RobotVisualizer; + +public class GyroSubsystem extends SubsystemBase { + private final GyroIo io; + + public GyroSubsystem(GyroIo io) { + this.io = io; + } + + public void setAngleOffset(double offset) { + io.setAngleOffset(offset); + } + + public void resetGyro() { + io.resetGyro(); + } + + @Override + public void periodic() { + io.periodic(); + } + + public static GyroIo createMockIo() { + return new MockGyroIo(); + } + + public static GyroIo createRealIo() { + RealGyroIo realGyroIo = new RealGyroIo(); + realGyroIo.start(); + return realGyroIo; + } + + public static GyroIo createSimIo(RobotVisualizer visualizer) { + // For now, we don't actually simulate the gyro + return new MockGyroIo(); + } +} diff --git a/src/main/java/frc/robot/subsystems/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/RollerSubsystem.java index fda3114..3f929d0 100644 --- a/src/main/java/frc/robot/subsystems/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RollerSubsystem.java @@ -43,7 +43,7 @@ public void periodic() { } public static SparkMaxIo createMockIo() { - return new MockSparkMaxIo(); + return new MockSparkMaxIo(LOGGING_NAME, MotorLoggableInputs.allMetrics()); } public static SparkMaxIo createRealIo() { diff --git a/src/main/java/frc/robot/subsystems/TiltSubsystem.java b/src/main/java/frc/robot/subsystems/TiltSubsystem.java index d8f2606..11c2a96 100644 --- a/src/main/java/frc/robot/subsystems/TiltSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TiltSubsystem.java @@ -54,7 +54,7 @@ public void periodic() { } public static SparkMaxIo createMockIo() { - return new MockSparkMaxIo(); + return new MockSparkMaxIo(LOGGING_NAME, MotorLoggableInputs.allMetrics()); } public static SparkMaxIo createRealIo() { diff --git a/src/main/java/frc/robot/utils/logging/input/GyroInputs.java b/src/main/java/frc/robot/utils/logging/input/GyroInputs.java new file mode 100644 index 0000000..90289a5 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/input/GyroInputs.java @@ -0,0 +1,81 @@ +package frc.robot.utils.logging.input; + +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class GyroInputs implements LoggableInputs { + + private GyroValues values; + + public GyroValues getGyroValues() { + return values; + } + + @Override + public void toLog(LogTable table) { + table.put("anglesInDeg", values.getAnglesInDeg()); + table.put("angleOffset", values.getAngleOffset()); + table.put("worldLinearAccelX", values.getWorldLinearAccelX()); + table.put("worldLinearAccelY", values.getWorldLinearAccelY()); + table.put("worldLinearAccelZ", values.getWorldLinearAccelZ()); + table.put("velocityX", values.getVelocityX()); + table.put("velocityY", values.getVelocityY()); + table.put("velocityZ", values.getVelocityZ()); + table.put("displacementX", values.getDisplacementX()); + table.put("displacementY", values.getDisplacementY()); + table.put("displacementZ", values.getDisplacementZ()); + table.put("rawGyroX", values.getRawGyroX()); + table.put("rawGyroY", values.getRawGyroY()); + table.put("rawGyroZ", values.getRawGyroZ()); + table.put("rawAccelX", values.getRawAccelX()); + table.put("rawAccelY", values.getRawAccelY()); + table.put("rawAccelZ", values.getRawAccelZ()); + table.put("rawMagX", values.getRawMagX()); + table.put("rawMagY", values.getRawMagY()); + table.put("rawMagZ", values.getRawMagZ()); + table.put("yaw", values.getYaw()); + table.put("pitch", values.getPitch()); + table.put("roll", values.getRoll()); + table.put("fusedHeading", values.getFusedHeading()); + table.put("compassHeading", values.getCompassHeading()); + table.put("robotCentricVelocityX", values.getRobotCentricVelocityX()); + table.put("robotCentricVelocityY", values.getRobotCentricVelocityY()); + table.put("robotCentricVelocityZ", values.getRobotCentricVelocityZ()); + } + + @Override + public void fromLog(LogTable table) { + values.setAnglesInDeg(table.get("anglesInDeg", values.getAnglesInDeg())); + values.setAngleOffset(table.get("angleOffset", values.getAngleOffset())); + values.setWorldLinearAccelX(table.get("worldLinearAccelX", values.getWorldLinearAccelX())); + values.setWorldLinearAccelY(table.get("worldLinearAccelY", values.getWorldLinearAccelY())); + values.setWorldLinearAccelZ(table.get("worldLinearAccelZ", values.getWorldLinearAccelZ())); + values.setVelocityX(table.get("velocityX", values.getVelocityX())); + values.setVelocityY(table.get("velocityY", values.getVelocityY())); + values.setVelocityZ(table.get("velocityZ", values.getVelocityZ())); + values.setDisplacementX(table.get("displacementX", values.getDisplacementX())); + values.setDisplacementY(table.get("displacementY", values.getDisplacementY())); + values.setDisplacementZ(table.get("displacementZ", values.getDisplacementZ())); + values.setRawGyroX(table.get("rawGyroX", values.getRawGyroX())); + values.setRawGyroY(table.get("rawGyroY", values.getRawGyroY())); + values.setRawGyroZ(table.get("rawGyroZ", values.getRawGyroZ())); + values.setRawAccelX(table.get("rawAccelX", values.getRawAccelX())); + values.setRawAccelY(table.get("rawAccelY", values.getRawAccelY())); + values.setRawAccelZ(table.get("rawAccelZ", values.getRawAccelZ())); + values.setRawMagX(table.get("rawMagX", values.getRawMagX())); + values.setRawMagY(table.get("rawMagY", values.getRawMagY())); + values.setRawMagZ(table.get("rawMagZ", values.getRawMagZ())); + values.setYaw(table.get("yaw", values.getYaw())); + values.setPitch(table.get("pitch", values.getPitch())); + values.setRoll(table.get("roll", values.getRoll())); + values.setCompassHeading(table.get("compassHeading", values.getCompassHeading())); + values.setFusedHeading(table.get("fusedHeading", values.getFusedHeading())); + values.setRobotCentricVelocityX(table.get("robotCentricVelocityX", values.getRobotCentricVelocityX())); + values.setRobotCentricVelocityY(table.get("robotCentricVelocityY", values.getRobotCentricVelocityY())); + values.setRobotCentricVelocityZ(table.get("robotCentricVelocityZ", values.getRobotCentricVelocityZ())); + } + + public void fromHardware(GyroValues values) { + this.values = values; + } +} diff --git a/src/main/java/frc/robot/utils/logging/input/GyroValues.java b/src/main/java/frc/robot/utils/logging/input/GyroValues.java new file mode 100644 index 0000000..5fbd0b5 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/input/GyroValues.java @@ -0,0 +1,256 @@ +package frc.robot.utils.logging.input; + +public class GyroValues { + private double anglesInDeg = 0; + private double angleOffset = 0; + private double worldLinearAccelX = 0; + private double worldLinearAccelY = 0; + private double worldLinearAccelZ = 0; + private double velocityX = 0; + private double velocityY = 0; + private double velocityZ = 0; + private double displacementX = 0; + private double displacementY = 0; + private double displacementZ = 0; + private double rawGyroX = 0; + private double rawGyroY = 0; + private double rawGyroZ = 0; + private double rawAccelX = 0; + private double rawAccelY = 0; + private double rawAccelZ = 0; + private double rawMagX = 0; + private double rawMagY = 0; + private double rawMagZ = 0; + private double yaw = 0; + private double pitch = 0; + private double roll = 0; + private double fusedHeading = 0; + private double compassHeading = 0; + private double robotCentricVelocityX = 0; + private double robotCentricVelocityY = 0; + private double robotCentricVelocityZ = 0; + + public double getAnglesInDeg() { + return anglesInDeg; + } + + public void setAnglesInDeg(double anglesInDeg) { + this.anglesInDeg = anglesInDeg; + } + + public double getAngleOffset() { + return angleOffset; + } + + public void setAngleOffset(double angleOffset) { + this.angleOffset = angleOffset; + } + + public double getWorldLinearAccelX() { + return worldLinearAccelX; + } + + public void setWorldLinearAccelX(double worldLinearAccelX) { + this.worldLinearAccelX = worldLinearAccelX; + } + + public double getWorldLinearAccelY() { + return worldLinearAccelY; + } + + public void setWorldLinearAccelY(double worldLinearAccelY) { + this.worldLinearAccelY = worldLinearAccelY; + } + + public double getWorldLinearAccelZ() { + return worldLinearAccelZ; + } + + public void setWorldLinearAccelZ(double worldLinearAccelZ) { + this.worldLinearAccelZ = worldLinearAccelZ; + } + + public double getVelocityX() { + return velocityX; + } + + public void setVelocityX(double velocityX) { + this.velocityX = velocityX; + } + + public double getVelocityY() { + return velocityY; + } + + public void setVelocityY(double velocityY) { + this.velocityY = velocityY; + } + + public double getVelocityZ() { + return velocityZ; + } + + public void setVelocityZ(double velocityZ) { + this.velocityZ = velocityZ; + } + + public double getDisplacementX() { + return displacementX; + } + + public void setDisplacementX(double displacementX) { + this.displacementX = displacementX; + } + + public double getDisplacementY() { + return displacementY; + } + + public void setDisplacementY(double displacementY) { + this.displacementY = displacementY; + } + + public double getDisplacementZ() { + return displacementZ; + } + + public void setDisplacementZ(double displacementZ) { + this.displacementZ = displacementZ; + } + + public double getRawGyroX() { + return rawGyroX; + } + + public void setRawGyroX(double rawGyroX) { + this.rawGyroX = rawGyroX; + } + + public double getRawGyroY() { + return rawGyroY; + } + + public void setRawGyroY(double rawGyroY) { + this.rawGyroY = rawGyroY; + } + + public double getRawGyroZ() { + return rawGyroZ; + } + + public void setRawGyroZ(double rawGyroZ) { + this.rawGyroZ = rawGyroZ; + } + + public double getRawAccelX() { + return rawAccelX; + } + + public void setRawAccelX(double rawAccelX) { + this.rawAccelX = rawAccelX; + } + + public double getRawAccelY() { + return rawAccelY; + } + + public void setRawAccelY(double rawAccelY) { + this.rawAccelY = rawAccelY; + } + + public double getRawAccelZ() { + return rawAccelZ; + } + + public void setRawAccelZ(double rawAccelZ) { + this.rawAccelZ = rawAccelZ; + } + + public double getRawMagX() { + return rawMagX; + } + + public void setRawMagX(double rawMagX) { + this.rawMagX = rawMagX; + } + + public double getRawMagY() { + return rawMagY; + } + + public void setRawMagY(double rawMagY) { + this.rawMagY = rawMagY; + } + + public double getRawMagZ() { + return rawMagZ; + } + + public void setRawMagZ(double rawMagZ) { + this.rawMagZ = rawMagZ; + } + + public double getYaw() { + return yaw; + } + + public void setYaw(double yaw) { + this.yaw = yaw; + } + + public double getPitch() { + return pitch; + } + + public void setPitch(double pitch) { + this.pitch = pitch; + } + + public double getRoll() { + return roll; + } + + public void setRoll(double roll) { + this.roll = roll; + } + + public double getFusedHeading() { + return fusedHeading; + } + + public void setFusedHeading(double fusedHeading) { + this.fusedHeading = fusedHeading; + } + + public double getCompassHeading() { + return compassHeading; + } + + public void setCompassHeading(double compassHeading) { + this.compassHeading = compassHeading; + } + + public double getRobotCentricVelocityX() { + return robotCentricVelocityX; + } + + public void setRobotCentricVelocityX(double robotCentricVelocityX) { + this.robotCentricVelocityX = robotCentricVelocityX; + } + + public double getRobotCentricVelocityY() { + return robotCentricVelocityY; + } + + public void setRobotCentricVelocityY(double robotCentricVelocityY) { + this.robotCentricVelocityY = robotCentricVelocityY; + } + + public double getRobotCentricVelocityZ() { + return robotCentricVelocityZ; + } + + public void setRobotCentricVelocityZ(double robotCentricVelocityZ) { + this.robotCentricVelocityZ = robotCentricVelocityZ; + } +} diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/GyroIo.java b/src/main/java/frc/robot/utils/logging/io/gyro/GyroIo.java new file mode 100644 index 0000000..5d138ea --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/io/gyro/GyroIo.java @@ -0,0 +1,15 @@ +package frc.robot.utils.logging.io.gyro; + + +import frc.robot.utils.logging.input.GyroValues; +import frc.robot.utils.logging.io.BaseIo; + +public interface GyroIo extends BaseIo { + String LOGGING_NAME = "Gyro"; + + void setAngleOffset(double offset); + + void resetGyro(); + + GyroValues getGyroValues(); +} diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/MockGyroIo.java b/src/main/java/frc/robot/utils/logging/io/gyro/MockGyroIo.java new file mode 100644 index 0000000..07759d9 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/io/gyro/MockGyroIo.java @@ -0,0 +1,28 @@ +package frc.robot.utils.logging.io.gyro; + +import frc.robot.utils.logging.input.GyroInputs; +import frc.robot.utils.logging.input.GyroValues; +import frc.robot.utils.logging.io.BaseIoImpl; + +public class MockGyroIo extends BaseIoImpl implements GyroIo { + public MockGyroIo() { + super(LOGGING_NAME, new GyroInputs()); + } + + @Override + public void setAngleOffset(double offset) { + } + + @Override + public void resetGyro() { + } + + @Override + public GyroValues getGyroValues() { + return getInputs().getGyroValues(); + } + + @Override + protected void updateInputs(GyroInputs inputs) { + } +} diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java b/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java new file mode 100644 index 0000000..eaae0b9 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java @@ -0,0 +1,43 @@ +package frc.robot.utils.logging.io.gyro; + +import com.studica.frc.AHRS; +import frc.robot.utils.logging.input.GyroInputs; +import frc.robot.utils.logging.input.GyroValues; +import frc.robot.utils.logging.io.BaseIoImpl; + +public class RealGyroIo extends BaseIoImpl implements GyroIo { + private ThreadedGyro threadedGyro; + + public RealGyroIo() { + super(LOGGING_NAME, new GyroInputs()); + threadedGyro = new ThreadedGyro(new AHRS(AHRS.NavXComType.kMXP_SPI)); + } + + public void start() { + threadedGyro.start(); + } + + public void stop() { + threadedGyro.stop(); + } + + @Override + public void setAngleOffset(double offset) { + threadedGyro.setAngleAdjustment(offset); + } + + @Override + public void resetGyro() { + threadedGyro.resetGyro(); + } + + @Override + public GyroValues getGyroValues() { + return getInputs().getGyroValues(); + } + + @Override + protected void updateInputs(GyroInputs inputs) { + inputs.fromHardware(threadedGyro.getGyroValues()); + } +} diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java b/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java new file mode 100644 index 0000000..a3e9f18 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java @@ -0,0 +1,121 @@ +package frc.robot.utils.logging.io.gyro; + +import com.studica.frc.AHRS; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.utils.logging.input.GyroValues; + +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicLong; +import java.util.concurrent.atomic.AtomicReference; + +public class ThreadedGyro { + private static final long GYRO_THREAD_RATE_MS = 20; + private final AHRS gyro; + private AtomicReference gyroValues; + private final AtomicBoolean shouldReset = new AtomicBoolean(false); + private final AtomicBoolean shouldOffset = new AtomicBoolean(false); + private final AtomicLong gyroOffset = new AtomicLong(); + private final ScheduledExecutorService executor; + + public ThreadedGyro(AHRS gyro) { + this.gyro = gyro; + this.gyroValues = new AtomicReference<>(); + this.executor = Executors.newScheduledThreadPool(1); + } + + public void start() { + updateGyro(); + executor.scheduleAtFixedRate( + () -> { + if (shouldReset.get()) { + gyro.reset(); + shouldReset.set(false); + } + if (shouldOffset.get()) { + gyro.setAngleAdjustment(Double.longBitsToDouble(gyroOffset.get())); + shouldOffset.set(false); + } + updateGyro(); + }, + 0, + GYRO_THREAD_RATE_MS, + TimeUnit.MILLISECONDS); + } + + public void stop() { + executor.shutdownNow(); + } + + public boolean stopAndWait(long maxTime, TimeUnit timeUnit) { + executor.shutdownNow(); + try { + return executor.awaitTermination(maxTime, timeUnit); + } catch (InterruptedException e) { + DriverStation.reportError( + "ThreadedGyro thread termination was interrupted: " + e.getMessage(), true); + return false; + } + } + + private void updateGyro() { + GyroValues newValues = new GyroValues(); + newValues.setAnglesInDeg((((gyro.getAngle()) % 360) * -1)); + + newValues.setWorldLinearAccelX(gyro.getWorldLinearAccelX()); + newValues.setWorldLinearAccelY(gyro.getWorldLinearAccelY()); + newValues.setWorldLinearAccelZ(gyro.getWorldLinearAccelZ()); + + newValues.setVelocityX(gyro.getVelocityX()); + newValues.setVelocityY(gyro.getVelocityY()); + newValues.setVelocityZ(gyro.getVelocityZ()); + + newValues.setDisplacementX(gyro.getDisplacementX()); + newValues.setDisplacementY(gyro.getDisplacementY()); + newValues.setDisplacementZ(gyro.getDisplacementZ()); + + newValues.setRawGyroX(gyro.getRawGyroX()); + newValues.setRawGyroY(gyro.getRawGyroY()); + newValues.setRawGyroZ(gyro.getRawGyroZ()); + + newValues.setRawAccelX(gyro.getRawAccelX()); + newValues.setRawAccelY(gyro.getRawAccelY()); + newValues.setRawAccelZ(gyro.getRawAccelZ()); + + newValues.setRawMagX(gyro.getRawMagX()); + newValues.setRawMagY(gyro.getRawMagY()); + newValues.setRawMagZ(gyro.getRawMagZ()); + + newValues.setYaw(gyro.getYaw()); + newValues.setPitch(gyro.getPitch()); + newValues.setRoll(gyro.getRoll()); + + newValues.setFusedHeading(gyro.getFusedHeading()); + newValues.setCompassHeading(gyro.getCompassHeading()); + + newValues.setRobotCentricVelocityX(gyro.getRobotCentricVelocityX()); + newValues.setRobotCentricVelocityY(gyro.getRobotCentricVelocityY()); + newValues.setRobotCentricVelocityZ(gyro.getRobotCentricVelocityZ()); + + gyroValues.set(newValues); + } + + public GyroValues getGyroValues() { + return gyroValues.get(); + } + + public void resetGyro() { + shouldReset.set(true); + } + + public void setAngleAdjustment(double degrees) { + gyroOffset.set(Double.doubleToLongBits(degrees)); + shouldOffset.set(true); + } + + public double getAngleOffset() { + return Double.longBitsToDouble(gyroOffset.get()); + } +} diff --git a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java index 1a21561..11d1c52 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java @@ -4,12 +4,16 @@ package frc.robot.utils.logging.io.motor; +import com.revrobotics.spark.SparkMax; +import frc.robot.utils.logging.input.MotorLoggableInputs; +import frc.robot.utils.logging.io.BaseIoImpl; + /** * Mock implementation (noop) for a SparkMax IO. */ -public class MockSparkMaxIo implements SparkMaxIo { - @Override - public void periodic() { +public class MockSparkMaxIo extends BaseIoImpl implements SparkMaxIo { + public MockSparkMaxIo(String name, MotorLoggableInputs inputs) { + super(name, inputs); } @Override @@ -26,11 +30,15 @@ public void stopMotor() { @Override public boolean isFwdSwitchPressed() { - return false; + return getInputs().getFwdLimit(); } @Override public boolean isRevSwitchPressed() { - return false; + return getInputs().getRevLimit(); + } + + @Override + protected void updateInputs(MotorLoggableInputs inputs) { } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java index 65d5310..56479e3 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java @@ -46,6 +46,6 @@ public boolean isRevSwitchPressed() { @Override protected void updateInputs(MotorLoggableInputs inputs) { - getInputs().fromHardware(motor); + inputs.fromHardware(motor); } }