diff --git a/simgui.json b/simgui.json index 10a8815..fc40c78 100644 --- a/simgui.json +++ b/simgui.json @@ -63,8 +63,7 @@ }, "Velocities": { "open": true - }, - "open": true + } }, "Climber": { "East": { diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index cdfbcaa..9a44699 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -1,16 +1,12 @@ package frc.robot.arm; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.lib.Subsystem; import frc.lib.controller.PositionControllerIO; import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; import frc.robot.RobotConstants; import frc.robot.arm.ArmConstants.ShoulderConstants; -import frc.robot.superstructure.SuperstructureConstants; -import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; /** Subsystem class for the arm subsystem. */ public class Arm extends Subsystem { @@ -25,7 +21,7 @@ public class Arm extends Subsystem { private final PositionControllerIOValues shoulderValues; /** Arm goal. */ - private State setpoint, goal; + private ArmState setpoint, goal; /** Creates a new instance of the arm subsystem. */ private Arm() { @@ -33,9 +29,9 @@ private Arm() { shoulderValues = new PositionControllerIOValues(); shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS); - setPosition(ShoulderAngleConstants.INITIAL.getRotations()); - setpoint = new State(ShoulderAngleConstants.INITIAL.getRotations(), 0); - goal = new State(ShoulderAngleConstants.INITIAL.getRotations(), 0); + setPosition(new ArmState(ShoulderConstants.INITIAL_ANGLE)); + setpoint = new ArmState(ShoulderConstants.INITIAL_ANGLE); + goal = new ArmState(ShoulderConstants.INITIAL_ANGLE); } /** @@ -55,12 +51,12 @@ public static Arm getInstance() { public void periodic() { shoulder.update(shoulderValues); - setpoint = ShoulderAngleConstants.MOTION_PROFILE.calculate( + setpoint = new ArmState(ShoulderConstants.MOTION_PROFILE.calculate( RobotConstants.PERIODIC_DURATION, - setpoint, - goal); + setpoint.shoulderRotations(), + goal.shoulderRotations())); - shoulder.setSetpoint(setpoint.position, setpoint.velocity); + shoulder.setSetpoint(setpoint.shoulderRotations().position, setpoint.shoulderRotations().velocity); } @Override @@ -68,23 +64,20 @@ public void addToShuffleboard(ShuffleboardTab tab) { PositionControllerIO.addToShuffleboard(tab, "Shoulder", shoulderValues); } - public State getState() { - return new TrapezoidProfile.State( - shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond); + public ArmState getState() { + return new ArmState(new TrapezoidProfile.State( + shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond)); } - public void setGoal(State goal) { + public void setGoal(ArmState goal) { this.goal = goal; } public boolean atGoal() { - return MathUtil.isNear( - getState().position, - goal.position, - SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations()); + return getState().at(goal); } - public void setPosition(double positionRotations) { - shoulder.setPosition(positionRotations); + public void setPosition(ArmState armState) { + shoulder.setPosition(armState.shoulderRotations().position); } } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index 74bad52..07e438d 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -1,13 +1,15 @@ package frc.robot.arm; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.lib.CAN; +import frc.lib.MotionProfileCalculator; import frc.lib.PIDFConstants; import frc.lib.controller.PositionControllerIO.PositionControllerIOConstants; /** Constants for the arm subsystem. */ public class ArmConstants { - /** Constants for the shoulder. */ public static class ShoulderConstants { /** Shoulder's leader CAN. */ @@ -36,6 +38,32 @@ public static class ShoulderConstants { CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571; CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07); } - } + public static final Rotation2d INITIAL_ANGLE = Rotation2d.fromDegrees(-26.45); + + public static final Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(-26.45); + + public static final Rotation2d SHOOT_ANGLE = Rotation2d.fromDegrees(-15); + + public static final Rotation2d EJECT_ANGLE = Rotation2d.fromDegrees(0); + + public static final Rotation2d AMP_ANGLE = Rotation2d.fromDegrees(90); + + /** Tolerance of the shoulder. */ + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); + + /** Maximum speed of the shoulder in rotations per second. */ + public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0); + + /** Maximum acceleration of the shoulder in rotations per second per second. */ + public static final double MAXIMUM_ACCELERATION = + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); + + /** Maximum speed and acceleration of the shoulder . */ + public static final TrapezoidProfile.Constraints CONSTRAINTS = + new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); + + /** Motion profile of the shoulder. */ + public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); + } } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java new file mode 100644 index 0000000..a91d3bd --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -0,0 +1,27 @@ +package frc.robot.arm; + +import java.util.Objects; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import frc.robot.arm.ArmConstants.ShoulderConstants; + +public record ArmState(State shoulderRotations) { + + public ArmState { + Objects.requireNonNull(shoulderRotations); + } + + public ArmState(Rotation2d shoulder) { + this(new State(shoulder.getRotations(), 0)); + } + + public boolean at(ArmState other) { + return MathUtil.isNear( + shoulderRotations.position, + other.shoulderRotations.position, + ShoulderConstants.TOLERANCE.getRotations()); + } + +} diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index b5937e7..971d8cd 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -1,13 +1,11 @@ package frc.robot.intake; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.lib.Subsystem; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; import frc.robot.intake.IntakeConstants.BackRollerConstants; import frc.robot.intake.IntakeConstants.FrontRollerConstants; -import frc.robot.intake.IntakeConstants.RollerConstants; /** Subsystem class for the intake subsystem. */ public class Intake extends Subsystem { @@ -21,7 +19,7 @@ public class Intake extends Subsystem { /** Roller values. */ private final VelocityControllerIOValues frontRollerValues, backRollerValues; - private double rollerGoal; + private IntakeState setpoint, goal; /** Creates a new instance of the intake subsystem. */ private Intake() { @@ -32,6 +30,9 @@ private Intake() { backRoller = IntakeFactory.createBackRoller(); backRollerValues = new VelocityControllerIOValues(); backRoller.configure(BackRollerConstants.CONTROLLER_CONSTANTS); + + setpoint = new IntakeState(0, 0); + goal = new IntakeState(0, 0); } /** @@ -52,8 +53,10 @@ public void periodic() { frontRoller.update(frontRollerValues); backRoller.update(backRollerValues); - frontRoller.setSetpoint(rollerGoal); - backRoller.setSetpoint(rollerGoal); + setpoint = goal; + + frontRoller.setSetpoint(setpoint.frontRollerVelocityRotationsPerSecond()); + backRoller.setSetpoint(setpoint.backRollerVelocityRotationsPerSecond()); } @Override @@ -62,22 +65,15 @@ public void addToShuffleboard(ShuffleboardTab tab) { VelocityControllerIO.addToShuffleboard(tab, "Back Roller", backRollerValues); } - public double getRollerVelocity() { - return (frontRollerValues.velocityRotationsPerSecond + backRollerValues.velocityRotationsPerSecond) / 2; + public IntakeState getState() { + return new IntakeState(frontRollerValues.velocityRotationsPerSecond, backRollerValues.velocityRotationsPerSecond); } - public void setGoal(double rollerVelocityRotationsPerSecond) { - this.rollerGoal = rollerVelocityRotationsPerSecond; + public void setGoal(IntakeState goal) { + this.goal = goal; } public boolean atGoal() { - return MathUtil.isNear( - frontRollerValues.velocityRotationsPerSecond, - rollerGoal, - RollerConstants.SPEED_TOLERANCE) - && MathUtil.isNear( - backRollerValues.velocityRotationsPerSecond, - rollerGoal, - RollerConstants.SPEED_TOLERANCE); + return getState().at(goal); } } diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java new file mode 100644 index 0000000..c10d976 --- /dev/null +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -0,0 +1,26 @@ +package frc.robot.intake; + +import java.util.Objects; + +import edu.wpi.first.math.MathUtil; +import frc.robot.intake.IntakeConstants.RollerConstants; + +public record IntakeState(double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) { + + public IntakeState { + Objects.requireNonNull(frontRollerVelocityRotationsPerSecond); + Objects.requireNonNull(backRollerVelocityRotationsPerSecond); + } + + public boolean at(IntakeState other) { + return MathUtil.isNear( + frontRollerVelocityRotationsPerSecond, + other.frontRollerVelocityRotationsPerSecond, + RollerConstants.SPEED_TOLERANCE) + && MathUtil.isNear( + backRollerVelocityRotationsPerSecond, + other.backRollerVelocityRotationsPerSecond, + RollerConstants.SPEED_TOLERANCE); + } + +} diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 0997871..2e4cced 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -1,6 +1,5 @@ package frc.robot.shooter; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.lib.Subsystem; import frc.lib.controller.VelocityControllerIO; @@ -26,7 +25,7 @@ public class Shooter extends Subsystem { /** Flywheel values. */ private final VelocityControllerIOValues flywheelValues; - private double serializerGoal, flywheelGoal; + private ShooterState setpoint, goal; /** Creates a new instance of the shooter subsystem. */ private Shooter() { @@ -37,6 +36,9 @@ private Shooter() { flywheel = ShooterFactory.createFlywheel(); flywheelValues = new VelocityControllerIOValues(); flywheel.configure(FlywheelConstants.CONTROLLER_CONSTANTS); + + setpoint = new ShooterState(0, 0); + goal = new ShooterState(0, 0); } /** @@ -57,8 +59,10 @@ public void periodic() { serializer.update(serializerValues); flywheel.update(flywheelValues); - flywheel.setSetpoint(flywheelGoal); - serializer.setSetpoint(serializerGoal); + setpoint = goal; + + flywheel.setSetpoint(setpoint.flywheelVelocityRotationsPerSecond()); + serializer.setSetpoint(setpoint.serializerVelocityRotationsPerSecond()); } @Override @@ -67,28 +71,16 @@ public void addToShuffleboard(ShuffleboardTab tab) { VelocityControllerIO.addToShuffleboard(tab, "Flywheel", flywheelValues); } - public double getFlywheelVelocity() { - return flywheelValues.velocityRotationsPerSecond; + public ShooterState getState() { + return new ShooterState(flywheelValues.velocityRotationsPerSecond, serializerValues.velocityRotationsPerSecond); } - public double getSerializerVelocity() { - return serializerValues.velocityRotationsPerSecond; - } - - public void setGoal( - double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { - this.flywheelGoal = flywheelVelocityRotationsPerSecond; - this.serializerGoal = serializerVelocityRotationsPerSecond; + public void setGoal(ShooterState goal) { + this.goal = goal; } public boolean atGoal() { - return MathUtil.isNear( - getFlywheelVelocity(), - flywheelGoal, - FlywheelConstants.SPEED_TOLERANCE) && - MathUtil.isNear( - getSerializerVelocity(), - serializerGoal, - FlywheelConstants.SPEED_TOLERANCE); + return getState().at(goal); } + } diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java new file mode 100644 index 0000000..b2c24bc --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -0,0 +1,20 @@ +package frc.robot.shooter; + +import java.util.Objects; + +import edu.wpi.first.math.MathUtil; +import frc.robot.shooter.ShooterConstants.FlywheelConstants; +import frc.robot.shooter.ShooterConstants.SerializerConstants; + +public record ShooterState(double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { + + public ShooterState { + Objects.requireNonNull(flywheelVelocityRotationsPerSecond); + Objects.requireNonNull(serializerVelocityRotationsPerSecond); + } + + public boolean at(ShooterState other) { + return MathUtil.isNear(flywheelVelocityRotationsPerSecond, other.flywheelVelocityRotationsPerSecond, FlywheelConstants.SPEED_TOLERANCE) && MathUtil.isNear(serializerVelocityRotationsPerSecond, other.serializerVelocityRotationsPerSecond, SerializerConstants.SPEED_TOLERANCE); + } + +} diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index b99ff9d..c5ae47c 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -1,6 +1,7 @@ package frc.robot.superstructure; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import java.util.function.Supplier; + import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -9,8 +10,11 @@ import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.arm.Arm; +import frc.robot.arm.ArmState; import frc.robot.intake.Intake; +import frc.robot.intake.IntakeState; import frc.robot.shooter.Shooter; +import frc.robot.shooter.ShooterState; /** Subsystem class for the superstructure subsystem. */ public class Superstructure extends Subsystem { @@ -60,55 +64,45 @@ public void periodic() { @Override public void addToShuffleboard(ShuffleboardTab tab) { - ShuffleboardLayout measurement = Telemetry.addColumn(tab, "Measurement"); - - measurement.addDouble( - "Shoulder Position (deg)", - () -> Units.rotationsToDegrees(this.measurement.shoulderRotations().position)); - measurement.addDouble( - "Shoulder Velocity (dps)", - () -> Units.rotationsToDegrees(this.measurement.shoulderRotations().velocity)); - - measurement.addDouble( - "Roller Velocity (rps)", () -> this.measurement.rollerVelocityRotationsPerSecond()); - - measurement.addDouble( - "Flywheel Velocity (rps)", () -> this.measurement.flywheelVelocityRotationsPerSecond()); - - measurement.addDouble( - "Serializer Velocity (rps)", () -> this.measurement.serializerVelocityRotationsPerSecond()); + addStateToShuffleboard(tab, "Measurement", () -> measurement); + addStateToShuffleboard(tab, "Goal", () -> goal); + } - ShuffleboardLayout goal = Telemetry.addColumn(tab, "Goal"); + private void addStateToShuffleboard(ShuffleboardTab tab, String name, Supplier state) { + ShuffleboardLayout layout = Telemetry.addColumn(tab, name); - goal.addDouble( + layout.addDouble( "Shoulder Position (deg)", - () -> Units.rotationsToDegrees(this.goal.shoulderRotations().position)); - goal.addDouble( + () -> Units.rotationsToDegrees(state.get().armState().shoulderRotations().position)); + layout.addDouble( "Shoulder Velocity (dps)", - () -> Units.rotationsToDegrees(this.goal.shoulderRotations().velocity)); + () -> Units.rotationsToDegrees(state.get().armState().shoulderRotations().velocity)); + + layout.addDouble( + "Front Roller Velocity (rps)", () -> state.get().intakeState().frontRollerVelocityRotationsPerSecond()); - goal.addDouble("Roller Velocity (rps)", () -> this.goal.rollerVelocityRotationsPerSecond()); + layout.addDouble( + "Back Roller Velocity (rps)", () -> state.get().intakeState().backRollerVelocityRotationsPerSecond()); - goal.addDouble("Flywheel Velocity (rps)", () -> this.goal.flywheelVelocityRotationsPerSecond()); + layout.addDouble( + "Flywheel Velocity (rps)", () -> state.get().shooterState().flywheelVelocityRotationsPerSecond()); - goal.addDouble( - "Serializer Velocity (rps)", () -> this.goal.serializerVelocityRotationsPerSecond()); + layout.addDouble( + "Serializer Velocity (rps)", () -> state.get().shooterState().serializerVelocityRotationsPerSecond()); } private void updateMeasurement() { - State measuredShoulderState = arm.getState(); + ArmState measuredShoulderState = arm.getState(); - double measuredIntakeRollerVelocity = intake.getRollerVelocity(); + IntakeState measuredIntakeState = intake.getState(); - double measuredShooterFlywheelVelocity = shooter.getFlywheelVelocity(); - double measuredShooterSerializerVelocity = shooter.getSerializerVelocity(); + ShooterState measuredShooterState = shooter.getState(); measurement = new SuperstructureState( measuredShoulderState, - measuredIntakeRollerVelocity, - measuredShooterFlywheelVelocity, - measuredShooterSerializerVelocity); + measuredIntakeState, + measuredShooterState); SuperstructureMechanism.getInstance().updateSuperstructure(measurement); } @@ -120,19 +114,15 @@ public SuperstructureState getState() { } public void setPosition(SuperstructureState state) { - arm.setPosition(state.shoulderRotations().position); + arm.setPosition(state.armState()); } public void setGoal(SuperstructureState goal) { this.goal = goal; - arm.setGoal(goal.shoulderRotations()); - - shooter.setGoal( - goal.flywheelVelocityRotationsPerSecond(), - goal.serializerVelocityRotationsPerSecond()); - - intake.setGoal(goal.rollerVelocityRotationsPerSecond()); + arm.setGoal(goal.armState()); + intake.setGoal(goal.intakeState()); + shooter.setGoal(goal.shooterState()); } public boolean at(SuperstructureState goal) { @@ -142,7 +132,7 @@ public boolean at(SuperstructureState goal) { } private Command to(SuperstructureState goal) { - return run(() -> setGoal(goal)).until(() -> at(goal)).raceWith(Commands.waitSeconds(1.0)); + return new ToGoal(goal); } public Command stow() { diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index d8a9506..c382e1c 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -1,38 +1,3 @@ package frc.robot.superstructure; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import frc.lib.MotionProfileCalculator; - -public class SuperstructureConstants { - - public static class ShoulderAngleConstants { - public static final Rotation2d INITIAL = Rotation2d.fromDegrees(-26.45); - - public static final Rotation2d STOW = Rotation2d.fromDegrees(-26.45); - - public static final Rotation2d SHOOT = Rotation2d.fromDegrees(-15); - - public static final Rotation2d EJECT = Rotation2d.fromDegrees(0); - - public static final Rotation2d AMP = Rotation2d.fromDegrees(90); - - /** Tolerance of the shoulder joint. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - - /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0); - - /** Maximum acceleration of the shoulder joint in rotations per second per second. */ - public static final double MAXIMUM_ACCELERATION = - MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); - - /** Maximum speed and acceleration of the shoulder joint. */ - public static final TrapezoidProfile.Constraints CONSTRAINTS = - new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); - - /** Motion profile of the shoulder joint using constraints. */ - public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); - } -} +public class SuperstructureConstants {} diff --git a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java index 910ad5b..70ee0e9 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java @@ -7,23 +7,19 @@ public class SuperstructureGoals { private final Queue goals; - public static Queue generate( + public static SuperstructureGoals generate( SuperstructureState start, SuperstructureState end) { Queue goals = new LinkedList(); goals.add(end); - return goals; + return new SuperstructureGoals(goals); } public SuperstructureGoals(Queue goals) { this.goals = goals; } - public SuperstructureGoals(SuperstructureState start, SuperstructureState end) { - this(generate(start, end)); - } - public SuperstructureState get() { return goals.element(); } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java index 4a56e1c..b9b9c5d 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java @@ -132,7 +132,7 @@ public Mechanism2d getMechanism() { } public void updateSuperstructure(SuperstructureState state) { - Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderRotations().position); + Rotation2d shoulderRotation = Rotation2d.fromRotations(state.armState().shoulderRotations().position); Rotation2d offsetShoulderRotation = shoulderRotation.minus(Rotation2d.fromDegrees(90)); @@ -141,21 +141,23 @@ public void updateSuperstructure(SuperstructureState state) { flywheel.setColor( new Color8Bit( FLYWHEEL_COLOR.sample( - Math.abs(state.flywheelVelocityRotationsPerSecond()), + Math.abs(state.shooterState().flywheelVelocityRotationsPerSecond()), 0, FlywheelConstants.MAXIMUM_SPEED))); serializer.setColor( new Color8Bit( SERIALIZER_COLOR.sample( - Math.abs(state.serializerVelocityRotationsPerSecond()), + Math.abs(state.shooterState().serializerVelocityRotationsPerSecond()), 0, SerializerConstants.MAXIMUM_SPEED))); + double averageRollerVelocity = (state.intakeState().frontRollerVelocityRotationsPerSecond() + state.intakeState().backRollerVelocityRotationsPerSecond()) / 2; + rollers.setColor( new Color8Bit( ROLLERS_COLOR.sample( - Math.abs(state.rollerVelocityRotationsPerSecond()), + Math.abs(averageRollerVelocity), 0, RollerConstants.MAXIMUM_SPEED))); } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 79688b6..c8b6985 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -1,73 +1,74 @@ package frc.robot.superstructure; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import frc.robot.arm.Arm; +import frc.robot.arm.ArmState; +import frc.robot.arm.ArmConstants.ShoulderConstants; import frc.robot.intake.Intake; +import frc.robot.intake.IntakeState; import frc.robot.intake.IntakeConstants.RollerConstants; import frc.robot.shooter.Shooter; +import frc.robot.shooter.ShooterState; import frc.robot.shooter.ShooterConstants.FlywheelConstants; import frc.robot.shooter.ShooterConstants.SerializerConstants; -import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; import java.util.Objects; /** Represents the state of the superstructure. */ public record SuperstructureState( - State shoulderRotations, - double rollerVelocityRotationsPerSecond, - double flywheelVelocityRotationsPerSecond, - double serializerVelocityRotationsPerSecond) { + ArmState armState, + IntakeState intakeState, + ShooterState shooterState) { public static final SuperstructureState INITIAL = - new SuperstructureState(ShoulderAngleConstants.INITIAL, 0, 0, 0); + new SuperstructureState(ShoulderConstants.INITIAL_ANGLE, 0, 0, 0); public static final SuperstructureState STOW = - new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, 0); + new SuperstructureState(ShoulderConstants.STOW_ANGLE, 0, 0, 0); public static final SuperstructureState INTAKE_POSITION = - new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, 0); + new SuperstructureState(ShoulderConstants.STOW_ANGLE, 0, 0, 0); public static final SuperstructureState INTAKE = new SuperstructureState( - ShoulderAngleConstants.STOW, + ShoulderConstants.STOW_ANGLE, RollerConstants.INTAKE_VELOCITY, 0, SerializerConstants.INTAKE_VELOCITY); - public static final SuperstructureState PULL = new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, SerializerConstants.PULL_VELOCITY); + public static final SuperstructureState PULL = new SuperstructureState(ShoulderConstants.STOW_ANGLE, 0, 0, SerializerConstants.PULL_VELOCITY); - public static final SuperstructureState EJECT_POSITION = new SuperstructureState(ShoulderAngleConstants.EJECT, 0, 0, 0); + public static final SuperstructureState EJECT_POSITION = new SuperstructureState(ShoulderConstants.EJECT_ANGLE, 0, 0, 0); - public static final SuperstructureState EJECT = new SuperstructureState(ShoulderAngleConstants.EJECT, 0, 0, SerializerConstants.PULL_VELOCITY); + public static final SuperstructureState EJECT = new SuperstructureState(ShoulderConstants.EJECT_ANGLE, 0, 0, SerializerConstants.PULL_VELOCITY); public static final SuperstructureState SPEAKER_SHOOT = new SuperstructureState( - ShoulderAngleConstants.SHOOT, + ShoulderConstants.SHOOT_ANGLE, 0, FlywheelConstants.SPEAKER_VELOCITY, SerializerConstants.SERIALIZE_VELOCITY); public static final SuperstructureState PASS_SPIN = new SuperstructureState( - ShoulderAngleConstants.STOW, 0, FlywheelConstants.PASS_VELOCITY, 0); + ShoulderConstants.STOW_ANGLE, 0, FlywheelConstants.PASS_VELOCITY, 0); public static final SuperstructureState PASS_SHOOT = new SuperstructureState( - ShoulderAngleConstants.STOW, + ShoulderConstants.STOW_ANGLE, 0, FlywheelConstants.PASS_VELOCITY, SerializerConstants.SERIALIZE_VELOCITY); public static final SuperstructureState AMP_POSITION = - new SuperstructureState(ShoulderAngleConstants.AMP, 0, 0, 0); + new SuperstructureState(ShoulderConstants.AMP_ANGLE, 0, 0, 0); public static final SuperstructureState AMP_SPIN = new SuperstructureState( - ShoulderAngleConstants.AMP, 0, FlywheelConstants.AMP_VELOCITY, 0); + ShoulderConstants.AMP_ANGLE, 0, FlywheelConstants.AMP_VELOCITY, 0); public static final SuperstructureState AMP_SHOOT = new SuperstructureState( - ShoulderAngleConstants.AMP, + ShoulderConstants.AMP_ANGLE, 0, FlywheelConstants.AMP_VELOCITY, SerializerConstants.SERIALIZE_VELOCITY); @@ -75,16 +76,14 @@ public record SuperstructureState( /** * Creates a new superstructure state. * - * @param shoulderRotations - * @param rollerVelocityRotationsPerSecond - * @param flywheelVelocityRotationsPerSecond - * @param serializerVelocityRotationsPerSecond + * @param armState + * @param intakeState + * @param shooterState */ public SuperstructureState { - Objects.requireNonNull(shoulderRotations); - Objects.requireNonNull(rollerVelocityRotationsPerSecond); - Objects.requireNonNull(flywheelVelocityRotationsPerSecond); - Objects.requireNonNull(serializerVelocityRotationsPerSecond); + Objects.requireNonNull(armState); + Objects.requireNonNull(intakeState); + Objects.requireNonNull(shooterState); } /** @@ -101,10 +100,9 @@ public SuperstructureState( double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { this( - new State(shoulderAngle.getRotations(), 0), - rollerVelocityRotationsPerSecond, - flywheelVelocityRotationsPerSecond, - serializerVelocityRotationsPerSecond); + new ArmState(shoulderAngle), + new IntakeState(rollerVelocityRotationsPerSecond, rollerVelocityRotationsPerSecond), + new ShooterState(flywheelVelocityRotationsPerSecond, serializerVelocityRotationsPerSecond)); } /** diff --git a/src/main/java/frc/robot/superstructure/ToGoal.java b/src/main/java/frc/robot/superstructure/ToGoal.java new file mode 100644 index 0000000..db3b441 --- /dev/null +++ b/src/main/java/frc/robot/superstructure/ToGoal.java @@ -0,0 +1,37 @@ +package frc.robot.superstructure; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ToGoal extends Command { + + private final Superstructure superstructure; + + private final SuperstructureState goal; + + private SuperstructureGoals goals; + + public ToGoal(SuperstructureState goal) { + superstructure = Superstructure.getInstance(); + + this.goal = goal; + + goals = SuperstructureGoals.generate(superstructure.getState(), goal); + + addRequirements(superstructure); + } + + @Override + public void initialize() { + goals = SuperstructureGoals.generate(superstructure.getState(), goal); + } + + @Override + public void execute() { + if (superstructure.at(goals.get())) { + superstructure.setGoal(goals.next()); + } else { + superstructure.setGoal(goals.get()); + } + } + +}