diff --git a/src/main/java/org/frc6423/frc2025/Constants.java b/src/main/java/org/frc6423/frc2025/Constants.java index da43005..6d8330f 100644 --- a/src/main/java/org/frc6423/frc2025/Constants.java +++ b/src/main/java/org/frc6423/frc2025/Constants.java @@ -12,6 +12,8 @@ /** A class */ public class Constants { + public static final double kTickSpeed = 0.02; + // * Select which robot to initalize private static final RobotType m_robotType = RobotType.SIMULATED; diff --git a/src/main/java/org/frc6423/frc2025/subsystems/swerve/SwerveSubsystem.java b/src/main/java/org/frc6423/frc2025/subsystems/swerve/SwerveSubsystem.java new file mode 100644 index 0000000..c82c8be --- /dev/null +++ b/src/main/java/org/frc6423/frc2025/subsystems/swerve/SwerveSubsystem.java @@ -0,0 +1,113 @@ +package org.frc6423.frc2025.subsystems.swerve; + +import static org.frc6423.frc2025.Constants.kTickSpeed; + +import java.util.Arrays; + +import org.frc6423.frc2025.subsystems.swerve.module.Module; +import org.frc6423.frc2025.util.swerveUtil.SwerveConfig; + +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SwerveSubsystem extends SubsystemBase { + private final SwerveConfig m_config; + + private final Module[] m_modules; + + private SwerveDriveKinematics m_kinematics; + private SwerveDrivePoseEstimator m_poseEstimator; + + public SwerveSubsystem(SwerveConfig config) { + // Create modules + var moduleConfigs = config.getModuleConfigs(); + m_modules = new Module[moduleConfigs.length]; + Arrays.stream(moduleConfigs) + .forEach((c) -> m_modules[c.kIndex] = new Module(c)); + + // Create math objects + m_kinematics = new SwerveDriveKinematics(config.getModuleLocs()); + m_poseEstimator = new SwerveDrivePoseEstimator(m_kinematics, getHeading(), getModulePoses(), new Pose2d()); + + m_config = config; + } + + @Override + public void periodic() { + + // Stop module input when driverstation is disabled + if (DriverStation.isDisabled()) { + for (Module module : m_modules) { + module.stop(); + } + } + } + + /** + * Runs swerve at desired robot relative velocity + * + * @param velocity desired velocity + * @param openloopEnabled enable or disable open loop voltage control + */ + public void runVelocities(ChassisSpeeds velocity, boolean openloopEnabled) { + velocity = ChassisSpeeds.discretize(velocity, kTickSpeed); + + SwerveModuleState[] desiredStates = m_kinematics.toSwerveModuleStates(velocity); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, m_config.getMaxLinearSpeedMetersPerSec()); + + for(int i = 0; i < desiredStates.length; i++) { + if (openloopEnabled) { + ChassisSpeeds currentVelocities = getVelocitiesRobotRelative(); + boolean focEnabled = + Math.sqrt( + Math.pow(currentVelocities.vxMetersPerSecond, 2) + + Math.pow(currentVelocities.vyMetersPerSecond, 2)) // converts linear velocity components to linear velocity + < 1 * m_config.getMaxLinearSpeedMetersPerSec(); + + // Converts desired motor velocity into input voltage + // OmegaRadsPerSec/(KvRadsPerVolt) + double driveVoltage = desiredStates[i].speedMetersPerSecond / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt; + m_modules[i].runSetpointOpenloop(driveVoltage, desiredStates[i].angle, focEnabled); + } else { + m_modules[i].runSetpoint(desiredStates[i]); + } + } + } + + /** Gets current robot velocity (robot relative) */ + public ChassisSpeeds getVelocitiesRobotRelative() { + return m_kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Gets current robot heading */ + public Rotation2d getHeading() { + return new Rotation2d(); + } + + /** Gets current robot field pose */ + public Pose2d getPose() { + return m_poseEstimator.getEstimatedPosition(); + } + + /** Returns an array of module field positions */ + public SwerveModulePosition[] getModulePoses() { + return Arrays.stream(m_modules) + .map(Module::getModulePose) + .toArray(SwerveModulePosition[]::new); + } + + /** Returns an array of module states */ + public SwerveModuleState[] getModuleStates() { + return Arrays.stream(m_modules) + .map(Module::getModuleState) + .toArray(SwerveModuleState[]::new); + } +} diff --git a/src/main/java/org/frc6423/frc2025/subsystems/swerve/constants/CompBotSwerveConfigs.java b/src/main/java/org/frc6423/frc2025/subsystems/swerve/constants/CompBotSwerveConfigs.java index 68331db..208fa73 100644 --- a/src/main/java/org/frc6423/frc2025/subsystems/swerve/constants/CompBotSwerveConfigs.java +++ b/src/main/java/org/frc6423/frc2025/subsystems/swerve/constants/CompBotSwerveConfigs.java @@ -1,5 +1,7 @@ package org.frc6423.frc2025.subsystems.swerve.constants; +import java.util.Arrays; + import org.frc6423.frc2025.util.swerveUtil.ModuleConfig; import org.frc6423.frc2025.util.swerveUtil.SwerveConfig; @@ -94,7 +96,7 @@ public double getWheelRadiusInches() { @Override public ModuleConfig[] getModuleConfigs() { - return new ModuleConfig[] { + var configs = new ModuleConfig[] { new ModuleConfig( 1, 1, @@ -132,6 +134,11 @@ public ModuleConfig[] getModuleConfigs() { getPivotConfigSparkMax(), getDriveConfigSparkMax()), }; + + Arrays.stream(configs) + .forEach((c) -> c.kWheelRadiusMeters = Units.inchesToMeters(getWheelRadiusInches())); // Shut up + + return configs; } // Gains diff --git a/src/main/java/org/frc6423/frc2025/subsystems/swerve/module/Module.java b/src/main/java/org/frc6423/frc2025/subsystems/swerve/module/Module.java index 8740130..c86fe24 100644 --- a/src/main/java/org/frc6423/frc2025/subsystems/swerve/module/Module.java +++ b/src/main/java/org/frc6423/frc2025/subsystems/swerve/module/Module.java @@ -5,7 +5,9 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; public class Module { private final ModuleIO m_IO; @@ -35,13 +37,13 @@ public void updateInputs() { /** Periodically ran logic */ public void periodic() { - Logger.processInputs("Swerve/Module" + m_index, m_inputs); + Logger.processInputs("Swerve/Module" + getModuleIndex(), m_inputs); } /** Run SwerveModuleState setpoint */ public SwerveModuleState runSetpoint(SwerveModuleState setpointState) { setpointState.optimize(getPivotAngle()); - setpointState.speedMetersPerSecond *= setpointState.angle.minus(getPivotAngle()).getCos(); + setpointState.speedMetersPerSecond *= Math.cos(setpointState.angle.minus(getPivotAngle()).getRadians()); double speedMPS = setpointState.speedMetersPerSecond; m_IO.setPivotAngle(setpointState.angle); @@ -56,9 +58,11 @@ public SwerveModuleState runSetpoint(SwerveModuleState setpointState, double dri } /** Runs SwerveModuleState setpoint but runs drive in open loop mode */ - public SwerveModuleState runSetpointOpenloop(SwerveModuleState setpointState, boolean FOCEnabled) { + public SwerveModuleState runSetpointOpenloop(double voltage, Rotation2d angle, boolean FOCEnabled) { + SwerveModuleState setpointState = new SwerveModuleState(voltage, angle); + setpointState.optimize(getPivotAngle()); - setpointState.speedMetersPerSecond *= setpointState.angle.minus(getPivotAngle()).getCos(); + setpointState.speedMetersPerSecond *= Math.cos(setpointState.angle.minus(getPivotAngle()).getRadians()); double speedMPS = setpointState.speedMetersPerSecond; m_IO.setPivotAngle(setpointState.angle); @@ -92,8 +96,33 @@ public int getModuleIndex() { return m_index; } + /** Get Module config */ + public ModuleConfig getModuleConfig() { + return m_config; + } + + /** Gets Drive Speed in MPS */ + public double getVelMetersPerSec() { + return m_inputs.driveVelRadsPerSec * m_config.kWheelRadiusMeters; + } + /** returns current module angle */ public Rotation2d getPivotAngle() { return m_inputs.pivotABSPose; } + + /** Returns drive pose in meters */ + public double getPoseMeters() { + return m_inputs.drivePoseRads * m_config.kWheelRadiusMeters; + } + + /** Returns swerve field pose */ + public SwerveModulePosition getModulePose() { + return new SwerveModulePosition(getPoseMeters(), getPivotAngle()); + } + + /** Returns Module state */ + public SwerveModuleState getModuleState() { + return new SwerveModuleState(getVelMetersPerSec(), getPivotAngle()); + } } diff --git a/src/main/java/org/frc6423/frc2025/util/swerveUtil/ModuleConfig.java b/src/main/java/org/frc6423/frc2025/util/swerveUtil/ModuleConfig.java index 4ddcc07..4d0ad11 100644 --- a/src/main/java/org/frc6423/frc2025/util/swerveUtil/ModuleConfig.java +++ b/src/main/java/org/frc6423/frc2025/util/swerveUtil/ModuleConfig.java @@ -12,6 +12,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; public class ModuleConfig { public int kIndex; @@ -29,6 +30,8 @@ public class ModuleConfig { public SparkMaxConfig kPivotConfigSparkMax, kDriveConfigSparkMax; + public double kWheelRadiusMeters = Units.inchesToMeters(2); + /** Create a new TalonFX modul */ public ModuleConfig( int index,