Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial Commit, REV template

Browse Source
This commit is contained in:
Bradley Bickford
2025-01-04 16:23:47 -05:00
commit cfd538f97e
22 changed files with 1590 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/deploy/example.txt Normal file
View File

@@ -0,0 +1,3 @@
Files placed in this directory will be deployed to the RoboRIO into the
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
to get a proper path relative to the deploy directory.

56
src/main/java/frc/robot/Configs.java Normal file
View File

@@ -0,0 +1,56 @@
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);
}
}
}

102
src/main/java/frc/robot/Constants.java Normal file
View File

@@ -0,0 +1,102 @@
// 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.
*
* <p>
* 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;
}
}

25
src/main/java/frc/robot/Main.java Normal file
View File

@@ -0,0 +1,25 @@
// 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.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

102
src/main/java/frc/robot/Robot.java Normal file
View File

@@ -0,0 +1,102 @@
// 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 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>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();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// 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() {}
}

122
src/main/java/frc/robot/RobotContainer.java Normal file
View File

@@ -0,0 +1,122 @@
// 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.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.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;
/*
* 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();
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the button bindings
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));
}
/**
* 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));
}
}

180
src/main/java/frc/robot/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,180 @@
// 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.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
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.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import frc.robot.Constants.DriveConstants;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// Create MAXSwerveModules
private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
DriveConstants.kFrontLeftDrivingCanId,
DriveConstants.kFrontLeftTurningCanId,
DriveConstants.kFrontLeftChassisAngularOffset);
private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
DriveConstants.kFrontRightDrivingCanId,
DriveConstants.kFrontRightTurningCanId,
DriveConstants.kFrontRightChassisAngularOffset);
private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
DriveConstants.kRearLeftDrivingCanId,
DriveConstants.kRearLeftTurningCanId,
DriveConstants.kBackLeftChassisAngularOffset);
private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
DriveConstants.kRearRightDrivingCanId,
DriveConstants.kRearRightTurningCanId,
DriveConstants.kBackRightChassisAngularOffset);
// The gyro sensor
private final ADIS16470_IMU m_gyro = new ADIS16470_IMU();
// 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()
});
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
}
@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()
});
}
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* 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);
}
/**
* 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;
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]);
}
/**
* 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)));
}
/**
* 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]);
}
/** 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);
}
}

114
src/main/java/frc/robot/subsystems/MAXSwerveModule.java Normal file
View File

@@ -0,0 +1,114 @@
// 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.subsystems;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import frc.robot.Configs;
public class MAXSwerveModule {
private final SparkMax m_drivingSpark;
private final SparkMax m_turningSpark;
private final RelativeEncoder m_drivingEncoder;
private final AbsoluteEncoder m_turningEncoder;
private final SparkClosedLoopController m_drivingClosedLoopController;
private final SparkClosedLoopController m_turningClosedLoopController;
private double m_chassisAngularOffset = 0;
private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
/**
* Constructs a MAXSwerveModule and configures the driving and turning motor,
* encoder, and PID controller. This configuration is specific to the REV
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
* Encoder.
*/
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
m_drivingEncoder = m_drivingSpark.getEncoder();
m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
// 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,
PersistMode.kPersistParameters);
m_turningSpark.configure(Configs.MAXSwerveModule.turningConfig, ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);
m_chassisAngularOffset = chassisAngularOffset;
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
m_drivingEncoder.setPosition(0);
}
/**
* Returns the current state of the module.
*
* @return The current state of the module.
*/
public SwerveModuleState getState() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModuleState(m_drivingEncoder.getVelocity(),
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
}
/**
* Returns the current position of the module.
*
* @return The current position of the module.
*/
public SwerveModulePosition getPosition() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModulePosition(
m_drivingEncoder.getPosition(),
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
}
/**
* Sets the desired state for the module.
*
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState desiredState) {
// Apply chassis angular offset to the desired state.
SwerveModuleState correctedDesiredState = new SwerveModuleState();
correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
// Optimize the reference state to avoid spinning further than 90 degrees.
correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
// Command driving and turning SPARKS towards their respective setpoints.
m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
m_desiredState = desiredState;
}
/** Zeroes all the SwerveModule encoders. */
public void resetEncoders() {
m_drivingEncoder.setPosition(0);
}
}