diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index a1fa103..1acc953 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,12 +1,9 @@ package frc.lib.util.swerve; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -21,12 +18,8 @@ public class SwerveModule { public int moduleNumber; private Rotation2d angleOffset; - private TalonFX mAngleMotor; - private TalonFX mDriveMotor; - private CANcoder angleEncoder; - private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration(); - private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); - private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); + private SwerveModuleIO io; + private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged(); private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward( Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); @@ -48,87 +41,32 @@ public class SwerveModule { * @param angleOffset Angle Offset of the CANCoder to align the wheels */ public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, - Rotation2d angleOffset) { + Rotation2d angleOffset, SwerveModuleIO io) { + this.io = io; this.moduleNumber = moduleNumber; this.angleOffset = angleOffset; - /* Angle Encoder Config */ - swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; - - angleEncoder = new CANcoder(cancoderID); - angleEncoder.getConfigurator().apply(swerveCANcoderConfig); - - /* Angle Motor Config */ - /* Motor Inverts and Neutral Mode */ - swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert; - swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode; - - /* Gear Ratio and Wrapping Config */ - swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; - swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; - - /* Current Limiting */ - swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = - Constants.Swerve.angleEnableCurrentLimit; - swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; - swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = - Constants.Swerve.angleCurrentThreshold; - swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = - Constants.Swerve.angleCurrentThresholdTime; - - /* PID Config */ - swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP; - swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI; - swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD; - mAngleMotor = new TalonFX(angleMotorID); - mAngleMotor.getConfigurator().apply(swerveAngleFXConfig); + // lastAngle = getState().angle.getDegrees(); + io.updateInputs(inputs); resetToAbsolute(); + Logger.processInputs("SwerveModule" + moduleNumber, inputs); - /* Drive Motor Config */ - /* Motor Inverts and Neutral Mode */ - swerveDriveFXConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert; - swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode; - - /* Gear Ratio Config */ - swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; - - /* Current Limiting */ - swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = - Constants.Swerve.driveEnableCurrentLimit; - swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; - swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = - Constants.Swerve.driveCurrentThreshold; - swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = - Constants.Swerve.driveCurrentThresholdTime; - - /* PID Config */ - swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; - swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; - swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; - - /* Open and Closed Loop Ramping */ - swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = - Constants.Swerve.openLoopRamp; - swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; - - swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = - Constants.Swerve.closedLoopRamp; - swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = - Constants.Swerve.closedLoopRamp; - mDriveMotor = new TalonFX(driveMotorID); - mDriveMotor.getConfigurator().apply(swerveDriveFXConfig); - mDriveMotor.getConfigurator().setPosition(0.0); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("SwerveModule" + moduleNumber, inputs); } /** - * Set the desired state of the Swerve Module + * Set the desir ed state of the Swerve Module * * @param desiredState The desired {@link SwerveModuleState} for the module * @param isOpenLoop Whether the state should be open or closed loop controlled */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { desiredState = SwerveModuleState.optimize(desiredState, getState().angle); - mAngleMotor.setControl(anglePosition.withPosition(desiredState.angle.getRotations())); + io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations())); setSpeed(desiredState, isOpenLoop); } @@ -141,13 +79,17 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { if (isOpenLoop) { driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; - mDriveMotor.setControl(driveDutyCycle); + io.setDriveMotor(driveDutyCycle); + inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; + } else { driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); - mDriveMotor.setControl(driveVelocity); + io.setDriveMotor(driveVelocity); + inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; + } } @@ -157,7 +99,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { * @return The rotation of the CANCoder in {@link Rotation2d} */ public Rotation2d getCANcoder() { - return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue()); + return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder); } /** @@ -165,7 +107,7 @@ public Rotation2d getCANcoder() { */ public void resetToAbsolute() { double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); - mAngleMotor.setPosition(absolutePosition); + io.setPositionAngleMotor(absolutePosition); } /** @@ -175,9 +117,9 @@ public void resetToAbsolute() { */ public SwerveModuleState getState() { return new SwerveModuleState( - Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), + Conversions.RPSToMPS(inputs.driveMotorSelectedSensorVelocity, Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); + Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); } /** @@ -187,8 +129,8 @@ public SwerveModuleState getState() { */ public SwerveModulePosition getPosition() { return new SwerveModulePosition( - Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), + Conversions.rotationsToMeters(inputs.driveMotorSelectedPosition, Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); + Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index 2c18612..bac9627 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -8,18 +8,13 @@ public interface SwerveModuleIO { @AutoLog public static class SwerveModuleInputs { - public double angleEncoderValue; public double driveMotorSelectedPosition; public double driveMotorSelectedSensorVelocity; public double angleMotorSelectedPosition; - public double driveMotorTemperature; - public double angleMotorTemperature; - + public double absolutePositionAngleEncoder; } - public default void updateInputs(SwerveModuleInputs inputs) { - int i = 0; - } + public default void updateInputs(SwerveModuleInputs inputs) {} public default void setDriveMotor(ControlRequest request) {}; @@ -27,24 +22,8 @@ public default void updateInputs(SwerveModuleInputs inputs) { public default void setAngleSelectedSensorPosition(double angle) {}; - public default void setAngleMotorPosition(ControlRequest request) {}; - - - public default double getEncoderPosition() { - return 0.0; - } - - public default double getVelocityOfMotor() { - return 0.0; - } + public default void setPositionAngleMotor(double absolutePosition) {}; - public default double getAngleMotor() { - return 0.0; - } - - public default double getPositionDriveMotor() { - return 0.0; - } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index bd860a8..c2c97f0 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -1,15 +1,128 @@ package frc.lib.util.swerve; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants; public class SwerveModuleReal implements SwerveModuleIO { + private TalonFX mAngleMotor; + private TalonFX mDriveMotor; + private CANcoder angleEncoder; + private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration(); + private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); + private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); + public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int angleCoderID, + Rotation2d angleOffset) { + mAngleMotor = new TalonFX(angleMotorID, "canivore"); + mDriveMotor = new TalonFX(driveMotorID, "canivore"); + angleEncoder = new CANcoder(angleCoderID, "canivore"); + + configAngleEncoder(); + configMotorDrive(); + configAngleMotor(); + + + } + + private void configAngleMotor() { + /* Angle Motor Config */ + /* Motor Inverts and Neutral Mode */ + swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert; + swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode; + + /* Gear Ratio and Wrapping Config */ + swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; + swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; + + /* Current Limiting */ + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.angleEnableCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.angleCurrentThreshold; + swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.angleCurrentThresholdTime; + + /* PID Config */ + swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP; + swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI; + swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD; + + mAngleMotor.getConfigurator().apply(swerveAngleFXConfig); + + } + + public void configMotorDrive() { + /* Drive Motor Config */ + /* Motor Inverts and Neutral Mode */ + swerveDriveFXConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert; + swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode; + + /* Gear Ratio Config */ + swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; + + /* Current Limiting */ + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.driveEnableCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.driveCurrentThreshold; + swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.driveCurrentThresholdTime; + + /* PID Config */ + swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; + swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; + swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; + + /* Open and Closed Loop Ramping */ + swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = + Constants.Swerve.openLoopRamp; + swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; + + swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; + swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; + + mDriveMotor.getConfigurator().apply(swerveDriveFXConfig); + mDriveMotor.getConfigurator().setPosition(0.0); + } + + public void configAngleEncoder() { + /* Angle Encoder Config */ + swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; + + angleEncoder.getConfigurator().apply(swerveCANcoderConfig); + } @Override public void setAngleMotor(ControlRequest request) { - // TODO Auto-generated method stub - SwerveModuleIO.super.setAngleMotor(request); + mAngleMotor.setControl(request); + } + + @Override + public void setDriveMotor(ControlRequest request) { + mDriveMotor.setControl(request); + } + + @Override + public void updateInputs(SwerveModuleInputs inputs) { + inputs.driveMotorSelectedPosition = mDriveMotor.getPosition().getValueAsDouble(); + inputs.driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity().getValueAsDouble(); + inputs.angleMotorSelectedPosition = mAngleMotor.getPosition().getValueAsDouble(); + inputs.absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition().getValueAsDouble(); + } + + @Override + public void setPositionAngleMotor(double absolutePosition) { + mAngleMotor.setPosition(absolutePosition); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6db87ec..b5f0bfc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,6 +1,5 @@ package frc.robot; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -49,10 +48,11 @@ public static final class Swerve { /* Module Gear Ratios */ public static final double driveGearRatio = (8.14 / 1.0); // MK4i L1 - public static final double angleGearRatio = ((150.0 / 7.0) / 1.0); // (150 / 7) : 1 + public static final double angleGearRatio = 12.8 / 1.0; /* Motor Inverts */ - public static final InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + public static final InvertedValue angleMotorInvert = + InvertedValue.CounterClockwise_Positive; public static final InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; @@ -106,45 +106,43 @@ public static final class Swerve { /* Module Specific Constants */ /** - * Front Left Module - Module 0 + * Front Left Module - Module 0. */ public static final class Mod0 { - public static final int driveMotorID = 50; - public static final int angleMotorID = 8; - public static final int canCoderID = 3; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(115.400390625); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final int DRIVE_MOTOR_ID = 6; + public static final int ANGLE_MOTOR_ID = 8; + public static final int CAN_CODER_ID = 4; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(138.604); } /** - * Front Right Module - Module 1 + * Front Right Module - Module 1. */ public static final class Mod1 { - public static final int driveMotorID = 3; - public static final int angleMotorID = 9; - public static final int canCoderID = 1; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-55.37109375); + public static final int DRIVE_MOTOR_ID = 1; + public static final int ANGLE_MOTOR_ID = 4; + public static final int CAN_CODER_ID = 1; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(280.107); } /** - * Back Left Module - Module 2 + * Back Left Module - Module 2. */ public static final class Mod2 { - public static final int driveMotorID = 2; - public static final int angleMotorID = 40; - public static final int canCoderID = 2; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(145.01953125); + public static final int DRIVE_MOTOR_ID = 3; + public static final int ANGLE_MOTOR_ID = 2; + public static final int CAN_CODER_ID = 2; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(121.553); } /** - * Back Right Module - Module 3 + * Back Right Module - Module 3. */ public static final class Mod3 { - public static final int driveMotorID = 6; - public static final int angleMotorID = 51; - public static final int canCoderID = 4; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(3.955078125); + public static final int DRIVE_MOTOR_ID = 7; + public static final int ANGLE_MOTOR_ID = 5; + public static final int CAN_CODER_ID = 3; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(248.027); } public static final HolonomicPathFollowerConfig pathFollowerConfig = diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9f71594..7771e5d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,14 @@ package frc.robot; -import edu.wpi.first.wpilibj.TimedRobot; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -14,7 +21,7 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private Command m_autonomousCommand; @@ -27,9 +34,35 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); + Logger.recordMetadata("ProjectName", "FRC2023"); // Set a metadata value + + if (isReal()) { + Logger.addDataReceiver(new WPILOGWriter("/media/sda2/")); // Log to a USB stick + // ("/U/logs") + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + new PowerDistribution(1, ModuleType.kCTRE); // Enables power distribution logging + } else { + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope + // (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save + // outputs + // to + // a + // new + // log + + // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the + // "Understanding Data Flow" page + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata + // values may be added. + // Instantiate our RobotContainer. This will perform all our button bindings, and put + // our + // autonomous chooser on the dashboard. + } + m_robotContainer = new RobotContainer(isReal()); + } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3cd7d06..9fef19d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,9 +17,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.TeleopSwerve; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.SwerveIO; -import frc.robot.subsystems.SwerveReal; +import frc.robot.subsystems.swerve.Swerve; +import frc.robot.subsystems.swerve.SwerveIO; +import frc.robot.subsystems.swerve.SwerveReal; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -33,24 +33,16 @@ public class RobotContainer { private final SendableChooser autoChooser = new SendableChooser<>(); - SwerveIO swerveIO;; - - /* Subsystems */ - private final Swerve s_Swerve = new Swerve(); - - + private final Swerve s_Swerve; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer(boolean isReal) { - if (isReal) { - // Instantiate IO Implements s_Swerve = new Swerve(new SwerveReal()); } else { s_Swerve = new Swerve(new SwerveIO() {}); } - s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop)); // autoChooser.addOption(resnickAuto, new ResnickAuto(s_Swerve)); @@ -59,7 +51,6 @@ public RobotContainer(boolean isReal) { configureButtonBindings(); } - public void periodic() { } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 4310916..ecf371a 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.swerve.Swerve; /** * Creates an command for driving the swerve drive during tele-op @@ -17,7 +17,7 @@ public class TeleopSwerve extends Command { private CommandXboxController controller; /** - * Creates an command for driving the swerve drive during tele-op + * Creates a command for driving the swerve drive during tele-op * * @param swerveDrive The instance of the swerve drive subsystem * @param fieldRelative Whether the movement is relative to the field or absolute diff --git a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java index 1082587..d80dd7c 100644 --- a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java +++ b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java @@ -4,7 +4,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.swerve.Swerve; +import frc.robot.subsystems.swerve.SwerveIO; /** * Zeros motors then executes normal wait command. @@ -19,10 +20,10 @@ public class ZeroMotorsWaitCommand extends Command { * * @param seconds how long the wait command should run */ - public ZeroMotorsWaitCommand(double seconds) { + public ZeroMotorsWaitCommand(double seconds, SwerveIO swerveIO) { this.m_duration = seconds; SendableRegistry.setName(this, getName() + ": " + seconds + " seconds"); - this.s_Swerve = new Swerve(); + this.s_Swerve = new Swerve(swerveIO); } @Override diff --git a/src/main/java/frc/robot/subsystems/SwerveIO.java b/src/main/java/frc/robot/subsystems/SwerveIO.java deleted file mode 100644 index 115245b..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveIO.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.subsystems; - -import org.littletonrobotics.junction.AutoLog; -import frc.lib.util.swerve.SwerveModule; -import frc.lib.util.swerve.SwerveModuleIO; - - - -public class SwerveIO { - - @AutoLog - public static class SwerveInputs { - public float yaw; - public float roll; - } - - public default void updateInputs(SwerveInputs inputs) {} - - public default - - SwerveModule SwerveModule - - frc.robot.subsystems.SwerveModule createSwerveModule(int moduleNumber, - frc.lib.util.swerve.SwerveModuleConstants constants) { - return new SwerveModule(moduleNumber, constants, new SwerveModuleIO() {}); - } -} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java deleted file mode 100644 index d3d40ae..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems; - -public class SwerveModule { - -} diff --git a/src/main/java/frc/robot/subsystems/SwerveReal.java b/src/main/java/frc/robot/subsystems/SwerveReal.java deleted file mode 100644 index 816892a..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveReal.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.kauailabs.navx.frc.AHRS; -import frc.lib.util.swerve.SwerveModuleReal; -import frc.robot.Constants; -import frc.robot.subsystems.SwerveIO.SwerveInputs; - -public class SwerveReal implements SwerveOI { - - private AHRS gyro = new AHRS(Constants.Swerve.navXID); - - // Real Swereve Initializer - - public SwerveReal() {} - - @Override - - public void updateInputs(SwerveInputs inputs) { - inputs.yaw = gyro.getYaw(); - } - - @Override - public SwerveModule creatSwerveModule(int moduleNumber, SwerveModuleConstants constants) { - return new SwerveModule(moduleNumber, constants, new SwerveModuleReal(constants)); - } - -} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java similarity index 84% rename from src/main/java/frc/robot/subsystems/Swerve.java rename to src/main/java/frc/robot/subsystems/swerve/Swerve.java index 6be3cd7..285261e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -1,9 +1,9 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.swerve; import java.util.Optional; +import org.littletonrobotics.junction.Logger; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -27,40 +27,28 @@ public class Swerve extends SubsystemBase { public SwerveModule[] swerveMods; public AHRS gyro = new AHRS(Constants.Swerve.navXID); private double fieldOffset = gyro.getYaw(); - - private SwerveIO io; private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); - - public Swerve(SwerveIO io) { - this.io = io; - SmartDashboard.putData("Field Pos", field); - - for (int i = 0; i < 4; i++) { - swerveMods[i] = io.createSwerveModule(i, Constants.Swerve.swerveConstants[i]); - } - io.updateInputs(inputs); - - swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.swerveKinematics, getYaw(), - getPositions(), new Pose2d()); - } + private SwerveIO swerveIO; /** * Swerve Subsystem */ - public Swerve() { + public Swerve(SwerveIO swerveIO) { + this.swerveIO = swerveIO; + swerveIO.updateInputs(inputs); swerveMods = new SwerveModule[] { - new SwerveModule(0, Constants.Swerve.Mod0.driveMotorID, - Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID, - Constants.Swerve.Mod0.angleOffset), - new SwerveModule(1, Constants.Swerve.Mod1.driveMotorID, - Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID, - Constants.Swerve.Mod1.angleOffset), - new SwerveModule(2, Constants.Swerve.Mod2.driveMotorID, - Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID, - Constants.Swerve.Mod2.angleOffset), - new SwerveModule(3, Constants.Swerve.Mod3.driveMotorID, - Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID, - Constants.Swerve.Mod3.angleOffset)}; + swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.DRIVE_MOTOR_ID, + Constants.Swerve.Mod0.ANGLE_MOTOR_ID, Constants.Swerve.Mod0.CAN_CODER_ID, + Constants.Swerve.Mod0.ANGLE_OFFSET), + swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.DRIVE_MOTOR_ID, + Constants.Swerve.Mod1.ANGLE_MOTOR_ID, Constants.Swerve.Mod1.CAN_CODER_ID, + Constants.Swerve.Mod1.ANGLE_OFFSET), + swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.DRIVE_MOTOR_ID, + Constants.Swerve.Mod2.ANGLE_MOTOR_ID, Constants.Swerve.Mod2.CAN_CODER_ID, + Constants.Swerve.Mod2.ANGLE_OFFSET), + swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.DRIVE_MOTOR_ID, + Constants.Swerve.Mod3.ANGLE_MOTOR_ID, Constants.Swerve.Mod3.CAN_CODER_ID, + Constants.Swerve.Mod3.ANGLE_OFFSET)}; swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions()); @@ -215,8 +203,10 @@ public void resetModulesToAbsolute() { @Override public void periodic() { swerveOdometry.update(getGyroYaw(), getModulePositions()); - + swerveIO.updateInputs(inputs); + Logger.processInputs("Swerve", inputs); for (SwerveModule mod : swerveMods) { + mod.periodic(); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..8edf500 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.swerve; + +import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.util.swerve.SwerveModule; +import frc.lib.util.swerve.SwerveModuleIO; + +/** IO Class for Swerve */ +public interface SwerveIO { + + /** Inputs Class for Swerve */ + @AutoLog + public static class SwerveInputs { + public float yaw; + public float roll; + } + + public default void updateInputs(SwerveInputs inputs) {} + + public default SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, + int angleMotorID, int cancoderID, Rotation2d angleOffset) { + return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset, + new SwerveModuleIO() { + + }); + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java new file mode 100644 index 0000000..4c4258b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems.swerve; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.util.swerve.SwerveModule; +import frc.lib.util.swerve.SwerveModuleReal; +import frc.robot.Constants; + +/** Real Class for Swerve */ +public class SwerveReal implements SwerveIO { + + private AHRS gyro = new AHRS(Constants.Swerve.navXID); + + /** Real Swerve Initializer */ + public SwerveReal() { + + } + + @Override + public void updateInputs(SwerveInputs inputs) { + inputs.yaw = gyro.getYaw(); + } + + @Override + public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, + int cancoderID, Rotation2d angleOffset) { + return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset, + new SwerveModuleReal(moduleNumber, driveMotorID, angleMotorID, cancoderID, + angleOffset)); + } + +}