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

Commit

Permalink
chore: Format.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 11, 2024
1 parent ad75032 commit 734cd58
Show file tree
Hide file tree
Showing 10 changed files with 49 additions and 40 deletions.
12 changes: 4 additions & 8 deletions src/main/java/frc/lib/controller/VelocityControllerIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -57,16 +57,12 @@ public static void addToShuffleboard(
velocityController.addDouble(
"Acceleration (dpsps)",
() -> Units.rotationsToDegrees(values.accelerationRotationsPerSecondPerSecond));
velocityController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
velocityController.addDouble(
"Velocity (rps)", () -> values.velocityRotationsPerSecond);
"Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
velocityController.addDouble("Velocity (rpm)", () -> values.velocityRotationsPerSecond * 60);
velocityController.addDouble(
"Acceleration (rpsps)",
() -> values.accelerationRotationsPerSecondPerSecond);
velocityController.addDouble(
"Velocity (rpm)", () -> values.velocityRotationsPerSecond * 60);
velocityController.addDouble(
"Acceleration (rpmpm)",
() -> values.accelerationRotationsPerSecondPerSecond * 60);
"Acceleration (rpmpm)", () -> values.accelerationRotationsPerSecondPerSecond * 60);
velocityController.addDouble("Voltage (V)", () -> values.motorVolts);
velocityController.addDouble("Current (A)", () -> values.motorAmps);
}
Expand Down
8 changes: 3 additions & 5 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
Expand Down Expand Up @@ -32,7 +32,7 @@ public class RobotContainer {

private final CommandXboxController driverController, operatorController;

private final XboxController rumbleController;
private final XboxController rumbleController;

/** Creates a new instance of the robot container. */
private RobotContainer() {
Expand Down Expand Up @@ -103,9 +103,7 @@ private void configureBindings() {

public Command rumble(RumbleType side) {
return Commands.startEnd(
() -> rumbleController.setRumble(side, 1),
() -> rumbleController.setRumble(side, 0)
);
() -> rumbleController.setRumble(side, 1), () -> rumbleController.setRumble(side, 0));
}

/**
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/arm/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,7 @@ public void periodic() {
setpoint =
new ArmState(
ShoulderConstants.MOTION_PROFILE.calculate(
dt,
setpoint.shoulderRotations(),
goal.shoulderRotations()));
dt, setpoint.shoulderRotations(), goal.shoulderRotations()));

shoulder.setSetpoint(
setpoint.shoulderRotations().position, setpoint.shoulderRotations().velocity);
Expand All @@ -78,7 +76,10 @@ public void periodic() {
public void addToShuffleboard(ShuffleboardTab tab) {
PositionControllerIO.addToShuffleboard(tab, "Shoulder", shoulderValues);

Telemetry.addColumn(tab, "Setpoint").addDouble("Setpoint (deg)", () -> Units.rotationsToDegrees(setpoint.shoulderRotations().position));
Telemetry.addColumn(tab, "Setpoint")
.addDouble(
"Setpoint (deg)",
() -> Units.rotationsToDegrees(setpoint.shoulderRotations().position));
}

public ArmState getState() {
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/arm/ArmState.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.math.util.Units;
import frc.robot.arm.ArmConstants.ShoulderConstants;

import java.util.Objects;

public record ArmState(State shoulderRotations) {
Expand Down Expand Up @@ -36,6 +35,8 @@ public ArmState(Rotation2d shoulder) {

public boolean at(ArmState other) {
return MathUtil.isNear(
shoulderRotations.position, other.shoulderRotations.position, Units.degreesToRotations(2.0));
shoulderRotations.position,
other.shoulderRotations.position,
Units.degreesToRotations(2.0));
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/intake/Intake.java
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ private boolean frontRollerStuck() {
private boolean backRollerStuck() {
return backRollerValues.motorAmps > BackRollerConstants.NOTE_AMPS;
}

public IntakeState getState() {
return new IntakeState(
frontRollerValues.velocityRotationsPerSecond, backRollerValues.velocityRotationsPerSecond);
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/intake/IntakeState.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,18 @@
import edu.wpi.first.math.MathUtil;
import frc.robot.intake.IntakeConstants.BackRollerConstants;
import frc.robot.intake.IntakeConstants.FrontRollerConstants;

import java.util.Objects;

public record IntakeState(
double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) {

public static final IntakeState IDLE = new IntakeState(0, 0);

public static final IntakeState INTAKE = new IntakeState(FrontRollerConstants.INTAKE_SPEED, BackRollerConstants.INTAKE_SPEED);
public static final IntakeState INTAKE =
new IntakeState(FrontRollerConstants.INTAKE_SPEED, BackRollerConstants.INTAKE_SPEED);

public static final IntakeState EJECT = new IntakeState(FrontRollerConstants.EJECT_SPEED, BackRollerConstants.EJECT_SPEED);
public static final IntakeState EJECT =
new IntakeState(FrontRollerConstants.EJECT_SPEED, BackRollerConstants.EJECT_SPEED);

public IntakeState {
Objects.requireNonNull(frontRollerVelocityRotationsPerSecond);
Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/shooter/Shooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,12 @@ public void periodic() {

setpoint = goal;

double flywheelSetpoint = FlywheelConstants.ACCELERATION_LIMITER.calculate(setpoint.flywheelVelocityRotationsPerSecond());
double serializerSetpoint = SerializerConstants.ACCELERATION_LIMITER.calculate(setpoint.serializerVelocityRotationsPerSecond());
double flywheelSetpoint =
FlywheelConstants.ACCELERATION_LIMITER.calculate(
setpoint.flywheelVelocityRotationsPerSecond());
double serializerSetpoint =
SerializerConstants.ACCELERATION_LIMITER.calculate(
setpoint.serializerVelocityRotationsPerSecond());

flywheel.setSetpoint(flywheelSetpoint);
serializer.setSetpoint(serializerSetpoint);
Expand All @@ -77,7 +81,7 @@ public void addToShuffleboard(ShuffleboardTab tab) {

public Trigger serializedNote() {
return new Trigger(() -> serializerValues.motorAmps > SerializerConstants.NOTE_AMPS);
}
}

public ShooterState getState() {
return new ShooterState(
Expand Down
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/shooter/ShooterConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ public static class SerializerConstants {
/** Maximum speed in rotations per second. */
public static final double MAXIMUM_SPEED = 45.319;

public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1));
public static final SlewRateLimiter ACCELERATION_LIMITER =
new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1));

