diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 884ef13..5f737e3 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -16,7 +16,7 @@ public class SwerveModule { public int moduleNumber; private Rotation2d angleOffset; - private double lastAngle; + // private double lastAngle; private SwerveModuleIO io; private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged(); @@ -49,7 +49,7 @@ public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int ca this.angleOffset = angleOffset; - lastAngle = getState().angle.getDegrees(); + // lastAngle = getState().angle.getDegrees(); } @@ -61,7 +61,6 @@ public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int ca */ 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); } @@ -75,14 +74,12 @@ 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); } else { driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); - // mDriveMotor.setControl(driveVelocity); io.setDriveMotor(driveDutyCycle); } } @@ -101,7 +98,6 @@ public Rotation2d getCANcoder() { */ public void resetToAbsolute() { double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); - // mAngleMotor.setPosition(absolutePosition); io.setPositionAngleMotor(absolutePosition); } @@ -112,11 +108,8 @@ public void resetToAbsolute() { */ public SwerveModuleState getState() { return new SwerveModuleState( - // Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), - // Constants.Swerve.wheelCircumference), Conversions.RPSToMPS(io.getVelocityDriveMotor().getValue(), Constants.Swerve.wheelCircumference), - // Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); Rotation2d.fromRotations(io.getPositionAngleMotor().getValue())); } @@ -127,11 +120,8 @@ public SwerveModuleState getState() { */ public SwerveModulePosition getPosition() { return new SwerveModulePosition( - // Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), - // Constants.Swerve.wheelCircumference), Conversions.rotationsToMeters(io.getPositionDriveMotor().getValue(), Constants.Swerve.wheelCircumference), - // Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); Rotation2d.fromRotations(io.getPositionAngleMotor().getValue())); } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 86ed4d3..6c13b4e 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -18,7 +18,7 @@ public class SwerveModuleReal implements SwerveModuleIO { private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, - Rotation2d angleOffset, SwerveModuleIO io) { + Rotation2d angleOffset) { angleEncoder = new CANcoder(cancoderID); mDriveMotor = new TalonFX(driveMotorID); @@ -100,30 +100,37 @@ private void configAngleEncoder() { angleEncoder.getConfigurator().apply(swerveCANcoderConfig); } + @Override public void setAngleMotor(ControlRequest request) { mAngleMotor.setControl(request); } + @Override public void setDriveMotor(ControlRequest request) { mDriveMotor.setControl(request); } + @Override public StatusSignal getAbsolutePositionAngleEncoder() { return angleEncoder.getAbsolutePosition(); } + @Override public void setPositionAngleMotor(double absolutePosition) { mAngleMotor.setPosition(absolutePosition); } + @Override public StatusSignal getVelocityDriveMotor() { return mDriveMotor.getVelocity(); } + @Override public StatusSignal getPositionAngleMotor() { return mAngleMotor.getPosition(); } + @Override public StatusSignal getPositionDriveMotor() { return mDriveMotor.getPosition(); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 38d7344..e163395 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -59,7 +59,7 @@ 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(); + m_robotContainer = new RobotContainer(isReal()); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ded8d30..9fef19d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.TeleopSwerve; 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 @@ -32,10 +34,15 @@ public class RobotContainer { private final SendableChooser autoChooser = new SendableChooser<>(); /* 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() { + public RobotContainer(boolean isReal) { + if (isReal) { + 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)); diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index a6073ac..ecf371a 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -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 326d2e9..d80dd7c 100644 --- a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java +++ b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; 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/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 254f778..1713d21 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.util.swerve.SwerveModule; -import frc.lib.util.swerve.SwerveModuleIO; import frc.robot.Constants; /** @@ -31,20 +30,20 @@ public class Swerve extends SubsystemBase { /** * Swerve Subsystem */ - public Swerve(SwerveIO swerveIO, SwerveModuleIO swerveModuleIO) { + public Swerve(SwerveIO swerveIO) { swerveMods = new SwerveModule[] { - new SwerveModule(0, Constants.Swerve.Mod0.driveMotorID, + swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID, Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID, - Constants.Swerve.Mod0.angleOffset, swerveModuleIO), - new SwerveModule(1, Constants.Swerve.Mod1.driveMotorID, + Constants.Swerve.Mod0.angleOffset), + swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.driveMotorID, Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID, - Constants.Swerve.Mod1.angleOffset, swerveModuleIO), - new SwerveModule(2, Constants.Swerve.Mod2.driveMotorID, + Constants.Swerve.Mod1.angleOffset), + swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.driveMotorID, Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID, - Constants.Swerve.Mod2.angleOffset, swerveModuleIO), - new SwerveModule(3, Constants.Swerve.Mod3.driveMotorID, + Constants.Swerve.Mod2.angleOffset), + swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID, Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID, - Constants.Swerve.Mod3.angleOffset, swerveModuleIO)}; + Constants.Swerve.Mod3.angleOffset)}; swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions()); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java index 4780e36..8edf500 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -1,6 +1,7 @@ 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; @@ -16,9 +17,12 @@ public static class SwerveInputs { public default void updateInputs(SwerveInputs inputs) {} - public default SwerveModule createSwerveModule(int moduleNumber, - frc.lib.util.swerve.SwerveModuleConstants constants) { - return new SwerveModule(moduleNumber, constants, new SwerveModuleIO() {}); + 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 index 2e87215..4c4258b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.swerve; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; 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 */ @@ -21,8 +22,11 @@ public void updateInputs(SwerveInputs inputs) { } @Override - public SwerveModule createSwerveModule(int moduleNumber, SwerveModuleConstants constants) { - return new SwerveModule(moduleNumber, constants, new SwerveModuleReal(constants)); + 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)); } }