Skip to content

Commit

Permalink
Rename SlotGains to DriveSlotGains
Browse files Browse the repository at this point in the history
Comment methods in DriveSubsystem
  • Loading branch information
IanTapply22 committed Jan 4, 2024
1 parent 2d27867 commit 1180d92
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 8 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/subsystems/drive/DriveConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@

public class DriveConstants {
/** Only touch these if you know what you're doing */
private static final SlotGains steerGains = new SlotGains(100.0, 0, 0.05, 0, 0);
private static final DriveSlotGains steerGains = new DriveSlotGains(100.0, 0, 0.05, 0, 0);

private static final SlotGains driveGains = new SlotGains(3.0, 0, 0, 0, 0);
private static final DriveSlotGains driveGains = new DriveSlotGains(3.0, 0, 0, 0, 0);

public static final double kMaxSpeed = 6.0; // 6 meters per second desired top speed
public static final double kMaxAngularRate =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

import com.ctre.phoenix6.configs.Slot0Configs;

public class SlotGains extends Slot0Configs {
public SlotGains(double kP, double kI, double kD, double kV, double kS) {
public class DriveSlotGains extends Slot0Configs {
public DriveSlotGains(double kP, double kI, double kD, double kV, double kS) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
Expand Down
39 changes: 35 additions & 4 deletions src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,13 @@ public class DriveSubsystem extends SwerveDrivetrain implements Subsystem {

@Override
public void simulationPeriodic() {
/* Assume */
updateSimState(0.02, 12);
}

/**
* Configures path planner to have a holonomic path follower so it can move in
* any direction. This is meant for holonomic drivetrains AND swerve
*/
private void configurePathPlanner() {
double driveBaseRadius = 0;
for (var moduleLocation : m_moduleLocations) {
Expand All @@ -53,9 +56,14 @@ private void configurePathPlanner() {
DriveConstants.kSpeedAt12VoltsMps,
driveBaseRadius,
new ReplanningConfig()),
this); // Subsystem for requirements
this);
}

/**
* Applies and runs a command on the swerve drivetrain
* @param requestSupplier The supplier of the swerve request to apply (the request)
* @return A command which runs the specified request
*/
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return new RunCommand(
() -> {
Expand All @@ -64,6 +72,11 @@ public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
this);
}

/**
* Returns a command that will run the specified pathplanner path
* @param pathName The name of the path created in pathplanner
* @return A command which runs the specified path
*/
public Command getAutoPath(String pathName) {
return new PathPlannerAuto(pathName);
}
Expand All @@ -85,10 +98,21 @@ public void seedFieldRelative(Pose2d location) {
}
}

/**
* Returns the current robot chassis speeds
*
* @return The current robot chassis speeds as a Twist2d object
*/
public ChassisSpeeds getCurrentRobotChassisSpeeds() {
return m_kinematics.toChassisSpeeds(getState().ModuleStates);
}

/**
* Updates the state of the drivetrain for simulation
*
* @param deltaTime The time since the last update
* @param batteryVoltage The current battery voltage
*/
private void startSimThread() {
m_lastSimTime = Utils.getCurrentTimeSeconds();

Expand All @@ -106,28 +130,35 @@ private void startSimThread() {
m_simNotifier.startPeriodic(DriveConstants.kSimLoopPeriod);
}

/**
* Add any methods you want to call when the drive subsystem is initialized and called
*/
public void initialize() {
configurePathPlanner();
}

/** Initialize the Drive subsystem when we create an instance of it * */
/** Initialize the drive subsystem when we create an instance of it and configure it * */
public DriveSubsystem(
SwerveDrivetrainConstants driveTrainConstants,
double OdometryUpdateFrequency,
SwerveModuleConstants... modules) {
super(driveTrainConstants, OdometryUpdateFrequency, modules);
this.initialize();

if (Utils.isSimulation()) {
startSimThread();
}
}

/** Initialize the drive subsystem when we create an instance of it and configure it * */
public DriveSubsystem(
SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
super(driveTrainConstants, modules);
this.initialize();

if (Utils.isSimulation()) {
startSimThread();
}
this.initialize();

}
}

0 comments on commit 1180d92

Please sign in to comment.