From 7d29459e3a861bf77a224579dcfc68e1d4dedc7d Mon Sep 17 00:00:00 2001 From: Alvin Zhang <114535782+mimizh2418@users.noreply.github.com> Date: Tue, 16 Jan 2024 14:34:57 -0800 Subject: [PATCH] Swerve actually works this time (#2) * fix: module offsets * refactor: rename Drivetrain, remove usage of var keyword * feat: put drive command into SwerveDriveCommand * chore: remove debug print * fix: swerve motor current is no longer logged as an array * fix: use gear ratio from constants in sim * refactor: less verbose constants --- .../org/team1540/robot2024/Constants.java | 2 + .../java/org/team1540/robot2024/Robot.java | 5 +- .../team1540/robot2024/RobotContainer.java | 55 ++++++---------- .../robot2024/commands/DriveCommands.java | 49 -------------- .../commands/SwerveDriveCommand.java | 66 +++++++++++++++++++ .../drive/{Drive.java => Drivetrain.java} | 47 ++++++------- .../subsystems/drive/GyroIONavx.java | 1 - .../robot2024/subsystems/drive/Module.java | 16 ++--- .../robot2024/subsystems/drive/ModuleIO.java | 4 +- .../subsystems/drive/ModuleIOSim.java | 12 ++-- .../subsystems/drive/ModuleIOTalonFX.java | 26 ++++---- .../robot2024/util/swerve/SwerveFactory.java | 18 ++--- 12 files changed, 148 insertions(+), 153 deletions(-) delete mode 100644 src/main/java/org/team1540/robot2024/commands/DriveCommands.java create mode 100644 src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java rename src/main/java/org/team1540/robot2024/subsystems/drive/{Drive.java => Drivetrain.java} (81%) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 57ee7115..b42c21b3 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -30,6 +30,8 @@ public enum Mode { REPLAY } + public static final double LOOP_PERIOD_SECS = 0.02; + public static class Drivetrain { public static final String CAN_BUS = ""; public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index f86b2365..e4126ac9 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -1,7 +1,5 @@ package org.team1540.robot2024; -import com.pathplanner.lib.commands.PathPlannerAuto; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -39,7 +37,7 @@ public void robotInit() { Logger.recordMetadata("GitDirty", "All changes committed"); break; case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); + Logger.recordMetadata("GitDirty", "Uncommitted changes"); break; default: Logger.recordMetadata("GitDirty", "Unknown"); @@ -113,7 +111,6 @@ public void disabledPeriodic() { @Override public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); - robotContainer.drive.setPose(PathPlannerAuto.getStaringPoseFromAutoFile("TestAuto")); // schedule the autonomous command (example) if (autonomousCommand != null) { autonomousCommand.schedule(); diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 2c1f2b53..79c5ff63 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.team1540.robot2024.commands.DriveCommands; import org.team1540.robot2024.commands.FeedForwardCharacterization; +import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -23,11 +23,11 @@ */ public class RobotContainer { // Subsystems - public final Drive drive; + public final Drivetrain drivetrain; // Controller - public final CommandXboxController controller = new CommandXboxController(0); - public final CommandXboxController controllerCopilot = new CommandXboxController(1); + public final CommandXboxController driver = new CommandXboxController(0); + public final CommandXboxController copilot = new CommandXboxController(1); // Dashboard inputs public final LoggedDashboardChooser autoChooser; @@ -39,28 +39,20 @@ public RobotContainer() { switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations - drive = - new Drive( + drivetrain = + new Drivetrain( new GyroIONavx(), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(1, SwerveFactory.SwerveCorner.FRONT_LEFT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(7, SwerveFactory.SwerveCorner.FRONT_RIGHT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(4, SwerveFactory.SwerveCorner.BACK_LEFT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(3, SwerveFactory.SwerveCorner.BACK_RIGHT))); - // drive = new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); - // flywheel = new Flywheel(new FlywheelIOTalonFX()); + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(3, SwerveFactory.SwerveCorner.FRONT_LEFT)), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(4, SwerveFactory.SwerveCorner.FRONT_RIGHT)), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(7, SwerveFactory.SwerveCorner.BACK_LEFT)), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(1, SwerveFactory.SwerveCorner.BACK_RIGHT))); break; case SIM: // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() { - }, + drivetrain = + new Drivetrain( + new GyroIO() {}, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), @@ -69,8 +61,8 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations - drive = - new Drive( + drivetrain = + new Drivetrain( new GyroIO() { }, new ModuleIO() { @@ -92,7 +84,7 @@ public RobotContainer() { autoChooser.addOption( "Drive FF Characterization", new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity)); + drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity)); // Configure the button bindings configureButtonBindings(); @@ -105,17 +97,12 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller.b().onTrue( + drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); + driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); + driver.b().onTrue( Commands.runOnce( - () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive + () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())), + drivetrain ).ignoringDisable(true) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/DriveCommands.java b/src/main/java/org/team1540/robot2024/commands/DriveCommands.java deleted file mode 100644 index d9f29287..00000000 --- a/src/main/java/org/team1540/robot2024/commands/DriveCommands.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.team1540.robot2024.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.subsystems.drive.Drive; - -import java.util.function.DoubleSupplier; - -public class DriveCommands { - private static final double DEADBAND = 0.1; - - private DriveCommands() { - } - - /** - * Field relative drive command using two joysticks (controlling linear and angular velocities). - */ - public static Command joystickDrive(Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { - return Commands.run(() -> { - // Apply deadband - double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); - Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calculate new linear velocity - Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection).transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); - - // Convert to field relative speeds & send command - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), - drive.getRotation() - ) - ); - }, drive); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java new file mode 100644 index 00000000..a595f2a0 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java @@ -0,0 +1,66 @@ +package org.team1540.robot2024.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.robot2024.subsystems.drive.Drivetrain; + +public class SwerveDriveCommand extends Command { + private final Drivetrain drivetrain; + private final CommandXboxController controller; + + private final SlewRateLimiter xLimiter = new SlewRateLimiter(2); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); + private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); + + public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) { + this.drivetrain = drivetrain; + this.controller = controller; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + xLimiter.reset(0); + yLimiter.reset(0); + rotLimiter.reset(0); + } + + @Override + public void execute() { + double xPercent = xLimiter.calculate(-controller.getLeftY()); + double yPercent = yLimiter.calculate(-controller.getLeftX()); + double rotPercent = rotLimiter.calculate(-controller.getRightX()); + + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xPercent, yPercent), 0.1); + Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); + double omega = MathUtil.applyDeadband(rotPercent, 0.1); + + // Calculate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); + + // Convert to field relative speeds & send command + drivetrain.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(), + omega * drivetrain.getMaxAngularSpeedRadPerSec(), + drivetrain.getRotation() + ) + ); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java similarity index 81% rename from src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java rename to src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 2850f663..7615d5b4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -18,21 +18,20 @@ import org.team1540.robot2024.util.LocalADStarAK; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import org.team1540.robot2024.Constants; - -public class Drive extends SubsystemBase { +import static org.team1540.robot2024.Constants.Drivetrain.*; +public class Drivetrain extends SubsystemBase { private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); private boolean forceModuleAngleChange = false; - public Drive( + public Drivetrain( GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, @@ -50,31 +49,27 @@ public Drive( this::setPose, () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, - new HolonomicPathFollowerConfig(Constants.Drivetrain.MAX_LINEAR_SPEED, Constants.Drivetrain.DRIVE_BASE_RADIUS, new ReplanningConfig()), + new HolonomicPathFollowerConfig(MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red, this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); + (activePath) -> Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]))); PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); + (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); } public void periodic() { gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { + Logger.processInputs("Drivetrain/Gyro", gyroInputs); + for (Module module : modules) { module.periodic(); } // Stop moving when disabled if (DriverStation.isDisabled()) { - for (var module : modules) { + for (Module module : modules) { module.stop(); } } @@ -92,7 +87,7 @@ public void periodic() { // The twist represents the motion of the robot since the last // loop cycle in x, y, and theta based only on the modules, // without the gyro. The gyro is always disconnected in simulation. - var twist = kinematics.toTwist2d(wheelDeltas); + Twist2d twist = kinematics.toTwist2d(wheelDeltas); if (gyroInputs.connected) { // If the gyro is connected, replace the theta component of the twist // with the change in angle since the last loop cycle. @@ -114,7 +109,7 @@ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, Constants.Drivetrain.MAX_LINEAR_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; @@ -163,14 +158,14 @@ public void runCharacterizationVolts(double volts) { */ public double getCharacterizationVelocity() { double driveVelocityAverage = 0.0; - for (var module : modules) { + for (Module module : modules) { driveVelocityAverage += module.getCharacterizationVelocity(); } return driveVelocityAverage / 4.0; } /** - * Returns the module states (turn angles and drive velocities) for all of the modules. + * Returns the module states (turn angles and drive velocities) for all the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -207,14 +202,14 @@ public void setPose(Pose2d pose) { * Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return Constants.Drivetrain.MAX_LINEAR_SPEED; + return MAX_LINEAR_SPEED; } /** * Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return Constants.Drivetrain.MAX_ANGULAR_SPEED; + return MAX_ANGULAR_SPEED; } /** @@ -222,10 +217,10 @@ public double getMaxAngularSpeedRadPerSec() { */ public static Translation2d[] getModuleTranslations() { return new Translation2d[]{ - new Translation2d(Constants.Drivetrain.TRACK_WIDTH_X / 2.0, Constants.Drivetrain.TRACK_WIDTH_Y / 2.0), - new Translation2d(Constants.Drivetrain.TRACK_WIDTH_X / 2.0, -Constants.Drivetrain.TRACK_WIDTH_Y / 2.0), - new Translation2d(-Constants.Drivetrain.TRACK_WIDTH_X / 2.0, Constants.Drivetrain.TRACK_WIDTH_Y / 2.0), - new Translation2d(-Constants.Drivetrain.TRACK_WIDTH_X / 2.0, -Constants.Drivetrain.TRACK_WIDTH_Y / 2.0) + new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) }; } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java index 43b8d6c8..5a7b8721 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java @@ -13,7 +13,6 @@ public GyroIONavx(){ lastAngle = navx.getRotation2d(); } - @Override public void updateInputs(GyroIOInputs inputs) { double lastTime = inputs.time; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java index 63c6288e..b63b8b38 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java @@ -8,8 +8,9 @@ import org.team1540.robot2024.Constants; import org.littletonrobotics.junction.Logger; -public class Module { +import static org.team1540.robot2024.Constants.Drivetrain.*; +public class Module { private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; @@ -53,7 +54,6 @@ public Module(ModuleIO io, int index) { } public void periodic() { - System.out.println(driveFeedback.getP() + " " + driveFeedback.getI() + " " + driveFeedback.getD()); io.updateInputs(inputs); Logger.processInputs("Drive/Module" + index, inputs); @@ -80,7 +80,7 @@ public void periodic() { double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / Constants.Drivetrain.WHEEL_RADIUS; + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; io.setDriveVoltage( driveFeedforward.calculate(velocityRadPerSec) + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); @@ -98,10 +98,10 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) { public SwerveModuleState runSetpoint(SwerveModuleState state, boolean forceTurn) { // Optimize state based on current angle // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); + SwerveModuleState optimizedState = SwerveModuleState.optimize(state, getAngle()); // Update setpoints, controllers run in "periodic" - angleSetpoint = ((Math.abs(optimizedState.speedMetersPerSecond) <= (Constants.Drivetrain.MAX_LINEAR_SPEED * 0.01)) && !forceTurn) ? angleSetpoint : optimizedState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering. + angleSetpoint = ((Math.abs(optimizedState.speedMetersPerSecond) <= (MAX_LINEAR_SPEED * 0.01)) && !forceTurn) ? angleSetpoint : optimizedState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering. speedSetpoint = optimizedState.speedMetersPerSecond; return optimizedState; @@ -154,14 +154,14 @@ public Rotation2d getAngle() { * Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return inputs.drivePositionRad * Constants.Drivetrain.WHEEL_RADIUS; + return inputs.drivePositionRad * WHEEL_RADIUS; } /** * Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * Constants.Drivetrain.WHEEL_RADIUS; + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; } /** @@ -175,7 +175,7 @@ public SwerveModulePosition getPosition() { * Returns the module position delta since the last call to this method. */ public SwerveModulePosition getPositionDelta() { - var delta = new SwerveModulePosition(getPositionMeters() - lastPositionMeters, getAngle()); + SwerveModulePosition delta = new SwerveModulePosition(getPositionMeters() - lastPositionMeters, getAngle()); lastPositionMeters = getPositionMeters(); return delta; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java index 269aee93..bf6c6a5a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java @@ -9,13 +9,13 @@ class ModuleIOInputs { public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[]{}; + public double driveCurrentAmps = 0.0; public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[]{}; + public double turnCurrentAmps = 0.0; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java index d9277ecc..2930a4dd 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java @@ -5,6 +5,9 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import static org.team1540.robot2024.Constants.Drivetrain.*; +import static org.team1540.robot2024.Constants.*; + /** * Physics sim implementation of module IO. * @@ -13,10 +16,9 @@ * approximation for the behavior of the module. */ public class ModuleIOSim implements ModuleIO { - private static final double LOOP_PERIOD_SECS = 0.02; private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); - private final DCMotorSim driveSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 6.15, 0.025); - private final DCMotorSim turnSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 150.0 / 7.0, 0.004); + private final DCMotorSim driveSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), DRIVE_GEAR_RATIO, 0.025); + private final DCMotorSim turnSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), TURN_GEAR_RATIO, 0.004); private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; @@ -28,13 +30,13 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[]{Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[]{Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index ec9f1a42..036664da 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java @@ -11,7 +11,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import org.team1540.robot2024.util.swerve.SwerveFactory; -import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Drivetrain.*; /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and @@ -41,9 +42,6 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; - // Gear ratios for SDS MK4i L2, adjust as necessary - - private final Rotation2d absoluteEncoderOffset; public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) { @@ -53,11 +51,9 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) { cancoder = hw.cancoder; absoluteEncoderOffset = hw.cancoderOffset; - setDriveBrakeMode(true); setTurnBrakeMode(true); - drivePosition = driveTalon.getPosition(); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); @@ -79,16 +75,16 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) { public void updateInputs(ModuleIOInputs inputs) { BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition, turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / Constants.Drivetrain.DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / Constants.Drivetrain.DRIVE_GEAR_RATIO; + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[]{driveCurrent.getValueAsDouble()}; + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).plus(absoluteEncoderOffset); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / Constants.Drivetrain.TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / Constants.Drivetrain.TURN_GEAR_RATIO; + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[]{turnCurrent.getValueAsDouble()}; + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); } @Override @@ -103,7 +99,7 @@ public void setTurnVoltage(double volts) { @Override public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); + MotorOutputConfigs config = new MotorOutputConfigs(); config.Inverted = InvertedValue.CounterClockwise_Positive; config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; driveTalon.getConfigurator().apply(config); @@ -111,8 +107,8 @@ public void setDriveBrakeMode(boolean enable) { @Override public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = Constants.Drivetrain.IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + MotorOutputConfigs config = new MotorOutputConfigs(); + config.Inverted = IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; turnTalon.getConfigurator().apply(config); } diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java index 7baeab32..7a05ca20 100644 --- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java +++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java @@ -12,9 +12,9 @@ public class SwerveFactory { private static final double[] moduleOffsetsRots = new double[]{ - -0.4130859, // Module 1 + -0.9130859, // Module 1 0.0, // Module 2 - -0.2197265, // Module 3 + -0.7197265, // Module 3 -0.7722, // Module 4 0.0, // Module 5 0.0, // Module 6 @@ -27,10 +27,10 @@ public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) { } public enum SwerveCorner { - FRONT_LEFT(180), + FRONT_LEFT(0), FRONT_RIGHT(90), BACK_LEFT(270), - BACK_RIGHT(0); + BACK_RIGHT(180); private final double offset; SwerveCorner(double offset) { @@ -53,7 +53,7 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { } TalonFXConfiguration driveConfig = new TalonFXConfiguration(); TalonFXConfiguration turnConfig = new TalonFXConfiguration(); - CANcoderConfiguration canCoderconfig = new CANcoderConfiguration(); + CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; driveConfig.CurrentLimits.SupplyCurrentThreshold = 60.0; @@ -65,9 +65,9 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - canCoderconfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1]; - canCoderconfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - canCoderconfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; + canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1]; + canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; this.driveMotor = new TalonFX(30 + id, canbus); this.driveMotor.getConfigurator().apply(driveConfig); @@ -79,7 +79,7 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { this.cancoder = new CANcoder(10 + id, canbus); - this.cancoder.getConfigurator().apply(canCoderconfig); + this.cancoder.getConfigurator().apply(canCoderConfig); this.cancoderOffset = Rotation2d.fromDegrees(corner.offset);