Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
refactor: Continue WIP.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 26, 2024
1 parent e536e97 commit 372b847
Show file tree
Hide file tree
Showing 10 changed files with 196 additions and 163 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -107,11 +107,11 @@ public Command rumble(RumbleType side) {
}

/**
* Gets the command to run during the autonomous period.
* Returns the command to run during the autonomous period.
*
* @return the command to run during the autonomous period.
*/
public Command getAutonomousCommand() {
return auto.getAutonomousCommand();
return auto.getSelectedCommand();
}
}
28 changes: 14 additions & 14 deletions src/main/java/frc/robot/arm/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@
/** Arm subsystem. */
public class Arm extends Subsystem {

/** Arm subsystem singleton. */
/** Arm singleton. */
private static Arm instance = null;

/** Shoulder's config. */
/** Shoulder config. */
private final MechanismConfig shoulderConfig =
new MechanismConfig()
.withAbsoluteEncoderConfig(
Expand Down Expand Up @@ -58,10 +58,10 @@ public class Arm extends Subsystem {
/** Motion profile of the shoulder. */
private final TrapezoidProfile shoulderMotionProfile;

/** Arm's goal. Set by superstructure. */
/** Arm goal. Set by superstructure. */
private ArmState goal;

/** Arm's setpoint. Updated periodically to reach goal within constraints. */
/** Arm setpoint. Updated periodically to reach goal within constraints. */
private ArmState setpoint;

/**
Expand All @@ -70,7 +70,7 @@ public class Arm extends Subsystem {
*/
private double previousTimeSeconds;

/** Creates the arm subsystem and configures arm hardware. */
/** Initializes the arm subsystem and configures arm hardware. */
private Arm() {
shoulder = ArmFactory.createShoulder(shoulderConfig);
shoulder.configure();
Expand All @@ -81,15 +81,15 @@ private Arm() {

previousTimeSeconds = Timer.getFPGATimestamp();

setPosition(ArmState.STOW);
setpoint = ArmState.STOW;
goal = ArmState.STOW;
setPosition(ArmState.STOW_POSITION);
setpoint = ArmState.STOW_POSITION;
goal = ArmState.STOW_POSITION;
}

/**
* Gets the instance of the arm subsystem.
* Returns the arm subsystem instance.
*
* @return the instance of the arm subsystem.
* @return the arm subsystem instance.
*/
public static Arm getInstance() {
if (instance == null) {
Expand Down Expand Up @@ -130,9 +130,9 @@ public void addToShuffleboard(ShuffleboardTab tab) {
}

/**
* Returns the arm's state.
* Returns the arm state.
*
* @return the arm's state.
* @return the arm state.
*/
public ArmState getState() {
return new ArmState(
Expand All @@ -141,9 +141,9 @@ public ArmState getState() {
}

/**
* Sets the arm's goal state.
* Sets the arm goal state.
*
* @param goal the arm's goal state.
* @param goal the arm goal state.
*/
public void setGoal(ArmState goal) {
this.goal = goal;
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/arm/ArmFactory.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,13 @@
import frc.robot.RobotConstants;
import frc.robot.RobotConstants.Subsystem;

/** Helper class for creating hardware for the arm subsystem. */
/** Factory for creating arm subsystem hardware. */
public class ArmFactory {

/**
* Creates a shoulder motor.
* Creates the shoulder controller.
*
* @return a shoulder motor.
* @return the shoulder controller.
*/
public static PositionControllerIO createShoulder(MechanismConfig config) {
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) {
Expand Down
34 changes: 27 additions & 7 deletions src/main/java/frc/robot/arm/ArmState.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,28 +6,48 @@
import edu.wpi.first.math.util.Units;
import java.util.Objects;

/** Arm state. */
public record ArmState(State shoulderRotations) {

public static final ArmState STOW = new ArmState(Rotation2d.fromDegrees(-26));
/** Stow position. */
public static final ArmState STOW_POSITION = new ArmState(Rotation2d.fromDegrees(-26));

public static final ArmState SUBWOOFER = new ArmState(Rotation2d.fromDegrees(-26));
/** Subwoofer shot position. */
public static final ArmState SUBWOOFER_POSITION = new ArmState(Rotation2d.fromDegrees(-26));

public static final ArmState EJECT = new ArmState(Rotation2d.fromDegrees(30));
/** Eject position. */
public static final ArmState EJECT_POSITION = new ArmState(Rotation2d.fromDegrees(30));

public static final ArmState SKIM = new ArmState(Rotation2d.fromDegrees(30));
/** Skim shot position. */
public static final ArmState SKIM_POSITION = new ArmState(Rotation2d.fromDegrees(30));

public static final ArmState LOB = new ArmState(Rotation2d.fromDegrees(-26));

public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(60));
/** Amp position. */
public static final ArmState AMP_POSITION = new ArmState(Rotation2d.fromDegrees(60));

/**
* Arm state.
*
* @param shoulderRotations the shoulder state in rotations and rotations per second.
*/
public ArmState {
Objects.requireNonNull(shoulderRotations);
}

/**
* Arm state.
*
* @param shoulder the shoulder position.
*/
public ArmState(Rotation2d shoulder) {
this(new State(shoulder.getRotations(), 0));
}

/**
* Returns true if this arm state is at another arm state.
*
* @param other another arm state.
* @return true if this arm state is at another arm state.
*/
public boolean at(ArmState other) {
return MathUtil.isNear(
shoulderRotations.position,
Expand Down
33 changes: 12 additions & 21 deletions src/main/java/frc/robot/auto/Auto.java
Original file line number Diff line number Diff line change
Expand Up @@ -16,25 +16,25 @@
import frc.robot.swerve.Swerve;
import frc.robot.swerve.SwerveConstants;

/** Subsystem class for the auto subsystem. */
/** Auto subsystem. */
public class Auto extends Subsystem {

/** Instance variable for the auto subsystem singleton. */
/** Auto singleton. */
private static Auto instance = null;

/** Reference to the odometry subsystem. */
/** Odometry subsystem reference. */
private final Odometry odometry;

/** Reference to the superstructure subsystem. */
/** Superstructure subsystem reference. */
private final Superstructure superstructure;

/** Reference to the swerve subsystem. */
/** Swerve subsystem reference. */
private final Swerve swerve;

/** Sendable chooser for the subsystem. */
/** Auto command chooser. */
private final SendableChooser<Command> autoChooser;

/** Creates a new instance of the auto subsystem. */
/** Initializes the auto subsystem. */
private Auto() {
odometry = Odometry.getInstance();
superstructure = Superstructure.getInstance();
Expand Down Expand Up @@ -65,9 +65,9 @@ private Auto() {
}

/**
* Gets the instance of the auto subsystem.
* Returns the auto subsystem instance.
*
* @return the instance of the auto subsystem.
* @return the auto subsystem instance.
*/
public static Auto getInstance() {
if (instance == null) {
Expand All @@ -86,20 +86,11 @@ public void addToShuffleboard(ShuffleboardTab tab) {
}

/**
* Gets the command to run during the autonomous period.
* Returns the selected auto command.
*
* @return the command to run during the autonomous period.
* @return the selected auto command.
*/
public Command getAutonomousCommand() {
public Command getSelectedCommand() {
return autoChooser.getSelected();
}

/**
* Gets the chooser for the command to run during the autonomous period.
*
* @return the chooser for the command to run during the autonomous period.
*/
public SendableChooser<Command> getAutonomousChooser() {
return autoChooser;
}
}
Loading

0 comments on commit 372b847

Please sign in to comment.