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

Commit

Permalink
refactor(superstructure): WIP.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 27, 2024
1 parent 7abd089 commit 566cc14
Show file tree
Hide file tree
Showing 12 changed files with 183 additions and 106 deletions.
7 changes: 3 additions & 4 deletions src/main/java/frc/robot/arm/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ public class Arm extends Subsystem {
/** Arm singleton. */
private static Arm instance = null;

/** Shoulder config. */
/** Shoulder controller config. */
private final MechanismConfig shoulderConfig =
new MechanismConfig()
.withAbsoluteEncoderConfig(
Expand Down Expand Up @@ -81,9 +81,8 @@ private Arm() {

previousTimeSeconds = Timer.getFPGATimestamp();

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

/**
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/arm/ArmState.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,19 +10,19 @@
public record ArmState(State shoulderRotations) {

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

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

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

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

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

/**
* Arm state.
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/auto/Auto.java
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,6 @@ private Auto() {
NamedCommands.registerCommand("stow", superstructure.stow());
NamedCommands.registerCommand(
"shoot", superstructure.subwoofer().withTimeout(1.5)); // 1 second could work
NamedCommands.registerCommand(
"shootNoPull", superstructure.subwooferNoPull().withTimeout(1.5)); // 1 second could work
NamedCommands.registerCommand("intake", superstructure.intakeInstant());

autoChooser = AutoBuilder.buildAutoChooser();
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/intake/Intake.java
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ public class Intake extends Subsystem {
/** Intake singleton. */
private static Intake instance = null;

/** Front roller config. */
/** Front roller controller config. */
private final MechanismConfig frontRollerConfig =
new MechanismConfig()
.withMotorConfig(
Expand All @@ -38,7 +38,7 @@ public class Intake extends Subsystem {
.withProportionalGain(0.1) // volts per rotation per second
);

/** Back roller config. */
/** Back roller controller config. */
private final MechanismConfig backRollerConfig =
new MechanismConfig()
.withMotorConfig(
Expand Down Expand Up @@ -83,8 +83,8 @@ private Intake() {

backRollerValues = new VelocityControllerIOValues();

setpoint = IntakeState.IDLING;
goal = IntakeState.IDLING;
setpoint = IntakeState.IDLE;
goal = IntakeState.IDLE;
}

/**
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/intake/IntakeState.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,10 @@ public record IntakeState(
double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) {

/** Idling state. */
public static final IntakeState IDLING = new IntakeState(0, 0);
public static final IntakeState IDLE = new IntakeState(0, 0);

/** Intaking state. */
public static final IntakeState INTAKING = new IntakeState(34.0, 34.0);
public static final IntakeState INTAKE = new IntakeState(34.0, 34.0);

/** Ejecting state. */
public static final IntakeState EJECTING = new IntakeState(-34.0, -34.0);
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/shooter/Shooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ public class Shooter extends Subsystem {
/** Shooter singleton. */
private static Shooter instance = null;

/** Flywheel controller config. */
private final MechanismConfig flywheelConfig =
new MechanismConfig()
.withMotorConfig(
Expand Down Expand Up @@ -49,7 +50,7 @@ public class Shooter extends Subsystem {
/** Flywheel controller acceleration limiter. */
private final SlewRateLimiter flywheelAccelerationLimiter;

/** Serializer config. */
/** Serializer controller config. */
private final MechanismConfig serializerConfig =
new MechanismConfig()
.withMotorConfig(
Expand Down
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/shooter/ShooterFactory.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,28 +13,28 @@
public class ShooterFactory {

/**
* Creates the serializer controller.
* Creates the flywheel controller.
*
* @param config the serializer controller config.
* @return the serializer controller.
* @param config the flywheel controller config.
* @return the flywheel controller.
*/
public static VelocityControllerIO createSerializer(MechanismConfig config) {
public static VelocityControllerIO createFlywheel(MechanismConfig config) {
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) {
return new VelocityControllerIOTalonFXPIDF(new CAN(42), config);
return new VelocityControllerIOTalonFXPIDF(new CAN(44), config);
}

return new VelocityControllerIOSim();
}

/**
* Creates the flywheel controller.
* Creates the serializer controller.
*
* @param config the flywheel controller config.
* @return the flywheel controller.
* @param config the serializer controller config.
* @return the serializer controller.
*/
public static VelocityControllerIO createFlywheel(MechanismConfig config) {
public static VelocityControllerIO createSerializer(MechanismConfig config) {
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) {
return new VelocityControllerIOTalonFXPIDF(new CAN(44), config);
return new VelocityControllerIOTalonFXPIDF(new CAN(42), config);
}

return new VelocityControllerIOSim();
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/shooter/ShooterState.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,22 +11,22 @@ public record ShooterState(
public static final ShooterState IDLING = new ShooterState(0, 0);

/** Intaking state. */
public static final ShooterState INTAKING = new ShooterState(0, 34);
public static final ShooterState INTAKE = new ShooterState(0, 34);

/** Pulling state. */
public static final ShooterState PULLING = new ShooterState(-20, -10);
public static final ShooterState PULL = new ShooterState(-20, -10);

/** Ejecting state. */
public static final ShooterState EJECTING = new ShooterState(0, -44);
public static final ShooterState EJECT = new ShooterState(0, -44);

/** Subwoofer shooting state. */
public static final ShooterState SUBWOOFER_SHOOTING = new ShooterState(60, 44);
public static final ShooterState SUBWOOFER = new ShooterState(60, 44);

/** Skim shooting state. */
public static final ShooterState SKIM_SHOOTING = new ShooterState(60, 44);
public static final ShooterState SKIM = new ShooterState(60, 44);

/** Amp shooting state. */
public static final ShooterState AMPING = new ShooterState(10, 20);
public static final ShooterState AMP = new ShooterState(10, 20);

/**
* Shooter state.
Expand Down
Loading

0 comments on commit 566cc14

Please sign in to comment.