diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a390c3fb..a3f67645 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,8 @@ package frc.robot; +import static java.lang.Math.PI; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -23,6 +25,12 @@ */ public final class Constants { public static final Mode currentMode = Mode.SIM; + // *START OF CONSTANTS FOR SWERVE* (STILL MISSING WHEEL POSITIONS - Im not on the cad thingy so i + // cant see it) +// double WHEEL_RADIUS = 0.0508; +// double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * 2 * PI; +// double DRIVE_GEAR_RATIO = 6.75; +// double STEERING_RATIO = (150 / 7); public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e3597cbf..196aecde 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -36,8 +36,8 @@ public class Drive extends SubsystemBase { private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + private static final double TRACK_WIDTH_X = Units.inchesToMeters(20.75); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.75); private static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; @@ -77,10 +77,11 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), Pathfinding.setPathfinder(new LocalADStarAK()); //noinspection ToArrayCallWithZeroLengthArrayArgument 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() { @@ -117,8 +118,8 @@ public void periodic() { // If the gyro is connected, replace the theta component of the twist // with the change in angle since the last loop cycle. twist = - new Twist2d( - twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians()); + new Twist2d( + twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians()); lastGyroRotation = gyroInputs.yawPosition; } // Apply the twist (change since last loop cycle) to the current pose diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index ac3372ab..6279fe3d 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -17,8 +17,7 @@ import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - default void updateInputs(GyroIOInputs inputs) { - } + default void updateInputs(GyroIOInputs inputs) {} @AutoLog class GyroIOInputs { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 349ce9a0..6996faa7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -164,9 +164,7 @@ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** - * Returns the module position delta since the last call to this method. - */ + /** Returns the module position delta since the last call to this method. */ public SwerveModulePosition getPositionDelta() { var delta = new SwerveModulePosition(getPositionMeters() - lastPositionMeters, getAngle()); lastPositionMeters = getPositionMeters(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 3d04df5e..06f07a61 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,35 +17,20 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - /** - * Updates the set of loggable inputs. - */ - default void updateInputs(ModuleIOInputs inputs) { - } + /** Updates the set of loggable inputs. */ + default void updateInputs(ModuleIOInputs inputs) {} - /** - * Run the drive motor at the specified voltage. - */ - default void setDriveVoltage(double volts) { - } + /** Run the drive motor at the specified voltage. */ + default void setDriveVoltage(double volts) {} - /** - * Run the turn motor at the specified voltage. - */ - default void setTurnVoltage(double volts) { - } + /** Run the turn motor at the specified voltage. */ + default void setTurnVoltage(double volts) {} - /** - * Enable or disable brake mode on the drive motor. - */ - default void setDriveBrakeMode(boolean enable) { - } + /** Enable or disable brake mode on the drive motor. */ + default void setDriveBrakeMode(boolean enable) {} - /** - * Enable or disable brake mode on the turn motor. - */ - default void setTurnBrakeMode(boolean enable) { - } + /** Enable or disable brake mode on the turn motor. */ + default void setTurnBrakeMode(boolean enable) {} @AutoLog class ModuleIOInputs { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 987eeced..90693598 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -48,6 +48,7 @@ public class ModuleIOSparkMax implements ModuleIO { @SuppressWarnings("FieldCanBeLocal") private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; public ModuleIOSparkMax(int index) { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index a58a0a24..a0a6793f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -57,6 +57,7 @@ public class ModuleIOTalonFX implements ModuleIO { // Gear ratios for SDS MK4i L2, adjust as necessary @SuppressWarnings("FieldCanBeLocal") private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + @SuppressWarnings("FieldCanBeLocal") private final double TURN_GEAR_RATIO = 150.0 / 7.0; @@ -119,7 +120,7 @@ public ModuleIOTalonFX(int index) { turnCurrent = turnTalon.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, drivePosition, turnPosition); // Required for odometry, use faster rate + 100.0, drivePosition, turnPosition); // Required for odometry, use faster rate BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity,