See + * https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + * for a description of the status frames. + * + * @param motor The motor to adjust the status frame periods on. + * @param usage The status frame feedack to enable. kAll is the default when a CANSparkMax is + * constructed. + * @param enableFollowing Whether to enable motor following. + */ + public static void setCANSparkMaxBusUsage( + CANSparkMax motor, Usage usage, boolean enableFollowing) { + if (enableFollowing) { + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus0, 10); + } else { + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus0, 500); + } + + if (usage == Usage.kAll) { + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, 20); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, 20); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus3, 50); + } else if (usage == Usage.kPositionOnly) { + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, 500); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, 20); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus3, 500); + } else if (usage == Usage.kVelocityOnly) { + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, 20); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, 500); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus3, 500); + } else if (usage == Usage.kMinimal) { + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, 500); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, 500); + motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus3, 500); + } + } + + /** + * This function allows reducing a Spark Max's CAN bus utilization by reducing the periodic status + * frame period of nonessential frames from 20ms to 500ms. + * + *
See
+ * https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames
+ * for a description of the status frames.
+ *
+ * @param motor The motor to adjust the status frame periods on.
+ * @param usage The status frame feedack to enable. kAll is the default when a CANSparkMax is
+ * constructed.
+ */
+ public static void setCANSparkMaxBusUsage(CANSparkMax motor, Usage usage) {
+ setCANSparkMaxBusUsage(motor, usage, false);
+ }
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..f055412
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,145 @@
+package frc.robot;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+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.units.Distance;
+import edu.wpi.first.units.Measure;
+import frc.lib.config.SwerveModuleConstants;
+import static edu.wpi.first.units.Units.*;
+
+public final class Constants {
+
+ public static final class Swerve {
+ public static final double stickDeadband = 0.1;
+
+ public static final int pigeonID = 6;
+ public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
+
+ /* Drivetrain Constants */
+ public static final Measure If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
new file mode 100644
index 0000000..e7deb72
--- /dev/null
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,94 @@
+// 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.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
new file mode 100644
index 0000000..8c4f327
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,74 @@
+// 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.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.autos.*;
+import frc.robot.commands.TeleopSwerve;
+import frc.robot.subsystems.*;
+
+/**
+ * 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 {
+ /* Controllers */
+ private final Joystick driver = new Joystick(0);
+
+ /* Drive Controls */
+ private final int translationAxis = XboxController.Axis.kLeftY.value;
+ private final int strafeAxis = XboxController.Axis.kLeftX.value;
+ private final int rotationAxis = XboxController.Axis.kRightX.value;
+
+ /* Driver Buttons */
+ private final JoystickButton zeroGyro =
+ new JoystickButton(driver, XboxController.Button.kY.value);
+ private final JoystickButton robotCentric =
+ new JoystickButton(driver, XboxController.Button.kLeftBumper.value);
+
+ /* Subsystems */
+ private final Swerve s_Swerve = new Swerve();
+
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ public RobotContainer() {
+ s_Swerve.setDefaultCommand(
+ new TeleopSwerve(
+ s_Swerve,
+ () -> -driver.getRawAxis(translationAxis),
+ () -> -driver.getRawAxis(strafeAxis),
+ () -> -driver.getRawAxis(rotationAxis),
+ () -> robotCentric.getAsBoolean()));
+
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ /* Driver Buttons */
+ //zeroGyro.whenPressed(new InstantCommand(() -> s_Swerve.zeroGyro()));
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // An ExampleCommand will run in autonomous
+ return new exampleAuto(s_Swerve);
+ }
+}
diff --git a/src/main/java/frc/robot/autos/exampleAuto.java b/src/main/java/frc/robot/autos/exampleAuto.java
new file mode 100644
index 0000000..1ab66e2
--- /dev/null
+++ b/src/main/java/frc/robot/autos/exampleAuto.java
@@ -0,0 +1,60 @@
+package frc.robot.autos;
+
+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.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import frc.robot.Constants;
+import frc.robot.subsystems.Swerve;
+import java.util.List;
+
+public class exampleAuto extends SequentialCommandGroup {
+ public exampleAuto(Swerve s_Swerve) {
+ TrajectoryConfig config =
+ new TrajectoryConfig(
+ Constants.AutoConstants.kMaxSpeedMetersPerSecond,
+ Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+ .setKinematics(Constants.Swerve.swerveKinematics);
+
+ // 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(
+ Constants.AutoConstants.kPThetaController,
+ 0,
+ 0,
+ Constants.AutoConstants.kThetaControllerConstraints);
+ thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+ SwerveControllerCommand swerveControllerCommand =
+ new SwerveControllerCommand(
+ exampleTrajectory,
+ s_Swerve::getPose,
+ Constants.Swerve.swerveKinematics,
+ new PIDController(Constants.AutoConstants.kPXController, 0, 0),
+ new PIDController(Constants.AutoConstants.kPYController, 0, 0),
+ thetaController,
+ s_Swerve::setModuleStates,
+ s_Swerve);
+
+ addCommands(
+ new InstantCommand(() -> s_Swerve.resetOdometry(exampleTrajectory.getInitialPose())),
+ swerveControllerCommand);
+ }
+}
diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java
new file mode 100644
index 0000000..09927b1
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TeleopSwerve.java
@@ -0,0 +1,58 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.Swerve;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+public class TeleopSwerve extends Command {
+ private Swerve s_Swerve;
+ private DoubleSupplier translationSup;
+ private DoubleSupplier strafeSup;
+ private DoubleSupplier rotationSup;
+ private BooleanSupplier robotCentricSup;
+
+ private SlewRateLimiter translationLimiter = new SlewRateLimiter(3.0);
+ private SlewRateLimiter strafeLimiter = new SlewRateLimiter(3.0);
+ private SlewRateLimiter rotationLimiter = new SlewRateLimiter(3.0);
+
+ public TeleopSwerve(
+ Swerve s_Swerve,
+ DoubleSupplier translationSup,
+ DoubleSupplier strafeSup,
+ DoubleSupplier rotationSup,
+ BooleanSupplier robotCentricSup) {
+ this.s_Swerve = s_Swerve;
+ addRequirements(s_Swerve);
+
+ this.translationSup = translationSup;
+ this.strafeSup = strafeSup;
+ this.rotationSup = rotationSup;
+ this.robotCentricSup = robotCentricSup;
+ }
+
+ @Override
+ public void execute() {
+ /* Get Values, Deadband*/
+ double translationVal =
+ translationLimiter.calculate(
+ MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.Swerve.stickDeadband));
+ double strafeVal =
+ strafeLimiter.calculate(
+ MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.Swerve.stickDeadband));
+ double rotationVal =
+ rotationLimiter.calculate(
+ MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.Swerve.stickDeadband));
+
+ /* Drive */
+ s_Swerve.drive(
+ new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed),
+ rotationVal * Constants.Swerve.maxAngularVelocity,
+ !robotCentricSup.getAsBoolean(),
+ true);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
new file mode 100644
index 0000000..9fb8720
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -0,0 +1,101 @@
+package frc.robot.subsystems;
+
+import com.kauailabs.navx.frc.AHRS;
+
+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.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Swerve extends SubsystemBase {
+ // The gyro sensor
+
+ private final AHRS gyro;
+
+ private SwerveDriveOdometry swerveOdometry;
+
+ private SwerveModule[] mSwerveMods;
+
+ private Field2d field;
+
+ public Swerve() {
+ gyro = new AHRS(SPI.Port.kMXP, (byte) 100);
+ zeroGyro();
+
+ mSwerveMods =
+ new SwerveModule[] {
+ new SwerveModule(0, Constants.Swerve.Mod0.constants),
+ new SwerveModule(1, Constants.Swerve.Mod1.constants),
+ new SwerveModule(2, Constants.Swerve.Mod2.constants),
+ new SwerveModule(3, Constants.Swerve.Mod3.constants)
+ };
+
+ swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getYaw(), getPositions());
+
+ field = new Field2d();
+ SmartDashboard.putData("Field", field);
+ }
+
+ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) {
+ SwerveModuleState[] swerveModuleStates =
+ Constants.Swerve.swerveKinematics.toSwerveModuleStates(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ translation.getX(), translation.getY(), rotation, getYaw())
+ : new ChassisSpeeds(translation.getX(), translation.getY(), rotation));
+ SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.Swerve.maxSpeed);
+
+ for (SwerveModule mod : mSwerveMods) {
+ mod.setDesiredState(swerveModuleStates[mod.moduleNumber], isOpenLoop);
+ }
+ }
+
+ /* Used by SwerveControllerCommand in Auto */
+ public void setModuleStates(SwerveModuleState[] desiredStates) {
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed);
+
+ for (SwerveModule mod : mSwerveMods) {
+ mod.setDesiredState(desiredStates[mod.moduleNumber], false);
+ }
+ }
+
+ public Pose2d getPose() {
+ return swerveOdometry.getPoseMeters();
+ }
+
+ public void resetOdometry(Pose2d pose) {
+ swerveOdometry.resetPosition(getYaw(), getPositions(), pose);
+ }
+
+ public SwerveModulePosition[] getPositions() {
+ SwerveModulePosition[] states = new SwerveModulePosition[4];
+ for (SwerveModule mod : mSwerveMods) {
+ states[mod.moduleNumber] = mod.getState();
+ }
+ return states;
+ }
+
+ public void zeroGyro() {
+ gyro.reset();
+ }
+
+ public Rotation2d getYaw() {
+ return (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(360 - gyro.getYaw()) : Rotation2d.fromDegrees(gyro.getYaw());
+ }
+
+ @Override
+ public void periodic() {
+ swerveOdometry.update(getYaw(), getPositions());
+ field.setRobotPose(getPose());
+ SmartDashboard.putNumber("Yaw", getYaw().getDegrees());
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
new file mode 100644
index 0000000..670e599
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -0,0 +1,161 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkAbsoluteEncoder;
+import com.revrobotics.SparkPIDController;
+import com.revrobotics.CANSparkBase.ControlType;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.config.SwerveModuleConstants;
+import frc.lib.math.OnboardModuleState;
+import frc.lib.util.CANSparkMaxUtil;
+import frc.lib.util.CANSparkMaxUtil.Usage;
+import frc.robot.Constants;
+
+public class SwerveModule extends SubsystemBase {
+ public int moduleNumber;
+ private Rotation2d lastAngle;
+ private Rotation2d angleOffset;
+
+ private CANSparkMax angleMotor;
+ private CANSparkMax driveMotor;
+
+ private RelativeEncoder driveEncoder;
+ private RelativeEncoder integratedAngleEncoder;
+
+ private final CANcoder m_turnCancoder;
+
+ private final SparkPIDController driveController;
+ private final SparkPIDController angleController;
+
+ private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(
+ Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA);
+
+ public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) {
+ this.moduleNumber = moduleNumber;
+ angleOffset = moduleConstants.angleOffset;
+
+ /* Angle Motor Config */
+ angleMotor = new CANSparkMax(moduleConstants.angleMotorID, MotorType.kBrushless);
+ integratedAngleEncoder = angleMotor.getEncoder();
+ angleController = angleMotor.getPIDController();
+ configAngleMotor();
+
+ m_turnCancoder = new CANcoder(moduleConstants.canCoderID, "CV1");
+ m_turnCancoder.getConfigurator().apply(new CANcoderConfiguration());
+
+ /* Drive Motor Config */
+ driveMotor = new CANSparkMax(moduleConstants.driveMotorID, MotorType.kBrushless);
+ driveEncoder = driveMotor.getEncoder();
+ driveController = driveMotor.getPIDController();
+ configDriveMotor();
+
+ lastAngle = getState().angle;
+ }
+
+ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+ // Custom optimize command, since default WPILib optimize assumes continuous
+ // controller which
+ // REV and CTRE are not
+ desiredState = OnboardModuleState.optimize(desiredState, getState().angle);
+
+ setAngle(desiredState);
+ setSpeed(desiredState, isOpenLoop);
+ }
+
+ private void resetToAbsolute() {
+ // double absolutePosition = getEncoder().getDegrees() -
+ // angleOffset.getDegrees();
+ // integratedAngleEncoder.setPosition(absolutePosition);
+ }
+
+ private void configAngleMotor() {
+ angleMotor.restoreFactoryDefaults();
+ CANSparkMaxUtil.setCANSparkMaxBusUsage(angleMotor, Usage.kPositionOnly);
+ angleMotor.setSmartCurrentLimit(Constants.Swerve.angleContinuousCurrentLimit);
+ angleMotor.setInverted(Constants.Swerve.angleInvert);
+ angleMotor.setIdleMode(Constants.Swerve.angleNeutralMode);
+ integratedAngleEncoder.setPositionConversionFactor(Constants.Swerve.angleConversionFactor);
+ angleController.setP(Constants.Swerve.angleKP);
+ angleController.setI(Constants.Swerve.angleKI);
+ angleController.setD(Constants.Swerve.angleKD);
+ angleController.setFF(Constants.Swerve.angleKFF);
+ angleMotor.enableVoltageCompensation(Constants.Swerve.voltageComp);
+ angleMotor.burnFlash();
+ resetToAbsolute();
+ }
+
+ private void configDriveMotor() {
+ driveMotor.restoreFactoryDefaults();
+ CANSparkMaxUtil.setCANSparkMaxBusUsage(driveMotor, Usage.kAll);
+ driveMotor.setSmartCurrentLimit(Constants.Swerve.driveContinuousCurrentLimit);
+ driveMotor.setInverted(Constants.Swerve.driveInvert);
+ driveMotor.setIdleMode(Constants.Swerve.driveNeutralMode);
+ driveEncoder.setVelocityConversionFactor(Constants.Swerve.driveConversionVelocityFactor);
+ driveEncoder.setPositionConversionFactor(Constants.Swerve.driveConversionPositionFactor);
+ driveController.setP(Constants.Swerve.angleKP);
+ driveController.setI(Constants.Swerve.angleKI);
+ driveController.setD(Constants.Swerve.angleKD);
+ driveController.setFF(Constants.Swerve.angleKFF);
+ driveMotor.enableVoltageCompensation(Constants.Swerve.voltageComp);
+ driveMotor.burnFlash();
+ driveEncoder.setPosition(0.0);
+ }
+
+ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
+ driveMotor.set(percentOutput);
+ } else {
+ driveController.setReference(
+ desiredState.speedMetersPerSecond,
+ CANSparkBase.ControlType.kVelocity,
+ 0,
+ feedforward.calculate(desiredState.speedMetersPerSecond));
+ }
+ }
+
+ private void setAngle(SwerveModuleState desiredState) {
+ // Prevent rotating module if speed is less then 1%. Prevents jittering.
+ Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01))
+ ? lastAngle
+ : desiredState.angle;
+
+ angleController.setReference(angle.getDegrees(), ControlType.kPosition);
+ lastAngle = angle;
+ }
+
+ private Rotation2d getAngle() {
+ return Rotation2d.fromDegrees(integratedAngleEncoder.getPosition());
+ }
+
+ // public Rotation2d getEncoder() {
+ // return Rotation2d.fromDegrees(angleEncoder.getPosition() * 360);
+ // }
+
+ public SwerveModulePosition getState() {
+ return new SwerveModulePosition(driveEncoder.getVelocity(), getAngle());
+ }
+
+ @Override
+ public void periodic() {
+ SmartDashboard.putNumber(String.valueOf(moduleNumber) + " angle", getAngle().getDegrees());
+ SmartDashboard.putNumber(String.valueOf(moduleNumber) + " distance", driveEncoder.getPosition());
+ SmartDashboard.putNumber(String.valueOf(moduleNumber) + " cancoder",
+ m_turnCancoder.getPosition().getValueAsDouble());
+ SmartDashboard.putNumber(String.valueOf(moduleNumber) + " cancoderAbs",
+ m_turnCancoder.getAbsolutePosition().getValueAsDouble());
+
+ SmartDashboard.putNumber("PCF", Constants.Swerve.driveConversionPositionFactor);
+ }
+}
diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
new file mode 100644
index 0000000..e978a5f
--- /dev/null
+++ b/vendordeps/NavX.json
@@ -0,0 +1,40 @@
+{
+ "fileName": "NavX.json",
+ "name": "NavX",
+ "version": "2024.1.0",
+ "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://dev.studica.com/maven/release/2024/"
+ ],
+ "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-frc-java",
+ "version": "2024.1.0"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-frc-cpp",
+ "version": "2024.1.0",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": false,
+ "libName": "navx_frc",
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxraspbian",
+ "linuxarm32",
+ "linuxarm64",
+ "linuxx86-64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ]
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
new file mode 100644
index 0000000..69a4079
--- /dev/null
+++ b/vendordeps/Phoenix6.json
@@ -0,0 +1,339 @@
+{
+ "fileName": "Phoenix6.json",
+ "name": "CTRE-Phoenix (v6)",
+ "version": "24.1.0",
+ "frcYear": 2024,
+ "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+ "mavenUrls": [
+ "https://maven.ctr-electronics.com/release/"
+ ],
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json",
+ "conflictsWith": [
+ {
+ "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a",
+ "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.",
+ "offlineFileName": "Phoenix6And5.json"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-java",
+ "version": "24.1.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonFX",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simCANCoder",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "24.1.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-cpp",
+ "version": "24.1.0",
+ "libName": "CTRE_Phoenix6_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "24.1.0",
+ "libName": "CTRE_PhoenixTools",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "wpiapi-cpp-sim",
+ "version": "24.1.0",
+ "libName": "CTRE_Phoenix6_WPISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "24.1.0",
+ "libName": "CTRE_PhoenixTools_Sim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "24.1.0",
+ "libName": "CTRE_SimTalonSRX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonFX",
+ "version": "24.1.0",
+ "libName": "CTRE_SimTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "24.1.0",
+ "libName": "CTRE_SimVictorSPX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "24.1.0",
+ "libName": "CTRE_SimPigeonIMU",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simCANCoder",
+ "version": "24.1.0",
+ "libName": "CTRE_SimCANCoder",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "24.1.0",
+ "libName": "CTRE_SimProTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "24.1.0",
+ "libName": "CTRE_SimProCANcoder",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "24.1.0",
+ "libName": "CTRE_SimProPigeon2",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ]
+}
\ No newline at end of file
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
new file mode 100644
index 0000000..0f3520e
--- /dev/null
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,74 @@
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2024.2.0",
+ "frcYear": "2024",
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "mavenUrls": [
+ "https://maven.revrobotics.com/"
+ ],
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-java",
+ "version": "2024.2.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.2.0",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-cpp",
+ "version": "2024.2.0",
+ "libName": "REVLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.2.0",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
\ No newline at end of file
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
new file mode 100644
index 0000000..67bf389
--- /dev/null
+++ b/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,38 @@
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "frcYear": "2024",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}