Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
Refactor subsystem constructors to do as little as possible (#87)
Browse files Browse the repository at this point in the history
This is recommended by the WPILib team as best practice

Also splits constants a bit and fixes constructor docs
  • Loading branch information
spacey-sooty authored Jul 25, 2024
1 parent 95140c1 commit 333bed5
Show file tree
Hide file tree
Showing 5 changed files with 38 additions and 55 deletions.
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,19 @@
public final class Constants {
public static final int driverport = 0;
public static final int codriverport = 1;

public static final int shooterPort = 31;
public static final double shooterP = 0.5;
public static final double shooterI = 0;
public static final double shooterD = 0;

public static final int climberPort = 32;
public static final double climberP = 0.35;
public static final double climberI = 0;
public static final double climberD = 0;

public static final int intakePort = 35;

public static final double armP = 21.0;
public static final double armI = 0;
public static final double armD = 0.015;
Expand All @@ -28,6 +32,7 @@ public final class Constants {
public static final int armLeadPort = 21;
public static final int armFollowerPort = 26;
public static final int armEncoderPort = 3;

public static final double DrivebaseMaxSpeed = TunerConstants.kSpeedAt12VoltsMps;
public static final double DrivebaseMaxAngularRate = 1.5 * Math.PI;
}
34 changes: 13 additions & 21 deletions src/main/java/frc/robot/subsystems/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -29,11 +29,17 @@ public enum Setpoint {
kStowed
}

private final PIDController m_pid;
private final CANSparkMax m_primaryMotor;
private final CANSparkMax m_followerMotor;
private final DutyCycleEncoder m_encoder;
private final ArmFeedforward m_feedforward;
private final CANSparkMax m_primaryMotor =
new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless);
private final CANSparkMax m_followerMotor =
new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless);
private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort);

private final PIDController m_pid =
new PIDController(Constants.armP, Constants.armI, Constants.armD);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(Constants.armS, Constants.armG, Constants.armV, Constants.armA);

private final DataLog m_log = DataLogManager.getLog();
private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output");
private final DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint");
Expand All @@ -44,25 +50,11 @@ public enum Setpoint {
private final DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output");
private final StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint");

public final Trigger m_atSetpoint;
public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);

/**
* Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
*
* @param primaryMotor The primary motor that controls the arm.
*/
/** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
public Arm() {
m_primaryMotor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless);
m_followerMotor = new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless);
m_followerMotor.follow(m_primaryMotor);

m_encoder = new DutyCycleEncoder(Constants.armEncoderPort);
m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD);
m_pid.setTolerance(0.2);
m_feedforward =
new ArmFeedforward(Constants.armS, Constants.armG, Constants.armV, Constants.armA);

m_atSetpoint = new Trigger(m_pid::atSetpoint);
}

/** Achieves and maintains speed for the primary motor. */
Expand Down
22 changes: 8 additions & 14 deletions src/main/java/frc/robot/subsystems/Climber.java
Original file line number Diff line number Diff line change
Expand Up @@ -18,23 +18,17 @@

/** Our Crescendo climber Subsystem */
public class Climber extends SubsystemBase {
private final PIDController m_pid;
private final CANSparkMax m_motor;
private final RelativeEncoder m_encoder;
private final CANSparkMax m_motor = new CANSparkMax(Constants.climberPort, MotorType.kBrushless);
private final RelativeEncoder m_encoder = m_motor.getEncoder();

private final PIDController m_pid =
new PIDController(Constants.climberP, Constants.climberI, Constants.climberD);

private final DoubleLogEntry log_pid_output =
new DoubleLogEntry(DataLogManager.getLog(), "/climber/pid/output");

/**
* Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
*
* @param motor The motor that the climber controls.
*/
public Climber() {
m_motor = new CANSparkMax(Constants.climberPort, MotorType.kBrushless);
m_encoder = m_motor.getEncoder();
m_pid = new PIDController(Constants.climberP, Constants.climberI, Constants.climberD);
m_pid.setTolerance(0.2, 0.5);
}
/** Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
public Climber() {}

/**
* Climb the robot!
Expand Down
7 changes: 3 additions & 4 deletions src/main/java/frc/robot/subsystems/Intake.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,12 @@
import frc.robot.Constants;

public class Intake extends SubsystemBase {
private final CANSparkMax m_motor;
private final CANSparkMax m_motor = new CANSparkMax(Constants.intakePort, MotorType.kBrushless);

private final DoubleLogEntry log_output =
new DoubleLogEntry(DataLogManager.getLog(), "/intake/output");

public Intake() {
m_motor = new CANSparkMax(Constants.intakePort, MotorType.kBrushless);
}
public Intake() {}

public Command intake() {
return Commands.run(
Expand Down
25 changes: 9 additions & 16 deletions src/main/java/frc/robot/subsystems/Shooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,26 +20,19 @@

/** Our Crescendo shooter Subsystem */
public class Shooter extends SubsystemBase {
private final PIDController m_pid;
private final CANSparkMax m_motor;
private final RelativeEncoder m_encoder;
private final PIDController m_pid =
new PIDController(Constants.shooterP, Constants.shooterI, Constants.shooterD);
private final CANSparkMax m_motor = new CANSparkMax(Constants.shooterPort, MotorType.kBrushless);
private final RelativeEncoder m_encoder = m_motor.getEncoder();

private final DataLog m_log = DataLogManager.getLog();
private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output");

public final Trigger m_atSetpoint;

/**
* Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
*
* @param motor The motor that the shooter controls.
*/
public Shooter() {
m_motor = new CANSparkMax(Constants.shooterPort, MotorType.kBrushless);
m_encoder = m_motor.getEncoder();
m_pid = new PIDController(Constants.shooterP, Constants.shooterI, Constants.shooterD);
public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
;

m_atSetpoint = new Trigger(m_pid::atSetpoint);
}
/** Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
public Shooter() {}

/** Acheives and maintains speed. */
private Command achieveSpeeds(double speed) {
Expand Down

0 comments on commit 333bed5

Please sign in to comment.