diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Configs.java b/src/main/java/frc/robot/Configs.java deleted file mode 100644 index ff6198d..0000000 --- a/src/main/java/frc/robot/Configs.java +++ /dev/null @@ -1,56 +0,0 @@ -package frc.robot; - -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.Constants.ModuleConstants; - -public final class Configs { - public static final class MAXSwerveModule { - public static final SparkMaxConfig drivingConfig = new SparkMaxConfig(); - public static final SparkMaxConfig turningConfig = new SparkMaxConfig(); - - static { - // Use module constants to calculate conversion factors and feed forward gain. - double drivingFactor = ModuleConstants.kWheelDiameterMeters * Math.PI - / ModuleConstants.kDrivingMotorReduction; - double turningFactor = 2 * Math.PI; - double drivingVelocityFeedForward = 1 / ModuleConstants.kDriveWheelFreeSpeedRps; - - drivingConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(50); - drivingConfig.encoder - .positionConversionFactor(drivingFactor) // meters - .velocityConversionFactor(drivingFactor / 60.0); // meters per second - drivingConfig.closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - // These are example gains you may need to them for your own robot! - .pid(0.04, 0, 0) - .velocityFF(drivingVelocityFeedForward) - .outputRange(-1, 1); - - turningConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(20); - turningConfig.absoluteEncoder - // Invert the turning encoder, since the output shaft rotates in the opposite - // direction of the steering motor in the MAXSwerve Module. - .inverted(true) - .positionConversionFactor(turningFactor) // radians - .velocityConversionFactor(turningFactor / 60.0); // radians per second - turningConfig.closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - // These are example gains you may need to them for your own robot! - .pid(1, 0, 0) - .outputRange(-1, 1) - // Enable PID wrap around for the turning motor. This will allow the PID - // controller to go through 0 to get to the setpoint i.e. going from 350 degrees - // to 10 degrees will go through 0 rather than the other direction which is a - // longer route. - .positionWrappingEnabled(true) - .positionWrappingInputRange(0, turningFactor); - } - } -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 6ac96df..0000000 --- a/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide - * numerical or boolean - * constants. This class should not be used for any other purpose. All constants - * should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

- * It is advised to statically import this class (or one of its inner classes) - * wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public static final class DriveConstants { - // Driving Parameters - Note that these are not the maximum capable speeds of - // the robot, rather the allowed maximum speeds - public static final double kMaxSpeedMetersPerSecond = 4.8; - public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second - - // Chassis configuration - public static final double kTrackWidth = Units.inchesToMeters(26.5); - // Distance between centers of right and left wheels on robot - public static final double kWheelBase = Units.inchesToMeters(26.5); - // Distance between front and back wheels on robot - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); - - // Angular offsets of the modules relative to the chassis in radians - public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; - public static final double kFrontRightChassisAngularOffset = 0; - public static final double kBackLeftChassisAngularOffset = Math.PI; - public static final double kBackRightChassisAngularOffset = Math.PI / 2; - - // SPARK MAX CAN IDs - public static final int kFrontLeftDrivingCanId = 11; - public static final int kRearLeftDrivingCanId = 13; - public static final int kFrontRightDrivingCanId = 15; - public static final int kRearRightDrivingCanId = 17; - - public static final int kFrontLeftTurningCanId = 10; - public static final int kRearLeftTurningCanId = 12; - public static final int kFrontRightTurningCanId = 14; - public static final int kRearRightTurningCanId = 16; - - public static final boolean kGyroReversed = false; - } - - public static final class ModuleConstants { - // The MAXSwerve module can be configured with one of three pinion gears: 12T, - // 13T, or 14T. This changes the drive speed of the module (a pinion gear with - // more teeth will result in a robot that drives faster). - public static final int kDrivingMotorPinionTeeth = 14; - - // Calculations required for driving motor conversion factors and feed forward - public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; - public static final double kWheelDiameterMeters = 0.0762; - public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 - // teeth on the bevel pinion - public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); - public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) - / kDrivingMotorReduction; - } - - public static final class OIConstants { - public static final int kDriverControllerPort = 0; - public static final double kDriveDeadband = 0.05; - } - - public static final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - - public static final double kPXController = 1; - public static final double kPYController = 1; - public static final double kPThetaController = 1; - - // Constraint for the motion profiled robot angle controller - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); - } - - public static final class NeoMotorConstants { - public static final double kFreeSpeedRpm = 5676; - } -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fed81b0..1b4a813 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,119 +4,39 @@ package frc.robot; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.PS4Controller.Button; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.OIConstants; +import frc.robot.constants.OIConstants; import frc.robot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import java.util.List; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -/* - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot - * (including subsystems, commands, and button mappings) should be declared here. - */ public class RobotContainer { - // The robot's subsystems - private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private DriveSubsystem drivetrain; - // The driver's controller - XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); + private CommandXboxController driver; - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ public RobotContainer() { - // Configure the button bindings + drivetrain = new DriveSubsystem(); + + driver = new CommandXboxController(OIConstants.kDriverControllerPort); + configureButtonBindings(); - - // Configure default commands - m_robotDrive.setDefaultCommand( - // The left stick controls translation of the robot. - // Turning is controlled by the X axis of the right stick. - new RunCommand( - () -> m_robotDrive.drive( - -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), - true), - m_robotDrive)); } - /** - * Use this method to define your button->command mappings. Buttons can be - * created by - * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its - * subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling - * passing it to a - * {@link JoystickButton}. - */ private void configureButtonBindings() { - new JoystickButton(m_driverController, Button.kR1.value) - .whileTrue(new RunCommand( - () -> m_robotDrive.setX(), - m_robotDrive)); + drivetrain.setDefaultCommand( + drivetrain.drive( + driver::getLeftY, + driver::getLeftX, + driver::getRightX, + () -> true + ) + ); + + driver.start().whileTrue(drivetrain.setXCommand()); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ public Command getAutonomousCommand() { - // Create config for trajectory - TrajectoryConfig config = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(DriveConstants.kDriveKinematics); - - // An example trajectory to follow. All units in meters. - Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), - // Pass through these two interior waypoints, making an 's' curve path - List.of(new Translation2d(1, 1), new Translation2d(2, -1)), - // End 3 meters straight ahead of where we started, facing forward - new Pose2d(3, 0, new Rotation2d(0)), - config); - - var thetaController = new ProfiledPIDController( - AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - exampleTrajectory, - m_robotDrive::getPose, // Functional interface to feed supplier - DriveConstants.kDriveKinematics, - - // Position controllers - new PIDController(AutoConstants.kPXController, 0, 0), - new PIDController(AutoConstants.kPYController, 0, 0), - thetaController, - m_robotDrive::setModuleStates, - m_robotDrive); - - // Reset odometry to the starting pose of the trajectory. - m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()); - - // Run path following command, then stop at the end. - return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false)); + return new PrintCommand("NO AUTO DEFINED"); } } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java new file mode 100644 index 0000000..9a99510 --- /dev/null +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -0,0 +1,18 @@ +package frc.robot.constants; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +public class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; + + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); +} diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java new file mode 100644 index 0000000..dc4d824 --- /dev/null +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -0,0 +1,42 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; + +public class DriveConstants { + // Driving Parameters - Note that these are not the maximum capable speeds of + // the robot, rather the allowed maximum speeds + public static final double kMaxSpeedMetersPerSecond = 4.8; + public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second + + // Chassis configuration + public static final double kTrackWidth = Units.inchesToMeters(26.5); + // Distance between centers of right and left wheels on robot + public static final double kWheelBase = Units.inchesToMeters(26.5); + // Distance between front and back wheels on robot + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + // Angular offsets of the modules relative to the chassis in radians + public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI / 2; + + // SPARK MAX CAN IDs + public static final int kFrontLeftDrivingCanId = 11; + public static final int kRearLeftDrivingCanId = 13; + public static final int kFrontRightDrivingCanId = 15; + public static final int kRearRightDrivingCanId = 17; + + public static final int kFrontLeftTurningCanId = 10; + public static final int kRearLeftTurningCanId = 12; + public static final int kFrontRightTurningCanId = 14; + public static final int kRearRightTurningCanId = 16; + + public static final boolean kGyroReversed = false; + } diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java new file mode 100644 index 0000000..a586023 --- /dev/null +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -0,0 +1,66 @@ +package frc.robot.constants; + +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class ModuleConstants { + // The MAXSwerve module can be configured with one of three pinion gears: 12T, + // 13T, or 14T. This changes the drive speed of the module (a pinion gear with + // more teeth will result in a robot that drives faster). + public static final int kDrivingMotorPinionTeeth = 14; + + // Calculations required for driving motor conversion factors and feed forward + public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; + public static final double kWheelDiameterMeters = 0.0762; + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 + // teeth on the bevel pinion + public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) + / kDrivingMotorReduction; + + public static final SparkMaxConfig drivingConfig = new SparkMaxConfig(); + public static final SparkMaxConfig turningConfig = new SparkMaxConfig(); + + static { + // Use module constants to calculate conversion factors and feed forward gain. + double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction; + double turningFactor = 2 * Math.PI; + double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps; + + drivingConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(50); + drivingConfig.encoder + .positionConversionFactor(drivingFactor) // meters + .velocityConversionFactor(drivingFactor / 60.0); // meters per second + drivingConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + // These are example gains you may need to them for your own robot! + .pid(0.04, 0, 0) + .velocityFF(drivingVelocityFeedForward) + .outputRange(-1, 1); + + turningConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(20); + turningConfig.absoluteEncoder + // Invert the turning encoder, since the output shaft rotates in the opposite + // direction of the steering motor in the MAXSwerve Module. + .inverted(true) + .positionConversionFactor(turningFactor) // radians + .velocityConversionFactor(turningFactor / 60.0); // radians per second + turningConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + // These are example gains you may need to them for your own robot! + .pid(1, 0, 0) + .outputRange(-1, 1) + // Enable PID wrap around for the turning motor. This will allow the PID + // controller to go through 0 to get to the setpoint i.e. going from 350 degrees + // to 10 degrees will go through 0 rather than the other direction which is a + // longer route. + .positionWrappingEnabled(true) + .positionWrappingInputRange(0, turningFactor); + } +} diff --git a/src/main/java/frc/robot/constants/NeoMotorConstants.java b/src/main/java/frc/robot/constants/NeoMotorConstants.java new file mode 100644 index 0000000..3bbe106 --- /dev/null +++ b/src/main/java/frc/robot/constants/NeoMotorConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class NeoMotorConstants { + public static final double kFreeSpeedRpm = 5676; +} diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java new file mode 100644 index 0000000..7b86c95 --- /dev/null +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -0,0 +1,7 @@ +package frc.robot.constants; + +public class OIConstants { + public static final int kDriverControllerPort = 0; + + public static final double kDriveDeadband = 0.05; +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index fdcf7c2..17b6c7b 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -4,6 +4,10 @@ package frc.robot.subsystems; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -13,168 +17,201 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.ADIS16470_IMU; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; -import frc.robot.Constants.DriveConstants; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.OIConstants; public class DriveSubsystem extends SubsystemBase { - // Create MAXSwerveModules - private final MAXSwerveModule m_frontLeft = new MAXSwerveModule( - DriveConstants.kFrontLeftDrivingCanId, - DriveConstants.kFrontLeftTurningCanId, - DriveConstants.kFrontLeftChassisAngularOffset); + // Create MAXSwerveModules + private MAXSwerveModule m_frontLeft; + private MAXSwerveModule m_frontRight; + private MAXSwerveModule m_rearLeft; + private MAXSwerveModule m_rearRight; - private final MAXSwerveModule m_frontRight = new MAXSwerveModule( - DriveConstants.kFrontRightDrivingCanId, - DriveConstants.kFrontRightTurningCanId, - DriveConstants.kFrontRightChassisAngularOffset); + // The gyro sensor + private ADIS16470_IMU m_gyro; - private final MAXSwerveModule m_rearLeft = new MAXSwerveModule( - DriveConstants.kRearLeftDrivingCanId, - DriveConstants.kRearLeftTurningCanId, - DriveConstants.kBackLeftChassisAngularOffset); + // Odometry class for tracking robot pose + private SwerveDriveOdometry m_odometry; - private final MAXSwerveModule m_rearRight = new MAXSwerveModule( - DriveConstants.kRearRightDrivingCanId, - DriveConstants.kRearRightTurningCanId, - DriveConstants.kBackRightChassisAngularOffset); + /** Creates a new DriveSubsystem. */ + public DriveSubsystem() { + m_frontLeft = new MAXSwerveModule( + DriveConstants.kFrontLeftDrivingCanId, + DriveConstants.kFrontLeftTurningCanId, + DriveConstants.kFrontLeftChassisAngularOffset + ); - // The gyro sensor - private final ADIS16470_IMU m_gyro = new ADIS16470_IMU(); + m_frontRight = new MAXSwerveModule( + DriveConstants.kFrontRightDrivingCanId, + DriveConstants.kFrontRightTurningCanId, + DriveConstants.kFrontRightChassisAngularOffset + ); - // Odometry class for tracking robot pose - SwerveDriveOdometry m_odometry = new SwerveDriveOdometry( - DriveConstants.kDriveKinematics, - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }); + m_rearLeft = new MAXSwerveModule( + DriveConstants.kRearLeftDrivingCanId, + DriveConstants.kRearLeftTurningCanId, + DriveConstants.kBackLeftChassisAngularOffset + ); - /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { - } + m_rearRight = new MAXSwerveModule( + DriveConstants.kRearRightDrivingCanId, + DriveConstants.kRearRightTurningCanId, + DriveConstants.kBackRightChassisAngularOffset + ); - @Override - public void periodic() { - // Update the odometry in the periodic block - m_odometry.update( - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() + m_gyro = new ADIS16470_IMU(); + + m_odometry = new SwerveDriveOdometry( + DriveConstants.kDriveKinematics, + Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() }); - } + } - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return m_odometry.getPoseMeters(); - } + @Override + public void periodic() { + // Update the odometry in the periodic block + m_odometry.update( + Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }); + } - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition( - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }, - pose); - } + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } - /** - * Method to drive the robot using joystick info. - * - * @param xSpeed Speed of the robot in the x direction (forward). - * @param ySpeed Speed of the robot in the y direction (sideways). - * @param rot Angular rate of the robot. - * @param fieldRelative Whether the provided x and y speeds are relative to the - * field. - */ - public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - // Convert the commanded speeds into the correct units for the drivetrain - double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond; - double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond; - double rotDelivered = rot * DriveConstants.kMaxAngularSpeed; + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition( + Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }, + pose + ); + } - var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ))) - : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_rearLeft.setDesiredState(swerveModuleStates[2]); - m_rearRight.setDesiredState(swerveModuleStates[3]); - } + public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot, + BooleanSupplier fieldRelative) { + return run(() -> { + drive( + -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband), + fieldRelative.getAsBoolean() + ); + }); + } - /** - * Sets the wheels into an X formation to prevent movement. - */ - public void setX() { - m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); - m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); - m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); - m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); - } + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the + * field. + */ + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + // Convert the commanded speeds into the correct units for the drivetrain + double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond; + double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond; + double rotDelivered = rot * DriveConstants.kMaxAngularSpeed; - /** - * Sets the swerve ModuleStates. - * - * @param desiredStates The desired SwerveModule states. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(desiredStates[0]); - m_frontRight.setDesiredState(desiredStates[1]); - m_rearLeft.setDesiredState(desiredStates[2]); - m_rearRight.setDesiredState(desiredStates[3]); - } + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, + Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ))) + : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + m_frontRight.setDesiredState(swerveModuleStates[1]); + m_rearLeft.setDesiredState(swerveModuleStates[2]); + m_rearRight.setDesiredState(swerveModuleStates[3]); + } - /** Resets the drive encoders to currently read a position of 0. */ - public void resetEncoders() { - m_frontLeft.resetEncoders(); - m_rearLeft.resetEncoders(); - m_frontRight.resetEncoders(); - m_rearRight.resetEncoders(); - } + public Command setXCommand() { + return run(() -> { + setX(); + }); + } - /** Zeroes the heading of the robot. */ - public void zeroHeading() { - m_gyro.reset(); - } + /** + * Sets the wheels into an X formation to prevent movement. + */ + public void setX() { + m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); + m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); + m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); + m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); + } - /** - * Returns the heading of the robot. - * - * @return the robot's heading in degrees, from -180 to 180 - */ - public double getHeading() { - return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees(); - } + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired SwerveModule states. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(desiredStates[0]); + m_frontRight.setDesiredState(desiredStates[1]); + m_rearLeft.setDesiredState(desiredStates[2]); + m_rearRight.setDesiredState(desiredStates[3]); + } - /** - * Returns the turn rate of the robot. - * - * @return The turn rate of the robot, in degrees per second - */ - public double getTurnRate() { - return m_gyro.getRate(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); - } + /** Resets the drive encoders to currently read a position of 0. */ + public void resetEncoders() { + m_frontLeft.resetEncoders(); + m_rearLeft.resetEncoders(); + m_frontRight.resetEncoders(); + m_rearRight.resetEncoders(); + } + + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from -180 to 180 + */ + public double getHeading() { + return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees(); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return m_gyro.getRate(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + } } diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index b2df024..1c562d0 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -17,7 +17,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; -import frc.robot.Configs; +import frc.robot.constants.ModuleConstants; public class MAXSwerveModule { private final SparkMax m_drivingSpark; @@ -51,9 +51,9 @@ public class MAXSwerveModule { // Apply the respective configurations to the SPARKS. Reset parameters before // applying the configuration to bring the SPARK to a known good state. Persist // the settings to the SPARK to avoid losing them on a power cycle. - m_drivingSpark.configure(Configs.MAXSwerveModule.drivingConfig, ResetMode.kResetSafeParameters, + m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - m_turningSpark.configure(Configs.MAXSwerveModule.turningConfig, ResetMode.kResetSafeParameters, + m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); m_chassisAngularOffset = chassisAngularOffset;