diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c95b440..8ef9925 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -76,7 +76,7 @@ public enum Modules {; public static final double kPXAuto = 0; //TODO: find public static final double kPYAuto = 0; //TODO: find - public static final double kThetaAuto = 0; //TODO: find + public static final double kPThetaAuto = 0; //TODO: find } public static final class ModuleConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 53ac148..a12c44a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,7 @@ private void configureButtonBindings() { ); new Button(controller::getLeftBumperPressed).whenPressed(swerve.resetGyroCommand()); - new Button(controller::getRightBumperPressed).whenPressed(swerve.resetModulesCommand(), false); // TODO: test + new Button(controller::getRightBumperPressed).whenPressed(swerve.resetModulesCommand(), false); } /** @@ -64,18 +64,6 @@ private void configureButtonBindings() { * * @return the command to run in autonomous */ -// public Command getAutonomousCommand() { -// return new RunCommand( -// ()-> { -// swerve.setModulesStates( -// new SwerveModuleState[]{ -// new SwerveModuleState(0, new Rotation2d(Constants.SwerveConstants.kOffsetAngle[0])), -// new SwerveModuleState(0, new Rotation2d(Constants.SwerveConstants.kOffsetAngle[1])), -// new SwerveModuleState(0, new Rotation2d(Constants.SwerveConstants.kOffsetAngle[2])), -// new SwerveModuleState(0, new Rotation2d(Constants.SwerveConstants.kOffsetAngle[3])) -// }); -// }, swerve); -// } public Command getAutonomousCommand(){ return swerve.resetModulesCommand(); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index dc80f19..5292bba 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,151 +1,244 @@ package frc.robot.subsystems; +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; +import java.util.ArrayList; +import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import com.kauailabs.navx.frc.AHRS; - -import static frc.robot.Constants.SwerveConstants.*; import static edu.wpi.first.math.MathUtil.applyDeadband; import static frc.robot.Constants.SwerveConstants.Modules.*; +import static frc.robot.Constants.SwerveConstants.*; public class Swerve extends SubsystemBase { - //create swerve modules - private final SwerveModule[] swerveModules = { - new SwerveModule( - kDriveMotorId[FRONT_LEFT], - kSpinningMotorId[FRONT_LEFT], - kDriveMotorReversed[FRONT_LEFT], - kSpinningMotorReversed[FRONT_LEFT], - kAbsEncoderChannel[FRONT_LEFT], - kOffsetAngle[FRONT_LEFT]), - new SwerveModule( - kDriveMotorId[FRONT_RIGHT], - kSpinningMotorId[FRONT_RIGHT], - kDriveMotorReversed[FRONT_RIGHT], - kSpinningMotorReversed[FRONT_RIGHT], - kAbsEncoderChannel[FRONT_RIGHT], - kOffsetAngle[FRONT_RIGHT]), - new SwerveModule( - kDriveMotorId[BACK_LEFT], - kSpinningMotorId[BACK_LEFT], - kDriveMotorReversed[BACK_LEFT], - kSpinningMotorReversed[BACK_LEFT], - kAbsEncoderChannel[BACK_LEFT], - kOffsetAngle[BACK_LEFT]), - new SwerveModule( - kDriveMotorId[BACK_RIGHT], - kSpinningMotorId[BACK_RIGHT], - kDriveMotorReversed[BACK_RIGHT], - kSpinningMotorReversed[BACK_RIGHT], - kAbsEncoderChannel[BACK_RIGHT], - kOffsetAngle[BACK_RIGHT])}; - - private final AHRS _gyro = new AHRS(SPI.Port.kMXP); - - public Swerve() { - resetGyro(); - } - - public void resetGyro() { - _gyro.reset(); - } - - private double getDegrees() { - return Math.IEEEremainder(_gyro.getAngle(), 360); - } - - public Rotation2d getRotation2d() { - return Rotation2d.fromDegrees(getDegrees() + 90); - } - - public Command resetModulesCommand(){ - return new FunctionalCommand( - () -> {}, - ()-> { - swerveModules[FRONT_LEFT].spinTo(0); - swerveModules[FRONT_RIGHT].spinTo(0); - swerveModules[BACK_LEFT].spinTo(0); - swerveModules[BACK_RIGHT].spinTo(0); - }, - (__)-> { - stopModules(); - resetEncoders(); - new PrintCommand("modules reset").schedule(); - }, - ()-> swerveModules[FRONT_LEFT].isReset - .and(swerveModules[FRONT_RIGHT].isReset) - .and(swerveModules[BACK_LEFT].isReset) - .and(swerveModules[BACK_RIGHT].isReset) - .get(), - this); - } - - public Command driveSwerveCommand( - DoubleSupplier xSpeedSupplier, - DoubleSupplier ySpeedSupplier, - DoubleSupplier spinningSpeedSupplier, - BooleanSupplier fieldOriented) { - - final SlewRateLimiter - xLimiter = new SlewRateLimiter(kMaxDriveAccelerationUnitsPerSecond), - yLimiter = new SlewRateLimiter(kMaxDriveAccelerationUnitsPerSecond), - spinningLimiter = new SlewRateLimiter(kTeleDriveMaxAngularAccelerationUnitsPerSecond); - - return new FunctionalCommand( - ()-> resetModulesCommand().schedule(false), - () -> { - //create the speeds for x,y and spinning and using a deadBand and Limiter to fix edge cases - double xSpeed = xLimiter.calculate(applyDeadband(xSpeedSupplier.getAsDouble(), kDeadband)) * kMaxDriveSpeed, - ySpeed = yLimiter.calculate(applyDeadband(ySpeedSupplier.getAsDouble(), kDeadband)) * kMaxDriveSpeed, - spinningSpeed = spinningLimiter.calculate(applyDeadband(spinningSpeedSupplier.getAsDouble(), kDeadband)) * kMaxDriveTurningSpeed; - - // create a CassisSpeeds object and apply it the speeds - ChassisSpeeds chassisSpeeds = fieldOriented.getAsBoolean() ? - ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, spinningSpeed, getRotation2d()) : - new ChassisSpeeds(xSpeed, ySpeed, spinningSpeed); - - //use the ChassisSpeedsObject to create an array of SwerveModuleStates - SwerveModuleState moduleStates[] = kSwerveKinematics.toSwerveModuleStates(chassisSpeeds); - - //apply the array to the swerve modules of the robot - setModulesStates(moduleStates); - }, - (__)-> { - stopModules(); - resetEncoders(); - }, - ()-> false, - this); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.clearProperties(); - builder.setSmartDashboardType("Gyro"); - - builder.addDoubleProperty("FL angle", swerveModules[FRONT_LEFT]::getResetRad, null); - builder.addDoubleProperty("FR angle", swerveModules[FRONT_RIGHT]::getResetRad, null); - builder.addDoubleProperty("BL angle", swerveModules[BACK_LEFT]::getResetRad, null); - builder.addDoubleProperty("BR angle", swerveModules[BACK_RIGHT]::getResetRad, null); - } - - - -// private double time = 0; + //create swerve modules + private final SwerveModule[] swerveModules = { + new SwerveModule( + kDriveMotorId[FRONT_LEFT], + kSpinningMotorId[FRONT_LEFT], + kDriveMotorReversed[FRONT_LEFT], + kSpinningMotorReversed[FRONT_LEFT], + kAbsEncoderChannel[FRONT_LEFT], + kOffsetAngle[FRONT_LEFT]), + new SwerveModule( + kDriveMotorId[FRONT_RIGHT], + kSpinningMotorId[FRONT_RIGHT], + kDriveMotorReversed[FRONT_RIGHT], + kSpinningMotorReversed[FRONT_RIGHT], + kAbsEncoderChannel[FRONT_RIGHT], + kOffsetAngle[FRONT_RIGHT]), + new SwerveModule( + kDriveMotorId[BACK_LEFT], + kSpinningMotorId[BACK_LEFT], + kDriveMotorReversed[BACK_LEFT], + kSpinningMotorReversed[BACK_LEFT], + kAbsEncoderChannel[BACK_LEFT], + kOffsetAngle[BACK_LEFT]), + new SwerveModule( + kDriveMotorId[BACK_RIGHT], + kSpinningMotorId[BACK_RIGHT], + kDriveMotorReversed[BACK_RIGHT], + kSpinningMotorReversed[BACK_RIGHT], + kAbsEncoderChannel[BACK_RIGHT], + kOffsetAngle[BACK_RIGHT])}; + + private final AHRS _gyro = new AHRS(SPI.Port.kMXP); + private final PIDController xAutoController = new PIDController(kPXAuto, 0, 0); + private final PIDController yAutoController = new PIDController(kPYAuto, 0, 0); + private final ProfiledPIDController thetaController = new ProfiledPIDController( + kPThetaAuto, + 0, + 0, + kThetaControllerConstraints); + + private final SwerveDriveOdometry _odometry = new SwerveDriveOdometry(kSwerveKinematics, getRotation2d()); + + public Swerve() { + resetGyro(); + } + + public void resetGyro() { + _gyro.reset(); + } + + private double getDegrees() { + return Math.IEEEremainder(_gyro.getAngle(), 360); + } + + public Rotation2d getRotation2d() { + return Rotation2d.fromDegrees(getDegrees() + 90); + } + + public Command resetModulesCommand() { + return new FunctionalCommand( + () -> { + }, + () -> { + swerveModules[FRONT_LEFT].spinTo(0); + swerveModules[FRONT_RIGHT].spinTo(0); + swerveModules[BACK_LEFT].spinTo(0); + swerveModules[BACK_RIGHT].spinTo(0); + }, + (__) -> { + stopModules(); + resetEncoders(); + new PrintCommand("modules reset").schedule(); + }, + () -> swerveModules[FRONT_LEFT].isReset + .and(swerveModules[FRONT_RIGHT].isReset) + .and(swerveModules[BACK_LEFT].isReset) + .and(swerveModules[BACK_RIGHT].isReset) + .get(), + this); + } + + + public Command driveSwerveCommand( + DoubleSupplier xSpeedSupplier, + DoubleSupplier ySpeedSupplier, + DoubleSupplier spinningSpeedSupplier, + BooleanSupplier fieldOriented) { + + final SlewRateLimiter + xLimiter = new SlewRateLimiter(kMaxDriveAccelerationUnitsPerSecond), + yLimiter = new SlewRateLimiter(kMaxDriveAccelerationUnitsPerSecond), + spinningLimiter = new SlewRateLimiter(kTeleDriveMaxAngularAccelerationUnitsPerSecond); + + return new FunctionalCommand( + () -> resetModulesCommand().schedule(false), + () -> { + //create the speeds for x,y and spinning and using a deadBand and Limiter to fix edge cases + double xSpeed = xLimiter.calculate(applyDeadband(xSpeedSupplier.getAsDouble(), kDeadband)) * kMaxDriveSpeed, + ySpeed = yLimiter.calculate(applyDeadband(ySpeedSupplier.getAsDouble(), kDeadband)) * kMaxDriveSpeed, + spinningSpeed = spinningLimiter.calculate(applyDeadband(spinningSpeedSupplier.getAsDouble(), kDeadband)) * kMaxDriveTurningSpeed; + + // create a CassisSpeeds object and apply it the speeds + ChassisSpeeds chassisSpeeds = fieldOriented.getAsBoolean() ? + ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, spinningSpeed, getRotation2d()) : + new ChassisSpeeds(xSpeed, ySpeed, spinningSpeed); + + //use the ChassisSpeedsObject to create an array of SwerveModuleStates + SwerveModuleState moduleStates[] = kSwerveKinematics.toSwerveModuleStates(chassisSpeeds); + + //apply the array to the swerve modules of the robot + setModulesStates(moduleStates); + }, + (__) -> { + stopModules(); + resetEncoders(); + }, + () -> false, + this); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.clearProperties(); + builder.setSmartDashboardType("Gyro"); + + builder.addDoubleProperty("FL angle", swerveModules[FRONT_LEFT]::getResetRad, null); + builder.addDoubleProperty("FR angle", swerveModules[FRONT_RIGHT]::getResetRad, null); + builder.addDoubleProperty("BL angle", swerveModules[BACK_LEFT]::getResetRad, null); + builder.addDoubleProperty("BR angle", swerveModules[BACK_RIGHT]::getResetRad, null); + } + + private void setModulesStates(SwerveModuleState[] states) { + SwerveDriveKinematics.desaturateWheelSpeeds(states, kPhysicalMaxSpeedMetersPerSecond); + swerveModules[FRONT_LEFT].setDesiredState(states[FRONT_LEFT]); + swerveModules[FRONT_RIGHT].setDesiredState(states[FRONT_RIGHT]); + swerveModules[BACK_LEFT].setDesiredState(states[BACK_LEFT]); + swerveModules[BACK_RIGHT].setDesiredState(states[BACK_RIGHT]); + } + + public Command resetGyroCommand() { + return new InstantCommand(this::resetGyro); + } + + private void resetEncoders() { + swerveModules[FRONT_LEFT].resetEncoders(); + swerveModules[FRONT_RIGHT].resetEncoders(); + swerveModules[BACK_LEFT].resetEncoders(); + swerveModules[BACK_RIGHT].resetEncoders(); + } + + private void stopModules() { + swerveModules[FRONT_LEFT].stop(); + swerveModules[FRONT_RIGHT].stop(); + swerveModules[BACK_LEFT].stop(); + swerveModules[BACK_RIGHT].stop(); + } + + // autonomous code + + public Pose2d getOdomertyPose() { + return _odometry.getPoseMeters(); + + } + + @Override + public void periodic() { + _odometry.update( + getRotation2d(), + swerveModules[FRONT_LEFT].getState(), + swerveModules[FRONT_RIGHT].getState(), + swerveModules[BACK_LEFT].getState(), + swerveModules[BACK_RIGHT].getState()); + SmartDashboard.putNumber("Robot heading to: ", getDegrees()); + SmartDashboard.putString("Robot location: ", getOdomertyPose().getTranslation().toString()); + } + + public Command resetOdometryCommand(Pose2d pose) { + return new InstantCommand(() -> _odometry.resetPosition(pose, getRotation2d())); + } + + public Command followTrajectoryCommand(Pose2d start, List wayPoints, Pose2d end) { + return followTrajectoryCommand( + TrajectoryGenerator.generateTrajectory( + start, + wayPoints, + end, + kConfig + )); + } + + public Command followTrajectoryCommand(Trajectory trajectory) { + return new SwerveControllerCommand( + trajectory, + this::getOdomertyPose, + kSwerveKinematics, + xAutoController, + yAutoController, + thetaController, + this::setModulesStates, + this); + } + + public Command spinVectorTo(Pose2d desiredPose) { + return followTrajectoryCommand(getOdomertyPose(), new ArrayList<>(), desiredPose); + } + + public Command vectorTo(Translation2d desiredTranslation) { + return spinVectorTo(new Pose2d(desiredTranslation, getRotation2d())); + } + + // private double time = 0; // @Override // public void periodic() { // if (Timer.getFPGATimestamp() - time > 10 && swerveModules[SwerveModule.FRONT_LEFT].getDriveVelocity() < 0.01){ @@ -156,57 +249,5 @@ public void initSendable(SendableBuilder builder) { // // time = Timer.getFPGATimestamp(); // } -// } - - private void setModulesStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds(states, kPhysicalMaxSpeedMetersPerSecond); - swerveModules[FRONT_LEFT].setDesiredState(states[FRONT_LEFT]); - swerveModules[FRONT_RIGHT].setDesiredState(states[FRONT_RIGHT]); - swerveModules[BACK_LEFT].setDesiredState(states[BACK_LEFT]); - swerveModules[BACK_RIGHT].setDesiredState(states[BACK_RIGHT]); - } - - public Command resetGyroCommand(){ - return new InstantCommand(this::resetGyro); - } - - private void resetEncoders(){ - swerveModules[FRONT_LEFT].resetEncoders(); - swerveModules[FRONT_RIGHT].resetEncoders(); - swerveModules[BACK_LEFT].resetEncoders(); - swerveModules[BACK_RIGHT].resetEncoders(); - } - - private void stopModules() { - swerveModules[FRONT_LEFT].stop(); - swerveModules[FRONT_RIGHT].stop(); - swerveModules[BACK_LEFT].stop(); - swerveModules[BACK_RIGHT].stop(); - } - - // autonomous code -// private final SwerveDriveOdometry _odometry = new SwerveDriveOdometry( -// kSwerveKinematics, -// -// getRotation2d()); -// -// public Pose2d getOdomertyPose() { -// return _odometry.getPoseMeters(); - -// } -// @Override -// public void periodic() { -// _odometry.update( -// getRotation2d(), -// swerveModules[SwerveModule.FRONT_LEFT].getState(), -// swerveModules[FRONT_RIGHT].getState(), -// swerveModules[BACK_LEFT].getState(), -// swerveModules[BACK_RIGHT].getState()); -// SmartDashboard.putNumber("Robot heading to: ", getDegrees()); -// SmartDashboard.putString("Robot location: ", getOdomertyPose().getTranslation().toString()); -// } - -// public Command resetOdometryCommand(Pose2d pose) { -// return new InstantCommand(() -> _odometry.resetPosition(pose, getRotation2d())); -// } -} +// } +} \ No newline at end of file