public static final double NOTE_AMPS = 20;
}
Expand Down Expand Up @@ -88,6 +89,7 @@ public static class FlywheelConstants {
/** Maximum speed in rotations per second. */
public static final double MAXIMUM_SPEED = 60;

public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1));
public static final SlewRateLimiter ACCELERATION_LIMITER =
new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1));
}
}
25 changes: 16 additions & 9 deletions src/main/java/frc/robot/shooter/ShooterState.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import edu.wpi.first.math.MathUtil;
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
import frc.robot.shooter.ShooterConstants.SerializerConstants;

import java.util.Objects;

public record ShooterState(
Expand All @@ -13,27 +12,35 @@ public record ShooterState(

public static final ShooterState INTAKE = new ShooterState(0, SerializerConstants.INTAKE_SPEED);

public static final ShooterState PULL = new ShooterState(FlywheelConstants.PULL_SPEED, SerializerConstants.PULL_SPEED);
public static final ShooterState PULL =
new ShooterState(FlywheelConstants.PULL_SPEED, SerializerConstants.PULL_SPEED);

public static final ShooterState EJECT = new ShooterState(0, SerializerConstants.EJECT_SPEED);

public static final ShooterState SPEAKER_READY = new ShooterState(FlywheelConstants.SPEAKER_SPEED, 0);
public static final ShooterState SPEAKER_READY =
new ShooterState(FlywheelConstants.SPEAKER_SPEED, 0);

public static final ShooterState SPEAKER_SHOOT = new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED);
public static final ShooterState SPEAKER_SHOOT =
new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED);

public static final ShooterState PODIUM_READY = new ShooterState(FlywheelConstants.PODIUM_SPEED, 0);
public static final ShooterState PODIUM_READY =
new ShooterState(FlywheelConstants.PODIUM_SPEED, 0);

public static final ShooterState PODIUM_SHOOT = new ShooterState(FlywheelConstants.PODIUM_SPEED, SerializerConstants.FAST_FEED_SPEED);
public static final ShooterState PODIUM_SHOOT =
new ShooterState(FlywheelConstants.PODIUM_SPEED, SerializerConstants.FAST_FEED_SPEED);

public static final ShooterState LOB_READY = new ShooterState(FlywheelConstants.LOB_SPEED, 0);

public static final ShooterState LOB_SHOOT = new ShooterState(FlywheelConstants.LOB_SPEED, SerializerConstants.FAST_FEED_SPEED);
public static final ShooterState LOB_SHOOT =
new ShooterState(FlywheelConstants.LOB_SPEED, SerializerConstants.FAST_FEED_SPEED);

public static final ShooterState SKIM_READY = new ShooterState(FlywheelConstants.SKIM_SPEED, 0);

public static final ShooterState SKIM_SHOOT = new ShooterState(FlywheelConstants.SKIM_SPEED, SerializerConstants.FAST_FEED_SPEED);
public static final ShooterState SKIM_SHOOT =
new ShooterState(FlywheelConstants.SKIM_SPEED, SerializerConstants.FAST_FEED_SPEED);

public static final ShooterState AMP_SHOOT = new ShooterState(FlywheelConstants.AMP_SPEED, SerializerConstants.SLOW_FEED_SPEED);
public static final ShooterState AMP_SHOOT =
new ShooterState(FlywheelConstants.AMP_SPEED, SerializerConstants.SLOW_FEED_SPEED);

public ShooterState {
Objects.requireNonNull(flywheelVelocityRotationsPerSecond);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,7 @@

public class SuperstructureConstants {

public static final double PULL_DURATION = 0.15;

public static final double EJECT_PAUSE = 0.25;
public static final double PULL_DURATION = 0.15;

public static final double EJECT_PAUSE = 0.25;
}

0 comments on commit 734cd58

Please sign in to comment.