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

Commit

Permalink
chore: Format.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 28, 2024
1 parent f255838 commit 5eea860
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 20 deletions.
5 changes: 2 additions & 3 deletions src/main/java/frc/lib/config/MotionProfileConfig.java
Original file line number Diff line number Diff line change
@@ -1,11 +1,10 @@
package frc.lib.config;

import java.util.function.Function;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import java.util.function.Function;

/** Motion profile config. */
public class MotionProfileConfig {
Expand Down Expand Up @@ -70,7 +69,7 @@ public double maximumAcceleration() {

/**
* Creates a new velocity clamper using this motion profile config.
*
*
* @return a new velocity clamper using this motion profile config.
*/
public Function<Double, Double> createVelocityClamper() {
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/shooter/Shooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,8 @@ private Shooter() {

serializerValues = new VelocityControllerIOValues();

serializerAccelerationLimiter = serializerConfig.motionProfileConfig().createAccelerationLimiter();
serializerAccelerationLimiter =
serializerConfig.motionProfileConfig().createAccelerationLimiter();

setpoint = ShooterState.IDLING;
goal = ShooterState.IDLING;
Expand Down
15 changes: 9 additions & 6 deletions src/main/java/frc/robot/swerve/DriveCommand.java
Original file line number Diff line number Diff line change
@@ -1,13 +1,12 @@
package frc.robot.swerve;

import java.util.function.Function;

import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.odometry.Odometry;
import java.util.function.Function;

/** Drives the swerve using driver input. */
public class DriveCommand extends Command {
Expand All @@ -22,7 +21,7 @@ public class DriveCommand extends Command {

/** Translation acceleration limiter. */
private final SlewRateLimiter xAccelerationLimiter;

/** Translation acceleration limiter. */
private final SlewRateLimiter yAccelerationLimiter;

Expand Down Expand Up @@ -86,9 +85,13 @@ public boolean isFinished() {
* @return the clamped chassis speeds.
*/
private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) {
double vxMetersPerSecond = xAccelerationLimiter.calculate(desiredChassisSpeeds.vxMetersPerSecond);
double vyMetersPerSecond = yAccelerationLimiter.calculate(desiredChassisSpeeds.vyMetersPerSecond);
double omegaRadiansPerSecond = rotationVelocityClamper.apply(Units.radiansToRotations(desiredChassisSpeeds.omegaRadiansPerSecond));
double vxMetersPerSecond =
xAccelerationLimiter.calculate(desiredChassisSpeeds.vxMetersPerSecond);
double vyMetersPerSecond =
yAccelerationLimiter.calculate(desiredChassisSpeeds.vyMetersPerSecond);
double omegaRadiansPerSecond =
rotationVelocityClamper.apply(
Units.radiansToRotations(desiredChassisSpeeds.omegaRadiansPerSecond));

return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
}
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/swerve/DriveRequest.java
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,7 @@ public static DriveRequest fromController(CommandXboxController controller) {

TranslationMode translationMode = TranslationMode.FIELD_CENTRIC;

Translation2d headingAxis =
new Translation2d(-controller.getRightY(), -controller.getRightX());
Translation2d headingAxis = new Translation2d(-controller.getRightY(), -controller.getRightX());

RotationMode rotationMode;

Expand All @@ -94,6 +93,7 @@ public static DriveRequest fromController(CommandXboxController controller) {
rotationVelocityAxis = headingAxis.getY();
}

return new DriveRequest(translationMode, rotationMode, translationAxis, headingAxis, rotationVelocityAxis);
return new DriveRequest(
translationMode, rotationMode, translationAxis, headingAxis, rotationVelocityAxis);
}
}
18 changes: 11 additions & 7 deletions src/main/java/frc/robot/swerve/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,14 @@ public class Swerve extends Subsystem {
private final SwerveDriveKinematics swerveKinematics;

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

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

/** Initializes the swerve subsystem and configures swerve hardware. */
private Swerve() {
Expand Down Expand Up @@ -188,7 +192,7 @@ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) {

/**
* Returns the motion profile config.
*
*
* @return the motion profile config.
*/
public MotionProfileConfig translationMotionProfileConfig() {
Expand All @@ -197,7 +201,7 @@ public MotionProfileConfig translationMotionProfileConfig() {

/**
* Returns the maximum translation velocity.
*
*
* @return the maximum translation velocity.
*/
public double maximumTranslationVelocity() {
Expand All @@ -206,7 +210,7 @@ public double maximumTranslationVelocity() {

/**
* Returns the maximum translation acceleration.
*
*
* @return the maximum translation acceleration.
*/
public double maximumTranslationAcceleration() {
Expand All @@ -215,7 +219,7 @@ public double maximumTranslationAcceleration() {

/**
* Returns the rotation motion profile config.
*
*
* @return the rotation motion profile config.
*/
public MotionProfileConfig rotationMotionProfileConfig() {
Expand All @@ -224,7 +228,7 @@ public MotionProfileConfig rotationMotionProfileConfig() {

/**
* Returns the maximum rotation velocity.
*
*
* @return the maximum rotation velocity.
*/
public Rotation2d maximumRotationVelocity() {
Expand Down

0 comments on commit 5eea860

Please sign in to comment.