Skip to content

Instantly share code, notes, and snippets.

Show Gist options
  • Save pleasehelpme798/4d4e7d45a38040e6dddb09c4f6a392c8 to your computer and use it in GitHub Desktop.
Save pleasehelpme798/4d4e7d45a38040e6dddb09c4f6a392c8 to your computer and use it in GitHub Desktop.
Code with line 11 fixed
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