diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 97938bbc..7615d5b4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -18,7 +18,8 @@ import org.team1540.robot2024.util.LocalADStarAK; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Drivetrain.*; public class Drivetrain extends SubsystemBase { private final GyroIO gyroIO; @@ -48,7 +49,7 @@ public Drivetrain( 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()); @@ -108,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]; @@ -201,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; } /** @@ -216,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/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java index 44dc2849..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,6 +8,8 @@ import org.team1540.robot2024.Constants; import org.littletonrobotics.junction.Logger; +import static org.team1540.robot2024.Constants.Drivetrain.*; + public class Module { private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); @@ -78,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)); @@ -99,7 +101,7 @@ public SwerveModuleState runSetpoint(SwerveModuleState state, boolean forceTurn) 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; @@ -152,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; } /** 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 96641b8e..2930a4dd 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java @@ -4,9 +4,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.team1540.robot2024.Constants; import static org.team1540.robot2024.Constants.Drivetrain.*; +import static org.team1540.robot2024.Constants.*; /** * Physics sim implementation of module IO. @@ -24,8 +24,8 @@ public class ModuleIOSim implements ModuleIO { @Override public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(Constants.LOOP_PERIOD_SECS); - turnSim.update(Constants.LOOP_PERIOD_SECS); + driveSim.update(LOOP_PERIOD_SECS); + turnSim.update(LOOP_PERIOD_SECS); inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); 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 9e798ad0..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 @@ -74,14 +75,14 @@ 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 = 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 = turnCurrent.getValueAsDouble(); } @@ -107,7 +108,7 @@ public void setDriveBrakeMode(boolean enable) { @Override public void setTurnBrakeMode(boolean enable) { MotorOutputConfigs config = new MotorOutputConfigs(); - config.Inverted = Constants.Drivetrain.IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + config.Inverted = IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; turnTalon.getConfigurator().apply(config); }