diff --git a/src/main/java/frc/robot/subsystems/swerve/module/Module.java b/src/main/java/frc/robot/subsystems/swerve/module/Module.java index 9f83346..cb550c4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/Module.java @@ -15,7 +15,7 @@ public class Module { public record ModuleConstants( int id, String prefix, int driveID, int turnID, int cancoderID, Rotation2d cancoderOffset) {} - private final ModuleIO io; + private final ModuleIOReal io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final ModuleConstants constants; @@ -32,7 +32,7 @@ public record ModuleConstants( private final Debouncer turnEncoderConnectedDebouncer = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - public Module(ModuleIO io) { + public Module(ModuleIOReal io) { this.io = io; this.constants = io.getModuleConstants(); diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java deleted file mode 100644 index e481c64..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.swerve.module.Module.ModuleConstants; -import org.littletonrobotics.junction.AutoLog; - -public interface ModuleIO { - - @AutoLog - public static class ModuleIOInputs { - // For the drive motor - // The conversions from rotations to meters are done in the SensorToMechanismRatio config for - // the motors - public boolean driveConnected = false; - public double driveVelocityMetersPerSec = 0.0; - public double drivePositionMeters = 0.0; - public double driveAppliedVolts = 0.0; - public double driveTempC = 0.0; - public double driveStatorCurrentAmps = 0.0; - public double driveSupplyCurrentAmps = 0.0; - - // For the turn motor - public boolean turnConnected = false; - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double turnTempC = 0.0; - public double turnStatorCurrentAmps = 0.0; - public double turnSupplyCurrentAmps = 0.0; - - public boolean cancoderConnected = false; - public Rotation2d cancoderAbsolutePosition = new Rotation2d(); - } - - public void updateInputs(ModuleIOInputs inputs); - - public void setDriveVoltage(double volts, boolean withFoc); - - public default void setDriveVoltage(double volts) { - setDriveVoltage(volts, true); - } - - public void setDriveVelocitySetpoint(double setpointMetersPerSecond); - - public void setTurnVoltage(double volts); - - public void setTurnPositionSetpoint(Rotation2d setpoint); - - public ModuleConstants getModuleConstants(); -} diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java index a2998c7..cc29237 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java @@ -19,15 +19,43 @@ import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.Registration; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.SignalType; import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; -public class ModuleIOReal implements ModuleIO { +public class ModuleIOReal { + + @AutoLog + public static class ModuleIOInputs { + // For the drive motor + // The conversions from rotations to meters are done in the SensorToMechanismRatio config for + // the motors + public boolean driveConnected = false; + public double driveVelocityMetersPerSec = 0.0; + public double drivePositionMeters = 0.0; + public double driveAppliedVolts = 0.0; + public double driveTempC = 0.0; + public double driveStatorCurrentAmps = 0.0; + public double driveSupplyCurrentAmps = 0.0; + + // For the turn motor + public boolean turnConnected = false; + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnTempC = 0.0; + public double turnStatorCurrentAmps = 0.0; + public double turnSupplyCurrentAmps = 0.0; + + public boolean cancoderConnected = false; + public Rotation2d cancoderAbsolutePosition = new Rotation2d(); + } + private final ModuleConstants constants; // Hardware - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; + protected final TalonFX driveTalon; + protected final TalonFX turnTalon; + protected final CANcoder cancoder; // Status signals // For drive @@ -124,7 +152,6 @@ public ModuleIOReal(ModuleConstants moduleConstants, CANBus canbus) { cancoder.optimizeBusUtilization(); } - @Override public void updateInputs(ModuleIOInputs inputs) { Logger.recordOutput( "Module" + constants.prefix() + "refresh status code", @@ -178,27 +205,26 @@ public void updateInputs(ModuleIOInputs inputs) { Rotation2d.fromRotations(cancoderAbsolutePosition.getValueAsDouble()); } - @Override public void setDriveVoltage(double volts, boolean withFoc) { driveTalon.setControl(driveVoltage.withOutput(volts).withEnableFOC(withFoc)); } - @Override + public void setDriveVoltage(double volts) { + setDriveVoltage(volts, true); + } + public void setDriveVelocitySetpoint(double setpointMetersPerSecond) { driveTalon.setControl(driveVelocityControl.withVelocity(setpointMetersPerSecond)); } - @Override public void setTurnVoltage(double volts) { turnTalon.setControl(turnVoltage.withOutput(volts)); } - @Override public void setTurnPositionSetpoint(Rotation2d setpoint) { turnTalon.setControl(turnPID.withPosition(setpoint.getRotations())); } - @Override public ModuleConstants getModuleConstants() { return constants; } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index db1395f..dad28f6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -1,110 +1,20 @@ package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.Radian; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.subsystems.swerve.module.Module.ModuleConstants; import frc.robot.utils.MaplePhoenixUtil; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; -public class ModuleIOSim implements ModuleIO { - - private final Module.ModuleConstants constants; - - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - - // Status signals - private final BaseStatusSignal drivePosition; - private final BaseStatusSignal driveVelocity; - private final BaseStatusSignal driveTemp; - private final BaseStatusSignal driveSupplyCurrent; - private final BaseStatusSignal driveStatorCurrent; - private final BaseStatusSignal driveAppliedVolts; - - private final BaseStatusSignal cancoderAbsolutePosition; - private final BaseStatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final BaseStatusSignal turnTemp; - private final BaseStatusSignal turnSupplyCurrent; - private final BaseStatusSignal turnStatorCurrent; - private final BaseStatusSignal turnAppliedVolts; - - // Control modes - private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); - private final VoltageOut turnVoltage = new VoltageOut(0.0).withEnableFOC(true); - private final VelocityTorqueCurrentFOC driveVelocityControl = - new VelocityTorqueCurrentFOC(0.0).withSlot(0); - private final MotionMagicVoltage turnPID = new MotionMagicVoltage(0.0); +public class ModuleIOSim extends ModuleIOReal { private final SwerveModuleSimulation simulation; public ModuleIOSim( Module.ModuleConstants constants, SwerveModuleSimulation simulation, CANBus canbus) { - this.constants = constants; - - // Initialize hardware - driveTalon = new TalonFX(constants.driveID(), canbus); - turnTalon = new TalonFX(constants.turnID(), canbus); - cancoder = new CANcoder(constants.cancoderID(), canbus); - - // Configure hardware - driveTalon.getConfigurator().apply(SwerveSubsystem.SWERVE_CONSTANTS.getDriveConfig()); - turnTalon - .getConfigurator() - .apply(SwerveSubsystem.SWERVE_CONSTANTS.getTurnConfig(constants.cancoderID())); - - cancoder - .getConfigurator() - .apply(SwerveSubsystem.SWERVE_CONSTANTS.getCancoderConfig(constants.cancoderOffset())); - - // Initialize status signals - drivePosition = driveTalon.getPosition(); - driveVelocity = driveTalon.getVelocity(); - driveTemp = driveTalon.getDeviceTemp(); - driveSupplyCurrent = driveTalon.getSupplyCurrent(); - driveStatorCurrent = driveTalon.getStatorCurrent(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - - cancoderAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnVelocity = turnTalon.getVelocity(); - turnTemp = turnTalon.getDeviceTemp(); - turnSupplyCurrent = turnTalon.getSupplyCurrent(); - turnStatorCurrent = turnTalon.getStatorCurrent(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - drivePosition, - driveVelocity, - driveTemp, - driveAppliedVolts, - driveStatorCurrent, - driveSupplyCurrent, - cancoderAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent, - turnSupplyCurrent); - - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - cancoder.optimizeBusUtilization(); + super(constants, canbus); // Setup this modules simulation (lowk the only difference between Sim and Real except for not // using Async odo) @@ -119,81 +29,4 @@ public ModuleIOSim( false, Angle.ofBaseUnits(constants.cancoderOffset().getRadians(), Radian))); } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveStatorCurrent, - driveSupplyCurrent, - driveTemp, - cancoderAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent, - turnSupplyCurrent, - turnTemp); - - inputs.driveConnected = - BaseStatusSignal.isAllGood( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveStatorCurrent, - driveSupplyCurrent, - driveTemp); - inputs.drivePositionMeters = drivePosition.getValueAsDouble(); - inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveTempC = driveTemp.getValueAsDouble(); - inputs.driveStatorCurrentAmps = driveStatorCurrent.getValueAsDouble(); - inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); - - inputs.turnConnected = - BaseStatusSignal.isAllGood( - turnPosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent, - turnSupplyCurrent, - turnTemp); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = turnVelocity.getValue().in(RadiansPerSecond); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnTempC = turnTemp.getValueAsDouble(); - inputs.turnStatorCurrentAmps = turnStatorCurrent.getValueAsDouble(); - inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); - - inputs.cancoderConnected = BaseStatusSignal.isAllGood(cancoderAbsolutePosition); - inputs.cancoderAbsolutePosition = - Rotation2d.fromRotations(cancoderAbsolutePosition.getValueAsDouble()); - } - - @Override - public void setDriveVoltage(double volts, boolean withFoc) { - driveTalon.setControl(driveVoltage.withOutput(volts).withEnableFOC(withFoc)); - } - - @Override - public void setDriveVelocitySetpoint(double setpointMetersPerSecond) { - driveTalon.setControl(driveVelocityControl.withVelocity(setpointMetersPerSecond)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(turnVoltage.withOutput(volts)); - } - - @Override - public void setTurnPositionSetpoint(Rotation2d setpoint) { - turnTalon.setControl(turnPID.withPosition(setpoint.getRotations())); - } - - @Override - public ModuleConstants getModuleConstants() { - return constants; - } }