Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
refactor(swerve): Remove swerve constants.
Browse files Browse the repository at this point in the history
  • Loading branch information
@haydenheroux
haydenheroux committed Apr 28, 2024
1 parent 4acf4b5 commit b8dd291
Show file tree
Hide file tree
Showing 4 changed files with 125 additions and 124 deletions.
17 changes: 10 additions & 7 deletions src/main/java/frc/lib/controller/SwerveModuleIOCustom.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import frc.lib.CAN;
import frc.lib.config.MechanismConfig;
import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
import frc.robot.swerve.SwerveConstants.MK4iConstants;
import frc.robot.swerve.SwerveFactory;

/** Custom swerve module. */
Expand All @@ -25,14 +25,17 @@ public class SwerveModuleIOCustom implements SwerveModuleIO {
/** Drive motor values. */
private final VelocityControllerIOValues driveMotorValues = new VelocityControllerIOValues();

private final double wheelCircumference = Units.inchesToMeters(4.0) * Math.PI;

/** Module setpoint */
private SwerveModuleState setpoint;

public SwerveModuleIOCustom(CAN steer, CAN azimuth, CAN drive, Rotation2d offset) {
steerMotor = SwerveFactory.createSteerMotor(steer, azimuth, offset);
public SwerveModuleIOCustom(
CAN steer, CAN azimuth, CAN drive, MechanismConfig steerConfig, MechanismConfig driveConfig) {
steerMotor = SwerveFactory.createSteerMotor(steer, azimuth, steerConfig);
steerMotor.configure();

driveMotor = SwerveFactory.createDriveMotor(drive);
driveMotor = SwerveFactory.createDriveMotor(drive, driveConfig);
driveMotor.configure();

setpoint = new SwerveModuleState();
Expand All @@ -44,7 +47,7 @@ public SwerveModuleState getState() {
driveMotor.update(driveMotorValues);

return new SwerveModuleState(
driveMotorValues.velocityRotationsPerSecond * MK4iConstants.WHEEL_CIRCUMFERENCE,
driveMotorValues.velocityRotationsPerSecond * wheelCircumference,
Rotation2d.fromRotations(steerMotorValues.positionRotations));
}

Expand All @@ -58,7 +61,7 @@ public void setSetpoint(SwerveModuleState setpoint, boolean lazy) {
setpoint = optimize(setpoint, getState(), lazy);

steerMotor.setSetpoint(setpoint.angle.getRotations(), 0);
driveMotor.setSetpoint(setpoint.speedMetersPerSecond / MK4iConstants.WHEEL_CIRCUMFERENCE);
driveMotor.setSetpoint(setpoint.speedMetersPerSecond / wheelCircumference);

this.setpoint = setpoint;
}
Expand Down Expand Up @@ -106,7 +109,7 @@ public SwerveModulePosition getPosition() {
driveMotor.update(driveMotorValues);

return new SwerveModulePosition(
driveMotorValues.positionRotations * MK4iConstants.WHEEL_CIRCUMFERENCE,
driveMotorValues.positionRotations * wheelCircumference,
Rotation2d.fromRotations(steerMotorValues.positionRotations));
}
}
117 changes: 82 additions & 35 deletions src/main/java/frc/robot/swerve/Swerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,5 @@
package frc.robot.swerve;

import java.util.function.Function;

import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
Expand All @@ -16,10 +14,15 @@
import frc.lib.DriveRequest;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
import frc.lib.config.FeedbackControllerConfig;
import frc.lib.config.FeedforwardControllerConfig;
import frc.lib.config.MechanismConfig;
import frc.lib.config.MotionProfileConfig;
import frc.lib.config.MotorConfig;
import frc.lib.controller.SwerveModuleIO;
import frc.robot.RobotConstants;
import frc.robot.odometry.Odometry;
import java.util.function.Function;

/** Swerve subsystem. */
public class Swerve extends Subsystem {
Expand All @@ -33,23 +36,59 @@ public class Swerve extends Subsystem {
/** Swerve kinematics. */
private final SwerveDriveKinematics swerveKinematics;

/** Steer motor config. */
private final MechanismConfig steerConfig =
new MechanismConfig()
.withMotorConfig(
new MotorConfig()
.withCCWPositive(false)
.withCurrentLimit(20)
.withMotorToMechanismRatio(150.0 / 7.0))
.withFeedforwardConfig(
new FeedforwardControllerConfig().withStaticFeedforward(0.205) // volts
)
.withFeedbackConfig(
new FeedbackControllerConfig()
.withContinuousInput(true)
.withProportionalGain(54.0) // volts per rotation
.withDerivativeGain(0.16) // volts per rotation per second
.withPositionTolerance(Units.degreesToRotations(1)) // rotations
);

/** Drive motor config. */
private final MechanismConfig driveConfig =
new MechanismConfig()
.withMotorConfig(
new MotorConfig()
.withCCWPositive(false)
.withCurrentLimit(40)
.withMotorToMechanismRatio(6.75))
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.14) // volts
.withVelocityFeedforward(0.725) // volts per rotation per second
)
.withFeedbackConfig(
new FeedbackControllerConfig()
.withProportionalGain(0.75) // volts per rotation per second
);

/** Translation motion profile config. */
private final MotionProfileConfig translationMotionProfileConfig =
new MotionProfileConfig()
.withMaximumVelocity(4.5) // meters per second
.withMaximumAcceleration(18); // meters per second per second
.withMaximumVelocity(4.5) // meters per second
.withMaximumAcceleration(18); // meters per second per second

/** Rotation motion profile config. */
// TODO Verify
private final MotionProfileConfig rotationMotionProfileConfig =
new MotionProfileConfig().withMaximumVelocity(1); // rotations per second

/** Initializes the swerve subsystem and configures swerve hardware. */
private Swerve() {
swerveModules[0] = SwerveFactory.createNorthWestModule();
swerveModules[1] = SwerveFactory.createNorthEastModule();
swerveModules[2] = SwerveFactory.createSouthEastModule();
swerveModules[3] = SwerveFactory.createSouthWestModule();
swerveModules[0] = SwerveFactory.createNorthWestModule(steerConfig, driveConfig);
swerveModules[1] = SwerveFactory.createNorthEastModule(steerConfig, driveConfig);
swerveModules[2] = SwerveFactory.createSouthEastModule(steerConfig, driveConfig);
swerveModules[3] = SwerveFactory.createSouthWestModule(steerConfig, driveConfig);

swerveKinematics =
new SwerveDriveKinematics(
Expand Down Expand Up @@ -82,8 +121,6 @@ public void addToShuffleboard(ShuffleboardTab tab) {
translationConstants.addDouble("Maximum Velocity (mps)", this::maximumTranslationVelocity);
translationConstants.addDouble(
"Maximum Accleration (mpsps)", this::maximumTranslationAcceleration);
translationConstants.addDouble(
"Wheel Circumference (m)", () -> SwerveConstants.MK4iConstants.WHEEL_CIRCUMFERENCE);

ShuffleboardLayout rotationConstants = Telemetry.addColumn(tab, "Rotation Constants");
rotationConstants.addDouble(
Expand Down Expand Up @@ -249,30 +286,40 @@ public Rotation2d maximumRotationVelocity() {
* @return a command that drives the swerve using an Xbox controller.
*/
public Command teleopDrive(CommandXboxController controller) {
final SlewRateLimiter xAccelerationLimiter = translationMotionProfileConfig.createAccelerationLimiter();
final SlewRateLimiter yAccelerationLimiter = translationMotionProfileConfig.createAccelerationLimiter();

final Function<Double, Double> rotationVelocityLimiter = rotationMotionProfileConfig.createVelocityClamper();

final Function<ChassisSpeeds, ChassisSpeeds> chassisSpeedsLimiter = chassisSpeeds -> {
return new ChassisSpeeds(
xAccelerationLimiter.calculate(chassisSpeeds.vxMetersPerSecond),
yAccelerationLimiter.calculate(chassisSpeeds.vyMetersPerSecond),
Units.rotationsToRadians(rotationVelocityLimiter.apply(Units.radiansToRotations(chassisSpeeds.omegaRadiansPerSecond)))
);
};

final Function<DriveRequest, ChassisSpeeds> chassisSpeedsGetter = request -> {
return ChassisSpeeds.fromFieldRelativeSpeeds(
request.translationAxis().getX() * translationMotionProfileConfig.maximumVelocity(),
request.translationAxis().getY() * translationMotionProfileConfig.maximumVelocity(),
request.rotationVelocityAxis() * Units.rotationsToRadians(rotationMotionProfileConfig.maximumVelocity()),
Odometry.getInstance().getDriverRelativeHeading());
};

return run(() -> {
setChassisSpeeds(chassisSpeedsLimiter.apply(chassisSpeedsGetter.apply(DriveRequest.fromController(controller))));
});
final SlewRateLimiter xAccelerationLimiter =
translationMotionProfileConfig.createAccelerationLimiter();
final SlewRateLimiter yAccelerationLimiter =
translationMotionProfileConfig.createAccelerationLimiter();

final Function<Double, Double> rotationVelocityLimiter =
rotationMotionProfileConfig.createVelocityClamper();

final Function<ChassisSpeeds, ChassisSpeeds> chassisSpeedsLimiter =
chassisSpeeds -> {
return new ChassisSpeeds(
xAccelerationLimiter.calculate(chassisSpeeds.vxMetersPerSecond),
yAccelerationLimiter.calculate(chassisSpeeds.vyMetersPerSecond),
Units.rotationsToRadians(
rotationVelocityLimiter.apply(
Units.radiansToRotations(chassisSpeeds.omegaRadiansPerSecond))));
};

final Function<DriveRequest, ChassisSpeeds> chassisSpeedsGetter =
request -> {
return ChassisSpeeds.fromFieldRelativeSpeeds(
request.translationAxis().getX() * translationMotionProfileConfig.maximumVelocity(),
request.translationAxis().getY() * translationMotionProfileConfig.maximumVelocity(),
request.rotationVelocityAxis()
* Units.rotationsToRadians(rotationMotionProfileConfig.maximumVelocity()),
Odometry.getInstance().getDriverRelativeHeading());
};

return run(
() -> {
setChassisSpeeds(
chassisSpeedsLimiter.apply(
chassisSpeedsGetter.apply(DriveRequest.fromController(controller))));
});
}

/**
Expand Down
63 changes: 0 additions & 63 deletions src/main/java/frc/robot/swerve/SwerveConstants.java
View file

This file was deleted.

Loading

0 comments on commit b8dd291

Please sign in to comment.