Created
July 14, 2024 21:31
-
-
Save pleasehelpme798/4d4e7d45a38040e6dddb09c4f6a392c8 to your computer and use it in GitHub Desktop.
Code with line 11 fixed
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
package frc.robot.commands; | |
import java.util.function.Supplier; | |
import edu.wpi.first.wpilibj.SlewRateLimiter; | |
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; | |
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; | |
import edu.wpi.first.wpilibj2.command.CommandBase; | |
import frc.robot.Constants.DriveConstants; | |
import frc.robot.Constants.OIConstants; | |
import frc.robot.Subsystems.SwerveSubsystem; | |
public class SwerveJoystickCmd extends CommandBase { | |
private final SwerveSubsystem swerveSubsystem; | |
private final Supplier<Double> xSpdFunction, ySpdFunction, turningSpdFunction; | |
private final Supplier<Boolean> fieldOrientedFunction; | |
private final SlewRateLimiter xLimiter, yLimiter, turningLimiter; | |
public SwerveJoystickCmd(SwerveSubsystem swerveSubsystem, | |
Supplier<Double> xSpdFunction, Supplier<Double> ySpdFunction, Supplier<Double> turningSpdFunction, | |
Supplier<Boolean> fieldOrientedFunction) { | |
this.swerveSubsystem = swerveSubsystem; | |
this.xSpdFunction = xSpdFunction; | |
this.ySpdFunction = ySpdFunction; | |
this.turningSpdFunction = turningSpdFunction; | |
this.fieldOrientedFunction = fieldOrientedFunction; | |
this.xLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond); | |
this.yLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond); | |
this.turningLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAngularAccelerationUnitsPerSecond); | |
addRequirements(swerveSubsystem); | |
} | |
@Override | |
public void initialize() { | |
} | |
@Override | |
public void execute() { | |
// 1. Get real-time joystick inputs | |
double xSpeed = xSpdFunction.get(); | |
double ySpeed = ySpdFunction.get(); | |
double turningSpeed = turningSpdFunction.get(); | |
// 2. Apply deadband | |
xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; | |
ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; | |
turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; | |
// 3. Make the driving smoother | |
xSpeed = xLimiter.calculate(xSpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; | |
ySpeed = yLimiter.calculate(ySpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; | |
turningSpeed = turningLimiter.calculate(turningSpeed) | |
* DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond; | |
// 4. Construct desired chassis speeds | |
ChassisSpeeds chassisSpeeds; | |
if (fieldOrientedFunction.get()) { | |
// Relative to field | |
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( | |
xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d()); | |
} else { | |
// Relative to robot | |
chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed); | |
} | |
// 5. Convert chassis speeds to individual module states | |
SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); | |
// 6. Output each module states to wheels | |
swerveSubsystem.setModuleStates(moduleStates); | |
} | |
@Override | |
public void end(boolean interrupted) { | |
swerveSubsystem.stopModules(); | |
} | |
@Override | |
public boolean isFinished() { | |
return false; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment