From 7ed5dc6f4a36b9ab9f930c34b88e30099a2e80c2 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 18 Mar 2024 12:38:04 -0400 Subject: [PATCH 1/8] refactor: Begin superstructure subsystem. --- src/main/java/frc/robot/RobotContainer.java | 16 +- src/main/java/frc/robot/RobotMechanisms.java | 70 +++--- src/main/java/frc/robot/arm/Arm.java | 205 +----------------- src/main/java/frc/robot/arm/ArmConstants.java | 54 ----- src/main/java/frc/robot/arm/ArmState.java | 155 ------------- .../frc/robot/arm/MoveShoulderCommand.java | 51 ----- .../java/frc/robot/arm/MoveWristCommand.java | 51 ----- src/main/java/frc/robot/auto/Auto.java | 63 ------ src/main/java/frc/robot/intake/Intake.java | 164 +------------- .../frc/robot/intake/IntakeConstants.java | 25 --- src/main/java/frc/robot/shooter/Shooter.java | 17 +- .../robot/superstructure/Superstructure.java | 108 +++++++++ .../SuperstructureConstants.java | 83 +++++++ .../superstructure/SuperstructureState.java | 112 ++++++++++ 14 files changed, 366 insertions(+), 808 deletions(-) delete mode 100644 src/main/java/frc/robot/arm/ArmState.java delete mode 100644 src/main/java/frc/robot/arm/MoveShoulderCommand.java delete mode 100644 src/main/java/frc/robot/arm/MoveWristCommand.java create mode 100644 src/main/java/frc/robot/superstructure/Superstructure.java create mode 100644 src/main/java/frc/robot/superstructure/SuperstructureConstants.java create mode 100644 src/main/java/frc/robot/superstructure/SuperstructureState.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b16180..5e57e79 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,7 @@ public class RobotContainer { private final Shooter shooter; private final Swerve swerve; - private final CommandXboxController driverController, operatorController; + private final CommandXboxController driverController; /** Creates a new instance of the robot container. */ private RobotContainer() { @@ -39,7 +39,6 @@ private RobotContainer() { swerve = Swerve.getInstance(); driverController = new CommandXboxController(0); - operatorController = new CommandXboxController(1); initializeTelemetry(); configureDefaultCommands(); @@ -67,25 +66,12 @@ private void initializeTelemetry() { /** Configures subsystem default commands. */ private void configureDefaultCommands() { - arm.setDefaultCommand(arm.stow()); - intake.setDefaultCommand(intake.stow()); swerve.setDefaultCommand(swerve.driveWithController(driverController)); } /** Configures controller bindings. */ private void configureBindings() { - driverController.a().whileTrue(swerve.forwards()); - driverController.b().whileTrue(swerve.sideways()); - driverController.x().whileTrue(swerve.cross()); driverController.y().onTrue(odometry.tare()); - - operatorController.leftTrigger().whileTrue(auto.intakePosition().andThen(auto.intakeNote())); - operatorController.leftBumper().whileTrue(intake.unstow().andThen(intake.outtake())); - - operatorController.rightTrigger().whileTrue(auto.shootPosition().andThen(shooter.spin())); - operatorController.rightBumper().whileTrue(shooter.serialize()); - - operatorController.a().whileTrue(arm.amp()); } /** diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index 2307fc7..f19b6ae 100644 --- a/src/main/java/frc/robot/RobotMechanisms.java +++ b/src/main/java/frc/robot/RobotMechanisms.java @@ -10,11 +10,11 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import frc.lib.InterpolatableColor; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; -import frc.robot.arm.ArmState; import frc.robot.intake.IntakeConstants.PivotMotorConstants; import frc.robot.intake.IntakeConstants.RollerMotorConstants; import frc.robot.shooter.ShooterConstants.FlywheelConstants; import frc.robot.shooter.ShooterConstants.SerializerConstants; +import frc.robot.superstructure.SuperstructureState; /** Helper class for rendering robot mechanisms. */ public class RobotMechanisms { @@ -23,7 +23,7 @@ public class RobotMechanisms { private final Mechanism2d mechanism; - private MechanismLigament2d shoulder, shooter, serializer, intake; + private MechanismLigament2d shoulder, flywheel, serializer, pivot; private final double WIDTH = 2 @@ -34,9 +34,12 @@ public class RobotMechanisms { private final Translation2d ORIGIN = new Translation2d(WIDTH / 2, 0); + public static final Translation2d SHOULDER_TO_ORIGIN = + new Translation2d(Units.inchesToMeters(-11.361), Units.inchesToMeters(7.721)); + private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray); - private final InterpolatableColor shooterColor = + private final InterpolatableColor flywheelColor = new InterpolatableColor(Color.kLightGray, Color.kSalmon); private final InterpolatableColor serializerColor = new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue); @@ -47,12 +50,13 @@ private RobotMechanisms() { mechanism = new Mechanism2d(WIDTH, HEIGHT); initializeArmMechanism(); - initializeFramePerimeterMechanisms(); initializeIntakeMechanism(); + + initializeFramePerimeterMechanisms(); } private void initializeArmMechanism() { - Translation2d armRootTranslation = ORIGIN.plus(ShoulderMotorConstants.SHOULDER_TO_ORIGIN); + Translation2d armRootTranslation = ORIGIN.plus(SHOULDER_TO_ORIGIN); double armThickness = Units.inchesToMeters(2) * 100; @@ -67,7 +71,7 @@ private void initializeArmMechanism() { 0, armThickness, DEFAULT_COLOR)); - shooter = + flywheel = shoulder.append( new MechanismLigament2d( "wrist", Units.inchesToMeters(4.321), 0, armThickness, DEFAULT_COLOR)); @@ -103,7 +107,7 @@ private void initializeFramePerimeterMechanisms() { private void initializeIntakeMechanism() { double intakeThickness = Units.inchesToMeters(3) * 100; - intake = + pivot = mechanism .getRoot( "intake", @@ -126,9 +130,9 @@ public Mechanism2d getMechanism() { return mechanism; } - public void updateArm(ArmState state) { - Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position); - Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position); + public void updateSuperstructure(SuperstructureState state) { + Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position); + Rotation2d wristRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position); shoulder.setAngle(shoulderRotation); @@ -138,32 +142,28 @@ public void updateArm(ArmState state) { Rotation2d shooterRotation = offsetWristRotation; Rotation2d serializerRotation = offsetWristRotation.plus(Rotation2d.fromDegrees(180)); - shooter.setAngle(shooterRotation); - serializer.setAngle(serializerRotation); - } - - public void updateIntake(Rotation2d pivotAngle, double rollerVelocityRotationsPerSecond) { - Color color = - intakeColor.sample( - Math.abs(rollerVelocityRotationsPerSecond), 0, RollerMotorConstants.MAXIMUM_SPEED); - - intake.setAngle(pivotAngle); - intake.setColor(new Color8Bit(color)); - } - - public void updateShooter(double velocityMetersPerSecond) { - Color color = - shooterColor.sample( - Math.abs(velocityMetersPerSecond), 0, FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED); - - shooter.setColor(new Color8Bit(color)); - } + pivot.setAngle(Rotation2d.fromRotations(state.pivotAngleRotations().position)); + pivot.setColor( + new Color8Bit( + intakeColor.sample( + Math.abs(state.rollerVelocityRotationsPerSecond()), + 0, + RollerMotorConstants.MAXIMUM_SPEED))); - public void updateSerializer(double velocityMetersPerSecond) { - Color color = - serializerColor.sample( - Math.abs(velocityMetersPerSecond), 0, SerializerConstants.MAXIMUM_TANGENTIAL_SPEED); + flywheel.setAngle(shooterRotation); + flywheel.setColor( + new Color8Bit( + flywheelColor.sample( + Math.abs(state.flywheelVelocityRotationsPerSecond()), + 0, + FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED))); - serializer.setColor(new Color8Bit(color)); + serializer.setAngle(serializerRotation); + serializer.setColor( + new Color8Bit( + serializerColor.sample( + Math.abs(state.serializerVelocityRotationsPerSecond()), + 0, + SerializerConstants.MAXIMUM_TANGENTIAL_SPEED))); } } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 98f97cf..2324b73 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -1,14 +1,12 @@ package frc.robot.arm; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.TrapezoidProfileTelemetry; -import frc.robot.RobotMechanisms; import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues; import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues; import frc.robot.arm.WristMotorIO.WristMotorIOValues; @@ -19,8 +17,6 @@ public class Arm extends Subsystem { /** Instance variable for the arm subsystem singleton. */ private static Arm instance = null; - private final RobotMechanisms mechanism; - /** Limit switch. */ private final LimitSwitchIO limitSwitch; @@ -39,16 +35,8 @@ public class Arm extends Subsystem { /** Wrist motor values. */ private final WristMotorIOValues wristMotorValues = new WristMotorIOValues(); - /** Arm goal and setpoint states. */ - private ArmState goal, setpoint; - - /** Telemetry for the shoulder and wrist trapezoid profiles. */ - private final TrapezoidProfileTelemetry shoulderProfileTelemetry, wristProfileTelemetry; - /** Creates a new instance of the arm subsystem. */ private Arm() { - mechanism = RobotMechanisms.getInstance(); - limitSwitch = ArmFactory.createLimitSwitch(); shoulderMotor = ArmFactory.createShoulderMotor(); wristMotor = ArmFactory.createWristMotor(); @@ -57,11 +45,7 @@ private Arm() { shoulderMotor.configure(); wristMotor.configure(); - setPosition(ArmState.INIT); - clearProfile(); - - shoulderProfileTelemetry = new TrapezoidProfileTelemetry("arm/shoulderProfile"); - wristProfileTelemetry = new TrapezoidProfileTelemetry("arm/wristProfile"); + // setPosition(ArmState.INIT); } /** @@ -82,13 +66,6 @@ public void periodic() { limitSwitch.update(limitSwitchValues); shoulderMotor.update(shoulderMotorValues); wristMotor.update(wristMotorValues); - - mechanism.updateArm(getMeasuredState()); - - shoulderProfileTelemetry.update( - getMeasuredState().shoulder(), getSetpoint().shoulder(), getGoal().shoulder()); - wristProfileTelemetry.update( - getMeasuredState().wrist(), getSetpoint().wrist(), getGoal().wrist()); } @Override @@ -105,21 +82,6 @@ public void addToShuffleboard(ShuffleboardTab tab) { position.addDouble( "Wrist Position (deg)", () -> Units.rotationsToDegrees(wristMotorValues.positionRotations)); - ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint"); - - setpoint.addDouble( - "Shoulder Setpoint (deg)", - () -> Units.rotationsToDegrees(getSetpoint().shoulder().position)); - setpoint.addDouble( - "Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().wrist().position)); - - ShuffleboardLayout goal = Telemetry.addColumn(tab, "Goal"); - - goal.addDouble( - "Shoulder Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().shoulder().position)); - goal.addDouble( - "Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().wrist().position)); - ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages"); voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.inputVoltage); @@ -139,164 +101,13 @@ public void addToShuffleboard(ShuffleboardTab tab) { () -> wristMotorValues.accelerationRotationsPerSecondPerSecond); } - /** - * Sets the position of the arm to the supplied state. - * - * @param state the state containing the position of the arm. - */ - public void setPosition(ArmState state) { - shoulderMotor.setPosition(state.shoulder().position); - wristMotor.setPosition(state.wrist().position); + public State getMeasuredShoulderState() { + return new TrapezoidProfile.State( + shoulderMotorValues.positionRotations, shoulderMotorValues.velocityRotationsPerSecond); } - /** - * Resets the goal and setpoints of the arm to the arm's current position. Commands the arm to - * hold its current position. - */ - public void clearProfile() { - ArmState position = getMeasuredState().position(); - - goal = position; - setpoint = position; - } - - /** - * Returns the arm's measured state. - * - * @return the arm's measured state. - */ - public ArmState getMeasuredState() { - shoulderMotor.update(shoulderMotorValues); - wristMotor.update(wristMotorValues); - - TrapezoidProfile.State measuredShoulderState = - new TrapezoidProfile.State( - shoulderMotorValues.positionRotations, shoulderMotorValues.velocityRotationsPerSecond); - TrapezoidProfile.State measuredWristState = - new TrapezoidProfile.State( - wristMotorValues.positionRotations, wristMotorValues.velocityRotationsPerSecond); - - return new ArmState(measuredShoulderState, measuredWristState); - } - - /** - * Returns true if the arm is at a given state. - * - * @param state the state to compare to. - * @return true if the arm is at a given state. - */ - public boolean at(ArmState state) { - return getMeasuredState().at(state); - } - - /** - * Returns the arm's goal. - * - * @return the arm's goal. - */ - public ArmState getGoal() { - return this.goal; - } - - /** - * Sets the arm's goal. - * - *

Calling this method does not alter the arm's motion; it simply updates a value used for - * telemetry. - * - * @param goal the arm's goal. - */ - public void setGoal(ArmState goal) { - this.goal = goal; - } - - /** - * Returns the arm's setpoint. - * - * @return the arm's setpoint. - */ - public ArmState getSetpoint() { - return this.setpoint; - } - - /** - * Sets the arm's setpoint. - * - *

Calling this method does not alter the arm's motion; it simply updates a value used for - * telemetry. - * - * @param setpoint the arm's setpoint. - */ - public void setSetpoint(ArmState setpoint) { - this.setpoint = setpoint; - } - - /** - * Applies a setpoint to the arm's controllers. - * - *

Calling this method alters the arm's motion. - * - * @param setpoint the arm's setpoint. - */ - public void applySetpoint(ArmState setpoint) { - shoulderMotor.setSetpoint(setpoint.shoulder().position, setpoint.shoulder().velocity); - wristMotor.setSetpoint(setpoint.wrist().position, setpoint.wrist().velocity); - } - - /** - * Returns a command that moves the shoulder to a goal's shoulder position. - * - * @param goal the goal position to move to. - * @return a command that moves the shoulder to a goal's shoulder position. - */ - private Command shoulderTo(ArmState goal) { - return new MoveShoulderCommand(goal); - } - - /** - * Returns a command that moves the wrist to a goal's wrist position. - * - * @param goal the goal position to move to. - * @return a command that moves the wrist to a goal's wrist position. - */ - public Command wristTo(ArmState goal) { - return new MoveWristCommand(goal); - } - - /** - * Returns a command that moves the arm to the amp position. - * - * @return a comamnd that moves the arm to the amp position. - */ - public Command amp() { - return shoulderTo(ArmState.AMP).andThen(wristTo(ArmState.AMP)); - } - - /** - * Returns a command that moves the arm to the stow position. - * - * @return a command that moves the arm to the stow position. - */ - public Command stow() { - return wristTo(ArmState.STOW).andThen(shoulderTo(ArmState.STOW)); - } - - /** - * Returns a command that moves the arm to the stow position and resets the position of the arm. - * - *

When the limit switch detects that the arm is in the stow position, the arm position is - * reset to be equal to the stow position. - * - * @return a command that moves the arm to the stow position and resets the position of the arm. - */ - public Command home() { - return wristTo(ArmState.STOW) - .andThen(shoulderTo(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) - .finallyDo( - interrupted -> { - if (!interrupted) { - setPosition(ArmState.STOW); - } - }); + public State getMeasuredWristState() { + return new TrapezoidProfile.State( + wristMotorValues.positionRotations, wristMotorValues.velocityRotationsPerSecond); } } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index a110c58..58c0b41 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -1,12 +1,8 @@ package frc.robot.arm; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.lib.JointConstants; -import frc.lib.MotionProfileCalculator; /** Constants for the arm subsystem. */ public class ArmConstants { @@ -23,33 +19,6 @@ public static class ShoulderMotorConstants { 51.2, DCMotor.getNEO(1), // motor 1); - - /** Minimum angle of the shoulder joint. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(29.5); - - /** Maximum angle of the shoulder joint. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90); - - /** Tolerance of the shoulder joint. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); - - /** Translation of the shoulder joint relative to the origin in meters. */ - public static final Translation2d SHOULDER_TO_ORIGIN = - new Translation2d(Units.inchesToMeters(-11.361), Units.inchesToMeters(7.721)); - - /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 0.5; - - /** 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); } /** Constants for the wrist motor. */ @@ -64,28 +33,5 @@ public static class WristMotorConstants { 20.454545, DCMotor.getNEO(1), // motor 1); - - /** Minimum angle of the wrist joint. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-86.759); - - /** Maximum angle of the wrist joint. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(85.98); - - /** Tolerance of the wrist joint. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); - - /** Maximum speed of the wrist joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 1.2; - - /** Maximum acceleration of the wrist joint in rotations per second per second. */ - public static final double MAXIMUM_ACCELERATION = - MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25); - - /** Maximum speed and acceleration of the wrist joint. */ - public static final TrapezoidProfile.Constraints CONSTRAINTS = - new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); - - /** Motion profile of the wrist joint using constraints. */ - 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 deleted file mode 100644 index 7fbd119..0000000 --- a/src/main/java/frc/robot/arm/ArmState.java +++ /dev/null @@ -1,155 +0,0 @@ -package frc.robot.arm; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import frc.robot.RobotConstants; -import frc.robot.arm.ArmConstants.ShoulderMotorConstants; -import frc.robot.arm.ArmConstants.WristMotorConstants; -import java.util.Objects; - -/** State of the arm. */ -public record ArmState(State shoulder, State wrist) { - - public static final ArmState INIT = - new ArmState(Rotation2d.fromDegrees(52.5), Rotation2d.fromDegrees(-35)); - - public static final ArmState STOW = - new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE); - - public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(18)); - - public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(4)); - - public static final ArmState AMP = - new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, Rotation2d.fromDegrees(0)); - - /** - * Creates an arm state. - * - * @param shoulder the shoulder's state. - * @param wrist the wrist's state. - */ - public ArmState { - Objects.requireNonNull(shoulder); - Objects.requireNonNull(wrist); - } - - /** - * Creates an arm state. - * - * @param shoulder the shoulder's rotation. - * @param wrist the wrist's rotation. - */ - public ArmState(Rotation2d shoulder, Rotation2d wrist) { - this(new State(shoulder.getRotations(), 0.0), new State(wrist.getRotations(), 0.0)); - } - - /** - * Creates an arm state representing the stationary position of the arm. - * - * @return an arm state representing the stationary position of the arm. - */ - public ArmState position() { - return new ArmState( - Rotation2d.fromRotations(shoulder.position), Rotation2d.fromRotations(wrist.position)); - } - - /** - * Copies this arm state with a new shoulder rotation. - * - * @param newShoulder the new shoulder rotation. - * @return a copy of this arm state with a new shoulder rotation. - */ - public ArmState withShoulder(Rotation2d newShoulder) { - return withShoulder(new State(newShoulder.getRotations(), 0.0)); - } - - /** - * Copies this arm state with a new shoulder state. - * - * @param newShoulder the new shoulder state. - * @return a copy of this arm state with a new shoulder state. - */ - public ArmState withShoulder(State newShoulder) { - return new ArmState(newShoulder, wrist); - } - - /** - * Copies this arm state with a new wrist rotation. - * - * @param newWrist the new wrist rotation. - * @return a copy of this arm state with a new wrist rotation. - */ - public ArmState withWrist(Rotation2d newWrist) { - return withWrist(new State(newWrist.getRotations(), 0.0)); - } - - /** - * Copies this arm state with a new wrist state. - * - * @param newWrist the new wrist state. - * @return a copy of this arm state with a new wrist state. - */ - public ArmState withWrist(State newWrist) { - return new ArmState(shoulder, newWrist); - } - - /** - * Returns true if the arm states are equal. - * - * @param other the other arm state. - * @return true if the arm states are equal. - */ - public boolean at(ArmState other) { - return atShoulderGoal(other) && atWristGoal(other); - } - - /** - * Returns true if the arm is at its shoulder goal. - * - * @param goal goal state of the arm. - * @return true if the arm is at its shoulder goal. - */ - public boolean atShoulderGoal(ArmState goal) { - return MathUtil.isNear( - this.shoulder().position, - goal.shoulder().position, - ShoulderMotorConstants.TOLERANCE.getRotations()); - } - - /** - * Returns the next setpoint involving shoulder-only movement. - * - * @param goal the arm's goal state. - * @return the next setpoint involving shoulder-only movement. - */ - public ArmState nextShoulderSetpoint(ArmState goal) { - return this.withShoulder( - ShoulderMotorConstants.MOTION_PROFILE.calculate( - RobotConstants.PERIODIC_DURATION, this.shoulder, goal.shoulder)); - } - - /** - * Returns true if the arm is at its wrist goal. - * - * @param goal goal state of the arm. - * @return true if the arm is at its wrist goal. - */ - public boolean atWristGoal(ArmState goal) { - return MathUtil.isNear( - this.wrist().position, goal.wrist().position, WristMotorConstants.TOLERANCE.getRotations()); - } - - /** - * Returns the next setpoint involving wrist-only movement. - * - * @param goal the arm's goal state. - * @return the next setpoint involving wrist-only movement. - */ - public ArmState nextWristSetpoint(ArmState goal) { - return this.withWrist( - WristMotorConstants.MOTION_PROFILE.calculate( - RobotConstants.PERIODIC_DURATION, this.wrist, goal.wrist)); - } -} diff --git a/src/main/java/frc/robot/arm/MoveShoulderCommand.java b/src/main/java/frc/robot/arm/MoveShoulderCommand.java deleted file mode 100644 index f5d1c20..0000000 --- a/src/main/java/frc/robot/arm/MoveShoulderCommand.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.arm; - -import edu.wpi.first.wpilibj2.command.Command; - -public class MoveShoulderCommand extends Command { - - private final Arm arm; - - private final ArmState shoulderGoal; - - private ArmState before; - - public MoveShoulderCommand(ArmState goal) { - this.arm = Arm.getInstance(); - - this.shoulderGoal = goal; - - addRequirements(arm); - } - - @Override - public void initialize() { - // Save the position of the arm before the shoulder starts moving - before = arm.getMeasuredState().position(); - - arm.setGoal(shoulderGoal.withWrist(before.wrist())); - arm.setSetpoint(before); - } - - @Override - public void execute() { - ArmState nextSetpoint = arm.getSetpoint().nextShoulderSetpoint(shoulderGoal); - - arm.setSetpoint(nextSetpoint); - arm.applySetpoint(nextSetpoint); - } - - @Override - public void end(boolean interrupted) { - ArmState after = arm.getMeasuredState().position(); - - // Since only the shoulder *should* have moved, assume that the wrist is in the same position as - // it was at the start of the command - arm.setPosition(after.withWrist(before.wrist())); - } - - @Override - public boolean isFinished() { - return arm.getMeasuredState().atShoulderGoal(shoulderGoal); - } -} diff --git a/src/main/java/frc/robot/arm/MoveWristCommand.java b/src/main/java/frc/robot/arm/MoveWristCommand.java deleted file mode 100644 index f47ee55..0000000 --- a/src/main/java/frc/robot/arm/MoveWristCommand.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.arm; - -import edu.wpi.first.wpilibj2.command.Command; - -public class MoveWristCommand extends Command { - - private final Arm arm; - - private final ArmState wristGoal; - - private ArmState before; - - public MoveWristCommand(ArmState goal) { - this.arm = Arm.getInstance(); - - this.wristGoal = goal; - - addRequirements(arm); - } - - @Override - public void initialize() { - // Save the position of the arm before the wrist starts moving - before = arm.getMeasuredState().position(); - - arm.setGoal(wristGoal.withShoulder(before.shoulder())); - arm.setSetpoint(before); - } - - @Override - public void execute() { - ArmState nextSetpoint = arm.getSetpoint().nextWristSetpoint(wristGoal); - - arm.setSetpoint(nextSetpoint); - arm.applySetpoint(nextSetpoint); - } - - @Override - public void end(boolean interrupted) { - ArmState after = arm.getMeasuredState().position(); - - // Since only the wrist *should* have moved, assume that the shoulder is in the same position as - // it was at the start of the command - arm.setPosition(after.withShoulder(before.shoulder())); - } - - @Override - public boolean isFinished() { - return arm.getMeasuredState().atWristGoal(wristGoal); - } -} diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index e831713..f215828 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -1,7 +1,6 @@ package frc.robot.auto; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; @@ -10,15 +9,10 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.AllianceFlipHelper; 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.odometry.Odometry; -import frc.robot.shooter.Shooter; import frc.robot.swerve.Swerve; import frc.robot.swerve.SwerveConstants; import java.util.function.Consumer; @@ -30,18 +24,9 @@ public class Auto extends Subsystem { /** Instance variable for the auto subsystem singleton. */ private static Auto instance = null; - /** Reference to the arm subsystem. */ - private final Arm arm; - - /** Reference to the intake subsystem. */ - private final Intake intake; - /** Reference to the odometry subsystem. */ private final Odometry odometry; - /** Reference to the shooter subssytem. */ - private final Shooter shooter; - /** Reference to the swerve subsystem. */ private final Swerve swerve; @@ -50,10 +35,7 @@ public class Auto extends Subsystem { /** Creates a new instance of the auto subsystem. */ private Auto() { - arm = Arm.getInstance(); - intake = Intake.getInstance(); odometry = Odometry.getInstance(); - shooter = Shooter.getInstance(); swerve = Swerve.getInstance(); Supplier robotPositionSupplier = () -> odometry.getPosition(); @@ -82,13 +64,6 @@ private Auto() { AllianceFlipHelper::shouldFlip, swerve); - NamedCommands.registerCommand("home", Arm.getInstance().home()); - NamedCommands.registerCommand("stow", stow()); - NamedCommands.registerCommand("readyIntake", intakePosition()); - NamedCommands.registerCommand("intakeNote", intakeNote()); - NamedCommands.registerCommand("readyShoot", shootPosition()); - NamedCommands.registerCommand("shootNote", shootNote()); - autoChooser = AutoBuilder.buildAutoChooser(); } @@ -130,42 +105,4 @@ public Command getAutonomousCommand() { public SendableChooser getAutonomousChooser() { return autoChooser; } - - public Command intakePosition() { - double seconds = 3.0; - - return Commands.parallel( - Commands.waitUntil(intake::isOut).andThen(arm.wristTo(ArmState.INTAKE)), - intake.unstow()) - .withTimeout(seconds); - } - - public Command intakeNote() { - return intakePosition().andThen(Commands.parallel(intake.intake(), shooter.intake())); - } - - public Command stow() { - double seconds = 2.0; - - return Commands.parallel( - arm.stow(), - Commands.waitUntil(() -> arm.at(ArmState.STOW)).withTimeout(2.0).andThen(intake.stow())) - .withTimeout(seconds); - } - - public Command shootPosition() { - double seconds = 3.0; - - return Commands.parallel( - Commands.waitUntil(intake::isOut).andThen(arm.wristTo(ArmState.SHOOT)), intake.unstow()) - .withTimeout(seconds); - } - - public Command shootNote() { - return shootPosition() - .andThen( - Commands.parallel( - shooter.spin(), shooter.serialize().beforeStarting(Commands.waitSeconds(1.0))) - .withTimeout(2.0)); - } } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 9bfac5f..1b73396 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -1,8 +1,6 @@ package frc.robot.intake; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; @@ -11,13 +9,10 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.TrapezoidProfileTelemetry; -import frc.robot.RobotConstants; -import frc.robot.RobotMechanisms; -import frc.robot.intake.IntakeConstants.PivotMotorConstants; import frc.robot.intake.IntakeConstants.RollerMotorConstants; import frc.robot.intake.PivotMotorIO.PivotMotorIOValues; import frc.robot.intake.RollerMotorIO.RollerMotorIOValues; +import frc.robot.superstructure.SuperstructureConstants; /** Subsystem class for the intake subsystem. */ public class Intake extends Subsystem { @@ -25,23 +20,12 @@ public class Intake extends Subsystem { /** Instance variable for the intake subsystem singleton. */ private static Intake instance = null; - private final RobotMechanisms mechanism; - /** Pivot motor. */ private final PivotMotorIO pivotMotor; /** Pivot motor values. */ private final PivotMotorIOValues pivotMotorValues = new PivotMotorIOValues(); - /** Pivot motor goal. */ - private TrapezoidProfile.State pivotGoal; - - /** Pivot motor setpoint. */ - private TrapezoidProfile.State pivotSetpoint; - - /** Telemetry for the pivot trapezoid profile. */ - private final TrapezoidProfileTelemetry pivotProfileTelemetry; - /** Roller motor. */ private final RollerMotorIO rollerMotor; @@ -50,23 +34,16 @@ public class Intake extends Subsystem { /** Creates a new instance of the intake subsystem. */ private Intake() { - mechanism = RobotMechanisms.getInstance(); - pivotMotor = IntakeFactory.createPivotMotor(); rollerMotor = IntakeFactory.createRollerMotor(); pivotMotor.configure(); rollerMotor.configure(); - Rotation2d initialAngle = PivotMotorConstants.MAXIMUM_ANGLE; + Rotation2d initialAngle = SuperstructureConstants.IntakePivotAngleConstants.MAXIMUM_ANGLE; pivotMotor.setPosition(initialAngle.getRotations()); pivotMotor.update(pivotMotorValues); - - pivotGoal = new TrapezoidProfile.State(initialAngle.getRotations(), 0); - pivotSetpoint = new TrapezoidProfile.State(initialAngle.getRotations(), 0); - - pivotProfileTelemetry = new TrapezoidProfileTelemetry("intake/pivotProfile"); } /** @@ -85,14 +62,7 @@ public static Intake getInstance() { @Override public void periodic() { pivotMotor.update(pivotMotorValues); - rollerMotor.update(rollerMotorValues); - - mechanism.updateIntake( - Rotation2d.fromRotations(pivotMotorValues.positionRotations), - rollerMotorValues.velocityRotationsPerSecond); - - pivotProfileTelemetry.update(getPivotMeasuredState(), getPivotSetpoint(), getPivotGoal()); } @Override @@ -100,10 +70,7 @@ public void addToShuffleboard(ShuffleboardTab tab) { ShuffleboardLayout pivot = Telemetry.addColumn(tab, "Pivot"); pivot.addDouble( - "Position (deg)", () -> Units.rotationsToDegrees(getPivotMeasuredState().position)); - pivot.addDouble("Setpoint (deg)", () -> Units.rotationsToDegrees(getPivotSetpoint().position)); - pivot.addDouble("Goal (deg)", () -> Units.rotationsToDegrees(getPivotGoal().position)); - pivot.addBoolean("Is Not Stowed?", this::isOut); + "Position (deg)", () -> Units.rotationsToDegrees(getMeasuredPivotState().position)); ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller"); @@ -116,134 +83,17 @@ public void addToShuffleboard(ShuffleboardTab tab) { * * @return the intake pivot's measured state. */ - public State getPivotMeasuredState() { + public State getMeasuredPivotState() { pivotMotor.update(pivotMotorValues); return new State( pivotMotorValues.positionRotations, pivotMotorValues.velocityRotationsPerSecond); } - /** - * Sets the intake pivot's goal. - * - * @param goal the intake pivot's goal. - */ - public void setPivotGoal(Rotation2d goal) { - setPivotGoal(new TrapezoidProfile.State(goal.getRotations(), 0.0)); - } - - /** - * Sets the intake pivot's goal. - * - * @param goal the intake pivot's goal. - */ - public void setPivotGoal(State goal) { - pivotGoal = goal; - } - - /** - * Returns the intake pivot's goal. - * - * @return the intake pivot's goal. - */ - public State getPivotGoal() { - return pivotGoal; - } - - /** - * Sets the intake pivot's setpoint. - * - * @param setpoint the intake pivot's setpoint. - */ - public void setPivotSetpoint(State setpoint) { - pivotSetpoint = setpoint; - } - - /** - * Returns the intake pivot's setpoint. - * - * @return the intake pivot's setpoint. - */ - public State getPivotSetpoint() { - return pivotSetpoint; - } - - /** - * Applies a pivot setpoint to the pivot motor's controller. - * - * @param setpoint the pivot's setpoint. - */ - public void applyPivotSetpoint(State setpoint) { - pivotMotor.setSetpoint(setpoint.position, setpoint.velocity); - } - - /** - * Returns true if the intake pivot is at its goal. - * - * @return true if the intake pivot is at its goal. - */ - private boolean atPivotGoal() { - return atPivotSetpoint() && pivotGoal.equals(pivotSetpoint); - } - - /** - * Returns true if the intake pivot is at its setpoint. - * - * @return true if the intake pivot is at its setpoint. - */ - private boolean atPivotSetpoint() { - pivotMotor.update(pivotMotorValues); - - return MathUtil.isNear( - pivotSetpoint.position, - pivotMotorValues.positionRotations, - PivotMotorConstants.TOLERANCE.getRotations()); - } - - /** - * Returns a command that pivots the intake to an angle. - * - * @param angle the angle to pivot the intake to. - * @return a command that pivots the intake to an angle. - */ - private Command pivotTo(Rotation2d angle) { - return run(() -> { - State nextPivotSetpoint = - PivotMotorConstants.MOTION_PROFILE.calculate( - RobotConstants.PERIODIC_DURATION, pivotSetpoint, pivotGoal); - - setPivotSetpoint(nextPivotSetpoint); - applyPivotSetpoint(nextPivotSetpoint); - }) - .beforeStarting(() -> setPivotGoal(angle)) - .until(this::atPivotGoal); - } - - /** - * Returns a command that pivots the intake to the unstowed position. - * - * @return a command that pivots the intake to the unstowed position. - */ - public Command unstow() { - return pivotTo(PivotMotorConstants.MINIMUM_ANGLE); - } - - /** - * Returns a command that pivots the intake to the stowed position. - * - * @return a command that pivots the intake to the stowed position. - */ - public Command stow() { - return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE); - } + public double getRollerVelocity() { + rollerMotor.update(rollerMotorValues); - /** - * Returns true if the intake is out (not stowed). - * - * @return true if the intake is out (not stowed). - */ - public boolean isOut() { - return pivotMotorValues.positionRotations < PivotMotorConstants.OUT_ANGLE.getRotations(); + return rollerMotorValues.velocityRotationsPerSecond; } /** diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index bb42afc..173bd7d 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -1,9 +1,7 @@ package frc.robot.intake; 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; /** Constants for the intake subsystem. */ public class IntakeConstants { @@ -13,34 +11,11 @@ public static class PivotMotorConstants { /** Distance between the pivot and the far edge of the intake. */ public static final double DISTANCE = Units.inchesToMeters(10.275); - /** Pivot motor's minimum angle. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-48); - - /** Pivot motor's maximum angle. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(86); - /** Angle between the measured intake position and the mass's position. */ public static final Rotation2d MASS_OFFSET = Rotation2d.fromDegrees(-34.34); /** Pivot motor's "out" angle. */ public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(0); - - /** Pivot motor's tolerance. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0); - - /** Maximum speed of the pivot in rotations per second. */ - public static final double MAXIMUM_SPEED = 1; - - /** Maximum acceleration of the pivot in rotations per second per second. */ - public static final double MAXIMUM_ACCELERATION = - MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); - - /** Maximum speed and acceleration of the pivot. */ - public static final TrapezoidProfile.Constraints CONSTRAINTS = - new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); - - /** Motion profile of the pivot using constraints. */ - public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); } /** Constants for the roller motor. */ diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 55f56b4..754d9fb 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.robot.RobotMechanisms; import frc.robot.shooter.FlywheelMotorIO.FlywheelMotorIOValues; import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues; import frc.robot.shooter.ShooterConstants.FlywheelConstants; @@ -53,10 +52,6 @@ public static Shooter getInstance() { public void periodic() { serializerMotor.update(serializerMotorValues); flywheelMotor.update(flywheelMotorValues); - - RobotMechanisms.getInstance().updateShooter(flywheelMotorValues.velocityRotationsPerSecond); - RobotMechanisms.getInstance() - .updateSerializer(serializerMotorValues.velocityRotationsPerSecond); } @Override @@ -74,6 +69,18 @@ public void addToShuffleboard(ShuffleboardTab tab) { flywheel.addDouble("Flywheel Current (A)", () -> flywheelMotorValues.currentAmps); } + public double getFlywheelVelocity() { + flywheelMotor.update(flywheelMotorValues); + + return flywheelMotorValues.velocityRotationsPerSecond; + } + + public double getSerializerVelocity() { + serializerMotor.update(serializerMotorValues); + + return serializerMotorValues.velocityRotationsPerSecond; + } + /** * Intakes a note. * diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java new file mode 100644 index 0000000..6586861 --- /dev/null +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -0,0 +1,108 @@ +package frc.robot.superstructure; + +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.lib.Subsystem; +import frc.lib.Telemetry; +import frc.robot.RobotMechanisms; +import frc.robot.arm.Arm; +import frc.robot.intake.Intake; +import frc.robot.shooter.Shooter; + +/** Subsystem class for the superstructure subsystem. */ +public class Superstructure extends Subsystem { + + /** Instance variable for the superstructure subsystem singleton. */ + private static Superstructure instance = null; + + /** Reference to the arm subsystem. */ + private final Arm arm; + + /** Reference to the intake subsystem. */ + private final Intake intake; + + /** Reference to the shooter subsystem. */ + private final Shooter shooter; + + private SuperstructureState measurement; + + /** Creates a new instance of the superstructure subsystem. */ + private Superstructure() { + arm = Arm.getInstance(); + intake = Intake.getInstance(); + shooter = Shooter.getInstance(); + } + + /** + * Gets the instance of the superstructure subsystem. + * + * @return the instance of the superstructre subsystem. + */ + public static Superstructure getInstance() { + if (instance == null) { + instance = new Superstructure(); + } + + return instance; + } + + @Override + public void periodic() { + State measuredShoulderState = arm.getMeasuredShoulderState(); + State measuredWristState = arm.getMeasuredWristState(); + + State measuredIntakePivotState = intake.getMeasuredPivotState(); + double measuredIntakeRollerVelocity = intake.getRollerVelocity(); + + double measuredShooterFlywheelVelocity = shooter.getFlywheelVelocity(); + double measuredShooterSerializerVelocity = shooter.getSerializerVelocity(); + + measurement = + new SuperstructureState( + measuredShoulderState, + measuredWristState, + measuredIntakePivotState, + measuredIntakeRollerVelocity, + measuredShooterFlywheelVelocity, + measuredShooterSerializerVelocity); + + RobotMechanisms.getInstance().updateSuperstructure(measurement); + } + + @Override + public void addToShuffleboard(ShuffleboardTab tab) { + ShuffleboardLayout measurement = Telemetry.addColumn(tab, "Measurement"); + + measurement.addDouble( + "Shoulder Position (deg)", + () -> Units.rotationsToDegrees(this.measurement.shoulderAngleRotations().position)); + measurement.addDouble( + "Shoulder Velocity (dps)", + () -> Units.rotationsToDegrees(this.measurement.shoulderAngleRotations().velocity)); + + measurement.addDouble( + "Wrist Position (deg)", + () -> Units.rotationsToDegrees(this.measurement.wristAngleRotations().position)); + measurement.addDouble( + "Wrist Velocity (dps)", + () -> Units.rotationsToDegrees(this.measurement.wristAngleRotations().velocity)); + + measurement.addDouble( + "Pivot Position (deg)", + () -> Units.rotationsToDegrees(this.measurement.pivotAngleRotations().position)); + measurement.addDouble( + "Pivot Velocity (dps)", + () -> Units.rotationsToDegrees(this.measurement.pivotAngleRotations().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()); + } +} diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java new file mode 100644 index 0000000..3dd9f0c --- /dev/null +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -0,0 +1,83 @@ +package frc.robot.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.lib.MotionProfileCalculator; + +public class SuperstructureConstants { + + public static class ShoulderAngleConstants { + /** Minimum angle of the shoulder joint. */ + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(29.5); + + /** Maximum angle of the shoulder joint. */ + public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90); + + /** Tolerance of the shoulder joint. */ + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); + + /** Maximum speed of the shoulder joint in rotations per second. */ + public static final double MAXIMUM_SPEED = 0.5; + + /** 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 static class WristAngleConstants { + /** Minimum angle of the wrist joint. */ + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-86.759); + + /** Maximum angle of the wrist joint. */ + public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(85.98); + + /** Tolerance of the wrist joint. */ + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); + + /** Maximum speed of the wrist joint in rotations per second. */ + public static final double MAXIMUM_SPEED = 1.2; + + /** Maximum acceleration of the wrist joint in rotations per second per second. */ + public static final double MAXIMUM_ACCELERATION = + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25); + + /** Maximum speed and acceleration of the wrist joint. */ + public static final TrapezoidProfile.Constraints CONSTRAINTS = + new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); + + /** Motion profile of the wrist joint using constraints. */ + public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); + } + + public static class IntakePivotAngleConstants { + /** Pivot motor's minimum angle. */ + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-48); + + /** Pivot motor's maximum angle. */ + public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(86); + + /** Pivot motor's tolerance. */ + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0); + + /** Maximum speed of the pivot in rotations per second. */ + public static final double MAXIMUM_SPEED = 1; + + /** Maximum acceleration of the pivot in rotations per second per second. */ + public static final double MAXIMUM_ACCELERATION = + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); + + /** Maximum speed and acceleration of the pivot. */ + public static final TrapezoidProfile.Constraints CONSTRAINTS = + new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); + + /** Motion profile of the pivot using constraints. */ + public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); + } +} diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java new file mode 100644 index 0000000..b784f82 --- /dev/null +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -0,0 +1,112 @@ +package frc.robot.superstructure; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import java.util.Objects; + +/** Represents the state of the superstructure. */ +public record SuperstructureState( + State shoulderAngleRotations, + State wristAngleRotations, + State pivotAngleRotations, + double rollerVelocityRotationsPerSecond, + double flywheelVelocityRotationsPerSecond, + double serializerVelocityRotationsPerSecond) { + + /** + * Creates a new superstructure state. + * + * @param shoulderAngleRotations + * @param wristAngleRotations + * @param intakePivotAngleRotations + * @param intakeRollerVelocityRotationsPerSecond + * @param flywheelVelocityRotationsPerSecond + * @param serializerVelocityRotationsPerSecond + */ + public SuperstructureState { + Objects.requireNonNull(shoulderAngleRotations); + Objects.requireNonNull(wristAngleRotations); + Objects.requireNonNull(pivotAngleRotations); + Objects.requireNonNull(rollerVelocityRotationsPerSecond); + Objects.requireNonNull(flywheelVelocityRotationsPerSecond); + Objects.requireNonNull(serializerVelocityRotationsPerSecond); + } + + /** + * Creates a new superstructure state. + * + * @param shoulderAngle + * @param wristAngle + * @param pivotAngle + * @param rollerVelocityRotationsPerSecond + * @param flywheelVelocityRotationsPerSecond + * @param serializerVelocityRotationsPerSecond + */ + public SuperstructureState( + Rotation2d shoulderAngle, + Rotation2d wristAngle, + Rotation2d pivotAngle, + double rollerVelocityRotationsPerSecond, + double flywheelVelocityRotationsPerSecond, + double serializerVelocityRotationsPerSecond) { + this( + new State(shoulderAngle.getRotations(), 0.0), + new State(wristAngle.getRotations(), 0.0), + new State(pivotAngle.getRotations(), 0.0), + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + /** + * Creates a new superstructure state. + * + * @param shoulderAngle + * @param wristAngle + * @param pivotAngle + */ + public SuperstructureState( + Rotation2d shoulderAngle, Rotation2d wristAngle, Rotation2d pivotAngle) { + this(shoulderAngle, wristAngle, pivotAngle, 0.0, 0.0, 0.0); + } + + /** + * Returns true if at the shoulder angle goal. + * + * @param goal + * @return true if at the shoulder angle goal. + */ + public boolean atShoulderAngleGoal(SuperstructureState goal) { + return MathUtil.isNear( + this.shoulderAngleRotations().position, + goal.shoulderAngleRotations().position, + SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations()); + } + + /** + * Returns true if at the wrist angle goal. + * + * @param goal + * @return true if at the wrist angle goal. + */ + public boolean atWristAngleGoal(SuperstructureState goal) { + return MathUtil.isNear( + this.wristAngleRotations().position, + goal.wristAngleRotations().position, + SuperstructureConstants.WristAngleConstants.TOLERANCE.getRotations()); + } + + /** + * Returns true if at the intake pivot angle goal. + * + * @param goal + * @return true if at the intake pivot angle goal. + */ + public boolean atPivotAngleGoal(SuperstructureState goal) { + return MathUtil.isNear( + this.pivotAngleRotations().position, + goal.pivotAngleRotations().position, + SuperstructureConstants.IntakePivotAngleConstants.TOLERANCE.getRotations()); + } +} From 3ef817d9e5f68fa7a6bc650176dc04518d8f20ae Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 18 Mar 2024 13:29:51 -0400 Subject: [PATCH 2/8] feat(superstructure): Implement basic goal following. --- simgui-ds.json | 2 +- simgui.json | 9 +- src/main/java/frc/robot/Robot.java | 2 - src/main/java/frc/robot/RobotContainer.java | 11 ++- src/main/java/frc/robot/RobotMechanisms.java | 2 +- src/main/java/frc/robot/arm/Arm.java | 16 ++++ src/main/java/frc/robot/intake/Intake.java | 15 ++-- .../robot/superstructure/Superstructure.java | 86 +++++++++++++++---- .../SuperstructureConstants.java | 20 +---- .../superstructure/SuperstructureState.java | 58 ++++++++++++- 10 files changed, 169 insertions(+), 52 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 2e2b48e..bc58bd9 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -102,7 +102,7 @@ "useGamepad": true }, { - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/simgui.json b/simgui.json index 4882da1..545666b 100644 --- a/simgui.json +++ b/simgui.json @@ -77,8 +77,7 @@ }, "Velocity": { "open": true - }, - "open": true + } }, "Shooter": { "Flywheel": { @@ -88,6 +87,12 @@ "open": true } }, + "Superstructure": { + "Measurement": { + "open": true + }, + "open": true + }, "Swerve": { "Constants": { "open": true diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dee8f36..1e00325 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,8 +18,6 @@ public void robotInit() { robotContainer = RobotContainer.getInstance(); swerve = Swerve.getInstance(); - // new Trigger(this::isEnabled).whileTrue(Arm.getInstance().home()); - new Trigger(this::isDisabled) .debounce(RobotConstants.DISABLE_COAST_DELAY) .onTrue(Commands.runOnce(() -> swerve.setBrake(false), swerve).ignoringDisable(true)); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e57e79..f057fd4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import frc.robot.intake.Intake; import frc.robot.odometry.Odometry; import frc.robot.shooter.Shooter; +import frc.robot.superstructure.Superstructure; import frc.robot.swerve.Swerve; /** Initializes subsystems and commands. */ @@ -24,9 +25,10 @@ public class RobotContainer { private final Intake intake; private final Odometry odometry; private final Shooter shooter; + private final Superstructure superstructure; private final Swerve swerve; - private final CommandXboxController driverController; + private final CommandXboxController driverController, operatorController; /** Creates a new instance of the robot container. */ private RobotContainer() { @@ -36,9 +38,11 @@ private RobotContainer() { intake = Intake.getInstance(); odometry = Odometry.getInstance(); shooter = Shooter.getInstance(); + superstructure = Superstructure.getInstance(); swerve = Swerve.getInstance(); driverController = new CommandXboxController(0); + operatorController = new CommandXboxController(1); initializeTelemetry(); configureDefaultCommands(); @@ -60,7 +64,7 @@ public static RobotContainer getInstance() { /** Initializes subsystem telemetry. */ private void initializeTelemetry() { - Telemetry.initializeTabs(arm, auto, climber, intake, odometry, shooter, swerve); + Telemetry.initializeTabs(arm, auto, climber, intake, odometry, shooter, superstructure, swerve); SmartDashboard.putData("Mechanism", RobotMechanisms.getInstance().getMechanism()); } @@ -72,6 +76,9 @@ private void configureDefaultCommands() { /** Configures controller bindings. */ private void configureBindings() { driverController.y().onTrue(odometry.tare()); + + operatorController.a().onTrue(superstructure.intake()).onFalse(superstructure.stow()); + operatorController.b().onTrue(superstructure.shoot()).onFalse(superstructure.stow()); } /** diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index f19b6ae..4c64752 100644 --- a/src/main/java/frc/robot/RobotMechanisms.java +++ b/src/main/java/frc/robot/RobotMechanisms.java @@ -132,7 +132,7 @@ public Mechanism2d getMechanism() { public void updateSuperstructure(SuperstructureState state) { Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position); - Rotation2d wristRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position); + Rotation2d wristRotation = Rotation2d.fromRotations(state.wristAngleRotations().position); shoulder.setAngle(shoulderRotation); diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 2324b73..54a67d0 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -110,4 +110,20 @@ public State getMeasuredWristState() { return new TrapezoidProfile.State( wristMotorValues.positionRotations, wristMotorValues.velocityRotationsPerSecond); } + + public void setShoulderPosition(double positionRotations) { + shoulderMotor.setPosition(positionRotations); + } + + public void setWristPosition(double positionRotations) { + wristMotor.setPosition(positionRotations); + } + + public void setShoulderSetpoint(double positionRotations, double velocityRotationsPerSecond) { + shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond); + } + + public void setWristSetpoint(double positionRotations, double velocityRotationsPerSecond) { + wristMotor.setSetpoint(positionRotations, velocityRotationsPerSecond); + } } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 1b73396..f638c0c 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -1,6 +1,5 @@ package frc.robot.intake; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; @@ -12,7 +11,6 @@ import frc.robot.intake.IntakeConstants.RollerMotorConstants; import frc.robot.intake.PivotMotorIO.PivotMotorIOValues; import frc.robot.intake.RollerMotorIO.RollerMotorIOValues; -import frc.robot.superstructure.SuperstructureConstants; /** Subsystem class for the intake subsystem. */ public class Intake extends Subsystem { @@ -39,11 +37,6 @@ private Intake() { pivotMotor.configure(); rollerMotor.configure(); - - Rotation2d initialAngle = SuperstructureConstants.IntakePivotAngleConstants.MAXIMUM_ANGLE; - - pivotMotor.setPosition(initialAngle.getRotations()); - pivotMotor.update(pivotMotorValues); } /** @@ -90,6 +83,14 @@ public State getMeasuredPivotState() { pivotMotorValues.positionRotations, pivotMotorValues.velocityRotationsPerSecond); } + public void setPivotPosition(double positionRotations) { + pivotMotor.setPosition(positionRotations); + } + + public void setPivotSetpoint(double positionRotations, double velocityRotationsPerSecond) { + pivotMotor.setSetpoint(positionRotations, velocityRotationsPerSecond); + } + public double getRollerVelocity() { rollerMotor.update(rollerMotorValues); diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 6586861..66dd419 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.RobotMechanisms; @@ -26,13 +27,18 @@ public class Superstructure extends Subsystem { /** Reference to the shooter subsystem. */ private final Shooter shooter; - private SuperstructureState measurement; + private SuperstructureState measurement, setpoint, goal; /** Creates a new instance of the superstructure subsystem. */ private Superstructure() { arm = Arm.getInstance(); intake = Intake.getInstance(); shooter = Shooter.getInstance(); + + setPosition(SuperstructureState.INITIAL); + + setpoint = SuperstructureState.INITIAL; + goal = SuperstructureState.INITIAL; } /** @@ -50,25 +56,17 @@ public static Superstructure getInstance() { @Override public void periodic() { - State measuredShoulderState = arm.getMeasuredShoulderState(); - State measuredWristState = arm.getMeasuredWristState(); + updateMeasurement(); - State measuredIntakePivotState = intake.getMeasuredPivotState(); - double measuredIntakeRollerVelocity = intake.getRollerVelocity(); + setpoint = SuperstructureState.nextSetpoint(setpoint, goal); - double measuredShooterFlywheelVelocity = shooter.getFlywheelVelocity(); - double measuredShooterSerializerVelocity = shooter.getSerializerVelocity(); + arm.setShoulderSetpoint( + setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); + arm.setWristSetpoint( + setpoint.wristAngleRotations().position, setpoint.wristAngleRotations().velocity); - measurement = - new SuperstructureState( - measuredShoulderState, - measuredWristState, - measuredIntakePivotState, - measuredIntakeRollerVelocity, - measuredShooterFlywheelVelocity, - measuredShooterSerializerVelocity); - - RobotMechanisms.getInstance().updateSuperstructure(measurement); + intake.setPivotSetpoint( + setpoint.pivotAngleRotations().position, setpoint.pivotAngleRotations().velocity); } @Override @@ -105,4 +103,58 @@ public void addToShuffleboard(ShuffleboardTab tab) { measurement.addDouble( "Serializer Velocity (rps)", () -> this.measurement.serializerVelocityRotationsPerSecond()); } + + private void updateMeasurement() { + State measuredShoulderState = arm.getMeasuredShoulderState(); + State measuredWristState = arm.getMeasuredWristState(); + + State measuredIntakePivotState = intake.getMeasuredPivotState(); + double measuredIntakeRollerVelocity = intake.getRollerVelocity(); + + double measuredShooterFlywheelVelocity = shooter.getFlywheelVelocity(); + double measuredShooterSerializerVelocity = shooter.getSerializerVelocity(); + + measurement = + new SuperstructureState( + measuredShoulderState, + measuredWristState, + measuredIntakePivotState, + measuredIntakeRollerVelocity, + measuredShooterFlywheelVelocity, + measuredShooterSerializerVelocity); + + RobotMechanisms.getInstance().updateSuperstructure(measurement); + } + + public void setPosition(SuperstructureState state) { + arm.setShoulderPosition(state.shoulderAngleRotations().position); + arm.setWristPosition(state.wristAngleRotations().position); + intake.setPivotPosition(state.pivotAngleRotations().position); + } + + private void setGoal(SuperstructureState goal) { + this.goal = goal; + + resetMotionProfile(); + } + + private void resetMotionProfile() { + setpoint = measurement; + } + + public Command to(SuperstructureState goal) { + return this.runOnce(() -> setGoal(goal)); + } + + public Command stow() { + return to(SuperstructureState.STOW); + } + + public Command intake() { + return to(SuperstructureState.INTAKE); + } + + public Command shoot() { + return to(SuperstructureState.SHOOT); + } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index 3dd9f0c..12b0d21 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -7,12 +7,6 @@ public class SuperstructureConstants { public static class ShoulderAngleConstants { - /** Minimum angle of the shoulder joint. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(29.5); - - /** Maximum angle of the shoulder joint. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90); - /** Tolerance of the shoulder joint. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -32,12 +26,6 @@ public static class ShoulderAngleConstants { } public static class WristAngleConstants { - /** Minimum angle of the wrist joint. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-86.759); - - /** Maximum angle of the wrist joint. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(85.98); - /** Tolerance of the wrist joint. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -56,13 +44,7 @@ public static class WristAngleConstants { public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); } - public static class IntakePivotAngleConstants { - /** Pivot motor's minimum angle. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-48); - - /** Pivot motor's maximum angle. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(86); - + public static class PivotAngleConstants { /** Pivot motor's tolerance. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0); diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index b784f82..b889c6a 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -3,6 +3,10 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import frc.robot.RobotConstants; +import frc.robot.superstructure.SuperstructureConstants.PivotAngleConstants; +import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; +import frc.robot.superstructure.SuperstructureConstants.WristAngleConstants; import java.util.Objects; /** Represents the state of the superstructure. */ @@ -14,6 +18,22 @@ public record SuperstructureState( double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { + public static final SuperstructureState INITIAL = + new SuperstructureState( + Rotation2d.fromDegrees(52.5), Rotation2d.fromDegrees(-35), Rotation2d.fromDegrees(86)); + + public static final SuperstructureState STOW = + new SuperstructureState( + Rotation2d.fromDegrees(29.5), Rotation2d.fromDegrees(85.98), Rotation2d.fromDegrees(86)); + + public static final SuperstructureState INTAKE = + new SuperstructureState( + Rotation2d.fromDegrees(29.5), Rotation2d.fromDegrees(4), Rotation2d.fromDegrees(-48)); + + public static final SuperstructureState SHOOT = + new SuperstructureState( + Rotation2d.fromDegrees(29.5), Rotation2d.fromDegrees(18), Rotation2d.fromDegrees(-48)); + /** * Creates a new superstructure state. * @@ -107,6 +127,42 @@ public boolean atPivotAngleGoal(SuperstructureState goal) { return MathUtil.isNear( this.pivotAngleRotations().position, goal.pivotAngleRotations().position, - SuperstructureConstants.IntakePivotAngleConstants.TOLERANCE.getRotations()); + SuperstructureConstants.PivotAngleConstants.TOLERANCE.getRotations()); + } + + /** + * Calculates the next setpoint. + * + * @param setpoint the previous setpoint. + * @param goal the goal. + * @return the next setpoint. + */ + public static SuperstructureState nextSetpoint( + SuperstructureState setpoint, SuperstructureState goal) { + State nextShoulderSetpoint = + ShoulderAngleConstants.MOTION_PROFILE.calculate( + RobotConstants.PERIODIC_DURATION, + setpoint.shoulderAngleRotations(), + goal.shoulderAngleRotations()); + + State nextWristSetpoint = + WristAngleConstants.MOTION_PROFILE.calculate( + RobotConstants.PERIODIC_DURATION, + setpoint.wristAngleRotations(), + goal.wristAngleRotations()); + + State nextPivotSetpoint = + PivotAngleConstants.MOTION_PROFILE.calculate( + RobotConstants.PERIODIC_DURATION, + setpoint.pivotAngleRotations(), + goal.pivotAngleRotations()); + + return new SuperstructureState( + nextShoulderSetpoint, + nextWristSetpoint, + nextPivotSetpoint, + goal.rollerVelocityRotationsPerSecond(), + goal.flywheelVelocityRotationsPerSecond(), + goal.serializerVelocityRotationsPerSecond()); } } From 9deccc0d4dc18cea4e44626e53ed69f8506a1b73 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 18 Mar 2024 13:56:24 -0400 Subject: [PATCH 3/8] feat(superstructure): Add queue for goal following. --- simgui.json | 3 + .../robot/superstructure/Superstructure.java | 63 +++++++++++++++---- .../superstructure/SuperstructureState.java | 4 ++ .../java/frc/robot/superstructure/ToGoal.java | 47 ++++++++++++++ 4 files changed, 105 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc/robot/superstructure/ToGoal.java diff --git a/simgui.json b/simgui.json index 545666b..4de94d1 100644 --- a/simgui.json +++ b/simgui.json @@ -88,6 +88,9 @@ } }, "Superstructure": { + "Goal": { + "open": true + }, "Measurement": { "open": true }, diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 66dd419..ea06ca0 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -57,16 +57,7 @@ public static Superstructure getInstance() { @Override public void periodic() { updateMeasurement(); - - setpoint = SuperstructureState.nextSetpoint(setpoint, goal); - - arm.setShoulderSetpoint( - setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); - arm.setWristSetpoint( - setpoint.wristAngleRotations().position, setpoint.wristAngleRotations().velocity); - - intake.setPivotSetpoint( - setpoint.pivotAngleRotations().position, setpoint.pivotAngleRotations().velocity); + updateSetpoint(); } @Override @@ -102,6 +93,36 @@ public void addToShuffleboard(ShuffleboardTab tab) { measurement.addDouble( "Serializer Velocity (rps)", () -> this.measurement.serializerVelocityRotationsPerSecond()); + + ShuffleboardLayout goal = Telemetry.addColumn(tab, "Goal"); + + goal.addDouble( + "Shoulder Position (deg)", + () -> Units.rotationsToDegrees(this.goal.shoulderAngleRotations().position)); + goal.addDouble( + "Shoulder Velocity (dps)", + () -> Units.rotationsToDegrees(this.goal.shoulderAngleRotations().velocity)); + + goal.addDouble( + "Wrist Position (deg)", + () -> Units.rotationsToDegrees(this.goal.wristAngleRotations().position)); + goal.addDouble( + "Wrist Velocity (dps)", + () -> Units.rotationsToDegrees(this.goal.wristAngleRotations().velocity)); + + goal.addDouble( + "Pivot Position (deg)", + () -> Units.rotationsToDegrees(this.goal.pivotAngleRotations().position)); + goal.addDouble( + "Pivot Velocity (dps)", + () -> Units.rotationsToDegrees(this.goal.pivotAngleRotations().velocity)); + + goal.addDouble("Roller Velocity (rps)", () -> this.goal.rollerVelocityRotationsPerSecond()); + + goal.addDouble("Flywheel Velocity (rps)", () -> this.goal.flywheelVelocityRotationsPerSecond()); + + goal.addDouble( + "Serializer Velocity (rps)", () -> this.goal.serializerVelocityRotationsPerSecond()); } private void updateMeasurement() { @@ -126,13 +147,25 @@ private void updateMeasurement() { RobotMechanisms.getInstance().updateSuperstructure(measurement); } + private void updateSetpoint() { + setpoint = SuperstructureState.nextSetpoint(setpoint, goal); + + arm.setShoulderSetpoint( + setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); + arm.setWristSetpoint( + setpoint.wristAngleRotations().position, setpoint.wristAngleRotations().velocity); + + intake.setPivotSetpoint( + setpoint.pivotAngleRotations().position, setpoint.pivotAngleRotations().velocity); + } + public void setPosition(SuperstructureState state) { arm.setShoulderPosition(state.shoulderAngleRotations().position); arm.setWristPosition(state.wristAngleRotations().position); intake.setPivotPosition(state.pivotAngleRotations().position); } - private void setGoal(SuperstructureState goal) { + public void setGoal(SuperstructureState goal) { this.goal = goal; resetMotionProfile(); @@ -142,8 +175,14 @@ private void resetMotionProfile() { setpoint = measurement; } + public boolean at(SuperstructureState goal) { + updateMeasurement(); + + return measurement.at(goal); + } + public Command to(SuperstructureState goal) { - return this.runOnce(() -> setGoal(goal)); + return new ToGoal(goal); } public Command stow() { diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index b889c6a..4617538 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -130,6 +130,10 @@ public boolean atPivotAngleGoal(SuperstructureState goal) { SuperstructureConstants.PivotAngleConstants.TOLERANCE.getRotations()); } + public boolean at(SuperstructureState other) { + return atShoulderAngleGoal(other) && atWristAngleGoal(other) && atPivotAngleGoal(other); + } + /** * Calculates the next setpoint. * 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..033922a --- /dev/null +++ b/src/main/java/frc/robot/superstructure/ToGoal.java @@ -0,0 +1,47 @@ +package frc.robot.superstructure; + +import edu.wpi.first.wpilibj2.command.Command; +import java.util.LinkedList; +import java.util.Queue; + +public class ToGoal extends Command { + + private final Superstructure superstructure; + + private final SuperstructureState goal; + + private final Queue intermediateGoals; + + public ToGoal(SuperstructureState goal) { + this.superstructure = Superstructure.getInstance(); + + this.goal = goal; + + this.intermediateGoals = new LinkedList(); + } + + @Override + public void initialize() { + intermediateGoals.clear(); + + // TODO Add more intermediate goals + + intermediateGoals.add(goal); + } + + @Override + public void execute() { + if (superstructure.at(intermediateGoals.element())) { + if (intermediateGoals.size() > 1) { + intermediateGoals.remove(); + } + } + + superstructure.setGoal(intermediateGoals.element()); + } + + @Override + public boolean isFinished() { + return superstructure.at(goal); + } +} From bde8ebbc640ec75aa2111f00d3d223cfb8bd7a04 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 18 Mar 2024 23:11:02 -0400 Subject: [PATCH 4/8] feat(superstructure): Add superstructure intermediate goals. --- simgui-ds.json | 4 +- simgui.json | 5 +- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/intake/IntakeConstants.java | 3 - .../robot/superstructure/Superstructure.java | 46 +++++- .../SuperstructureConstants.java | 3 + .../superstructure/SuperstructureGoals.java | 33 +++++ .../superstructure/SuperstructureState.java | 133 ++++++++++++++++++ .../java/frc/robot/superstructure/ToGoal.java | 24 ++-- 9 files changed, 231 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/superstructure/SuperstructureGoals.java diff --git a/simgui-ds.json b/simgui-ds.json index bc58bd9..af8e13e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -98,11 +98,11 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", "useGamepad": true }, { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 4de94d1..9f0a0e0 100644 --- a/simgui.json +++ b/simgui.json @@ -94,7 +94,10 @@ "Measurement": { "open": true }, - "open": true + "open": true, + "setpoint": { + "open": true + } }, "Swerve": { "Constants": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f057fd4..fd14757 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -77,8 +77,10 @@ private void configureDefaultCommands() { private void configureBindings() { driverController.y().onTrue(odometry.tare()); - operatorController.a().onTrue(superstructure.intake()).onFalse(superstructure.stow()); - operatorController.b().onTrue(superstructure.shoot()).onFalse(superstructure.stow()); + operatorController.leftTrigger().onTrue(superstructure.intake()); + operatorController.rightTrigger().onTrue(superstructure.shoot()); + + operatorController.x().onTrue(superstructure.stow()); } /** diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index 173bd7d..2bde4d8 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -13,9 +13,6 @@ public static class PivotMotorConstants { /** Angle between the measured intake position and the mass's position. */ public static final Rotation2d MASS_OFFSET = Rotation2d.fromDegrees(-34.34); - - /** Pivot motor's "out" angle. */ - public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(0); } /** Constants for the roller motor. */ diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index ea06ca0..0c2d1d8 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -94,6 +94,38 @@ public void addToShuffleboard(ShuffleboardTab tab) { measurement.addDouble( "Serializer Velocity (rps)", () -> this.measurement.serializerVelocityRotationsPerSecond()); + ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint"); + + setpoint.addDouble( + "Shoulder Position (deg)", + () -> Units.rotationsToDegrees(this.setpoint.shoulderAngleRotations().position)); + setpoint.addDouble( + "Shoulder Velocity (dps)", + () -> Units.rotationsToDegrees(this.setpoint.shoulderAngleRotations().velocity)); + + setpoint.addDouble( + "Wrist Position (deg)", + () -> Units.rotationsToDegrees(this.setpoint.wristAngleRotations().position)); + setpoint.addDouble( + "Wrist Velocity (dps)", + () -> Units.rotationsToDegrees(this.setpoint.wristAngleRotations().velocity)); + + setpoint.addDouble( + "Pivot Position (deg)", + () -> Units.rotationsToDegrees(this.setpoint.pivotAngleRotations().position)); + setpoint.addDouble( + "Pivot Velocity (dps)", + () -> Units.rotationsToDegrees(this.setpoint.pivotAngleRotations().velocity)); + + setpoint.addDouble( + "Roller Velocity (rps)", () -> this.setpoint.rollerVelocityRotationsPerSecond()); + + setpoint.addDouble( + "Flywheel Velocity (rps)", () -> this.setpoint.flywheelVelocityRotationsPerSecond()); + + setpoint.addDouble( + "Serializer Velocity (rps)", () -> this.setpoint.serializerVelocityRotationsPerSecond()); + ShuffleboardLayout goal = Telemetry.addColumn(tab, "Goal"); goal.addDouble( @@ -147,6 +179,12 @@ private void updateMeasurement() { RobotMechanisms.getInstance().updateSuperstructure(measurement); } + public SuperstructureState getState() { + updateMeasurement(); + + return measurement; + } + private void updateSetpoint() { setpoint = SuperstructureState.nextSetpoint(setpoint, goal); @@ -168,12 +206,12 @@ public void setPosition(SuperstructureState state) { public void setGoal(SuperstructureState goal) { this.goal = goal; - resetMotionProfile(); + // resetMotionProfile(); } - private void resetMotionProfile() { - setpoint = measurement; - } + // private void resetMotionProfile() { + // setpoint = measurement; + // } public boolean at(SuperstructureState goal) { updateMeasurement(); diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index 12b0d21..a712bb0 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -45,6 +45,9 @@ public static class WristAngleConstants { } public static class PivotAngleConstants { + /** Pivot motor's "out" angle. */ + public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(0); + /** Pivot motor's tolerance. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0); diff --git a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java new file mode 100644 index 0000000..7a9c9a7 --- /dev/null +++ b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java @@ -0,0 +1,33 @@ +package frc.robot.superstructure; + +import java.util.LinkedList; +import java.util.Queue; + +public class SuperstructureGoals { + + private final Queue goals; + + public SuperstructureGoals(SuperstructureState start, SuperstructureState end) { + goals = new LinkedList(); + + if (start.pivotIn() && end.pivotOut()) { + goals.add(start.withPivotAngleOf(end)); + } + + goals.add(end); + } + + public SuperstructureState get() { + return goals.element(); + } + + public SuperstructureState next() { + boolean hasNext = goals.size() > 1; + + if (hasNext) { + goals.remove(); + } + + return get(); + } +} diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 4617538..01aa385 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -91,6 +91,127 @@ public SuperstructureState( this(shoulderAngle, wristAngle, pivotAngle, 0.0, 0.0, 0.0); } + public SuperstructureState withShoulderAngle(Rotation2d newShoulderAngle) { + return new SuperstructureState( + new State(newShoulderAngle.getRotations(), 0.0), + wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withShoulderAngleOf(SuperstructureState other) { + return new SuperstructureState( + other.shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withWristAngle(Rotation2d newWristAngle) { + return new SuperstructureState( + shoulderAngleRotations, + new State(newWristAngle.getRotations(), 0.0), + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withWristAngleOf(SuperstructureState other) { + return new SuperstructureState( + shoulderAngleRotations, + other.wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withPivotAngle(Rotation2d newPivotAngle) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + new State(newPivotAngle.getRotations(), 0.0), + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withPivotAngleOf(SuperstructureState other) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + other.pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withRollerVelocity(double newRollerVelocityRotationsPerSecond) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + newRollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withRollerVelocityOf(SuperstructureState other) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + other.rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withFlywheelVelocity(double newFlywheelVelocityRotationsPerSecond) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + newFlywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withFlywheelVelocityOf(SuperstructureState other) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + other.flywheelVelocityRotationsPerSecond, + serializerVelocityRotationsPerSecond); + } + + public SuperstructureState withSerializerVelocity( + double newSerializerVelocityRotationsPerSecond) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + newSerializerVelocityRotationsPerSecond); + } + + public SuperstructureState withSerializerVelocityOf(SuperstructureState other) { + return new SuperstructureState( + shoulderAngleRotations, + wristAngleRotations, + pivotAngleRotations, + rollerVelocityRotationsPerSecond, + flywheelVelocityRotationsPerSecond, + other.serializerVelocityRotationsPerSecond); + } + /** * Returns true if at the shoulder angle goal. * @@ -134,6 +255,18 @@ public boolean at(SuperstructureState other) { return atShoulderAngleGoal(other) && atWristAngleGoal(other) && atPivotAngleGoal(other); } + public boolean wristStowed() { + return wristAngleRotations.position > 0; + } + + public boolean pivotOut() { + return pivotAngleRotations.position < PivotAngleConstants.OUT_ANGLE.getRotations(); + } + + public boolean pivotIn() { + return !pivotOut(); + } + /** * Calculates the next setpoint. * diff --git a/src/main/java/frc/robot/superstructure/ToGoal.java b/src/main/java/frc/robot/superstructure/ToGoal.java index 033922a..b78434d 100644 --- a/src/main/java/frc/robot/superstructure/ToGoal.java +++ b/src/main/java/frc/robot/superstructure/ToGoal.java @@ -1,8 +1,6 @@ package frc.robot.superstructure; import edu.wpi.first.wpilibj2.command.Command; -import java.util.LinkedList; -import java.util.Queue; public class ToGoal extends Command { @@ -10,34 +8,32 @@ public class ToGoal extends Command { private final SuperstructureState goal; - private final Queue intermediateGoals; + private SuperstructureGoals goals; public ToGoal(SuperstructureState goal) { this.superstructure = Superstructure.getInstance(); this.goal = goal; - this.intermediateGoals = new LinkedList(); + goals = new SuperstructureGoals(superstructure.getState(), this.goal); + + addRequirements(this.superstructure); } @Override public void initialize() { - intermediateGoals.clear(); - - // TODO Add more intermediate goals - - intermediateGoals.add(goal); + goals = new SuperstructureGoals(superstructure.getState(), this.goal); } @Override public void execute() { - if (superstructure.at(intermediateGoals.element())) { - if (intermediateGoals.size() > 1) { - intermediateGoals.remove(); - } + SuperstructureState goal = goals.get(); + + if (superstructure.at(goal)) { + goal = goals.next(); } - superstructure.setGoal(intermediateGoals.element()); + superstructure.setGoal(goal); } @Override From cfacae5cc8f398a3c47841f324e6575b6128a885 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 19 Mar 2024 00:00:35 -0400 Subject: [PATCH 5/8] feat(superstructure): Implement shoulder movements. --- simgui.json | 3 +++ src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/superstructure/Superstructure.java | 4 ++++ .../SuperstructureConstants.java | 21 +++++++++++++++++-- .../superstructure/SuperstructureGoals.java | 10 +++++++-- .../superstructure/SuperstructureState.java | 20 +++++++----------- 6 files changed, 43 insertions(+), 16 deletions(-) diff --git a/simgui.json b/simgui.json index 9f0a0e0..9189977 100644 --- a/simgui.json +++ b/simgui.json @@ -94,6 +94,9 @@ "Measurement": { "open": true }, + "Setpoint": { + "open": true + }, "open": true, "setpoint": { "open": true diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fd14757..c309d79 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,6 +80,7 @@ private void configureBindings() { operatorController.leftTrigger().onTrue(superstructure.intake()); operatorController.rightTrigger().onTrue(superstructure.shoot()); + operatorController.a().onTrue(superstructure.amp()); operatorController.x().onTrue(superstructure.stow()); } diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 0c2d1d8..960f1e6 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -234,4 +234,8 @@ public Command intake() { public Command shoot() { return to(SuperstructureState.SHOOT); } + + public Command amp() { + return to(SuperstructureState.AMP); + } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index a712bb0..f2d2759 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -7,6 +7,12 @@ public class SuperstructureConstants { public static class ShoulderAngleConstants { + public static final Rotation2d INITIAL = Rotation2d.fromDegrees(52.5); + + public static final Rotation2d STOW = Rotation2d.fromDegrees(29.5); + + public static final Rotation2d AMP = Rotation2d.fromDegrees(90); + /** Tolerance of the shoulder joint. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -26,6 +32,16 @@ public static class ShoulderAngleConstants { } public static class WristAngleConstants { + public static final Rotation2d INITIAL = Rotation2d.fromDegrees(-35); + + public static final Rotation2d STOW = Rotation2d.fromDegrees(85.98); + + public static final Rotation2d INTAKE = Rotation2d.fromDegrees(4); + + public static final Rotation2d SHOOT = Rotation2d.fromDegrees(18); + + public static final Rotation2d AMP = Rotation2d.fromDegrees(0); + /** Tolerance of the wrist joint. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -45,8 +61,9 @@ public static class WristAngleConstants { } public static class PivotAngleConstants { - /** Pivot motor's "out" angle. */ - public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(0); + public static final Rotation2d UP = Rotation2d.fromDegrees(86); + + public static final Rotation2d DOWN = Rotation2d.fromDegrees(-48); /** Pivot motor's tolerance. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0); diff --git a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java index 7a9c9a7..0180250 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java @@ -1,5 +1,6 @@ package frc.robot.superstructure; +import frc.robot.superstructure.SuperstructureConstants.WristAngleConstants; import java.util.LinkedList; import java.util.Queue; @@ -10,8 +11,13 @@ public class SuperstructureGoals { public SuperstructureGoals(SuperstructureState start, SuperstructureState end) { goals = new LinkedList(); - if (start.pivotIn() && end.pivotOut()) { - goals.add(start.withPivotAngleOf(end)); + if (!start.atShoulderAngleGoal(end)) { + goals.add(start.withWristAngle(WristAngleConstants.STOW)); + goals.add( + start + .withShoulderAngleOf(end) + .withWristAngle(WristAngleConstants.STOW) + .withPivotAngleOf(end)); } goals.add(end); diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 01aa385..900af48 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -20,19 +20,23 @@ public record SuperstructureState( public static final SuperstructureState INITIAL = new SuperstructureState( - Rotation2d.fromDegrees(52.5), Rotation2d.fromDegrees(-35), Rotation2d.fromDegrees(86)); + ShoulderAngleConstants.INITIAL, WristAngleConstants.INITIAL, PivotAngleConstants.UP); public static final SuperstructureState STOW = new SuperstructureState( - Rotation2d.fromDegrees(29.5), Rotation2d.fromDegrees(85.98), Rotation2d.fromDegrees(86)); + ShoulderAngleConstants.STOW, WristAngleConstants.STOW, PivotAngleConstants.UP); public static final SuperstructureState INTAKE = new SuperstructureState( - Rotation2d.fromDegrees(29.5), Rotation2d.fromDegrees(4), Rotation2d.fromDegrees(-48)); + ShoulderAngleConstants.STOW, WristAngleConstants.INTAKE, PivotAngleConstants.DOWN); public static final SuperstructureState SHOOT = new SuperstructureState( - Rotation2d.fromDegrees(29.5), Rotation2d.fromDegrees(18), Rotation2d.fromDegrees(-48)); + ShoulderAngleConstants.STOW, WristAngleConstants.SHOOT, PivotAngleConstants.DOWN); + + public static final SuperstructureState AMP = + new SuperstructureState( + ShoulderAngleConstants.AMP, WristAngleConstants.AMP, PivotAngleConstants.UP); /** * Creates a new superstructure state. @@ -259,14 +263,6 @@ public boolean wristStowed() { return wristAngleRotations.position > 0; } - public boolean pivotOut() { - return pivotAngleRotations.position < PivotAngleConstants.OUT_ANGLE.getRotations(); - } - - public boolean pivotIn() { - return !pivotOut(); - } - /** * Calculates the next setpoint. * From 4d2d167d1792da8902d25777216fd8f2f978d038 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 19 Mar 2024 08:25:56 -0400 Subject: [PATCH 6/8] fix(auto): Use deadlines. --- .../deploy/pathplanner/autos/amp.full.auto | 55 ------------------- .../pathplanner/autos/amp.shootLeave.auto | 17 +++++- src/main/java/frc/robot/auto/Auto.java | 13 +++++ 3 files changed, 28 insertions(+), 57 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/amp.full.auto diff --git a/src/main/deploy/pathplanner/autos/amp.full.auto b/src/main/deploy/pathplanner/autos/amp.full.auto deleted file mode 100644 index d579d60..0000000 --- a/src/main/deploy/pathplanner/autos/amp.full.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 6.63 - }, - "rotation": 60.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "home" - } - }, - { - "type": "named", - "data": { - "name": "shootNote" - } - }, - { - "type": "path", - "data": { - "pathName": "amp.toNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "amp.toSubwoofer" - } - }, - { - "type": "named", - "data": { - "name": "shootNote" - } - }, - { - "type": "named", - "data": { - "name": null - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto index 8572861..7374646 100644 --- a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto +++ b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto @@ -18,9 +18,22 @@ } }, { - "type": "named", + "type": "deadline", "data": { - "name": "shootNote" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] } }, { diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index f215828..78210c1 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -1,6 +1,7 @@ package frc.robot.auto; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; @@ -13,6 +14,7 @@ import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.odometry.Odometry; +import frc.robot.superstructure.Superstructure; import frc.robot.swerve.Swerve; import frc.robot.swerve.SwerveConstants; import java.util.function.Consumer; @@ -27,6 +29,9 @@ public class Auto extends Subsystem { /** Reference to the odometry subsystem. */ private final Odometry odometry; + /** Reference to the superstructure subsystem. */ + private final Superstructure superstructure; + /** Reference to the swerve subsystem. */ private final Swerve swerve; @@ -36,6 +41,7 @@ public class Auto extends Subsystem { /** Creates a new instance of the auto subsystem. */ private Auto() { odometry = Odometry.getInstance(); + superstructure = Superstructure.getInstance(); swerve = Swerve.getInstance(); Supplier robotPositionSupplier = () -> odometry.getPosition(); @@ -64,6 +70,13 @@ private Auto() { AllianceFlipHelper::shouldFlip, swerve); + NamedCommands.registerCommand("home", superstructure.stow()); + NamedCommands.registerCommand("stow", superstructure.stow()); + NamedCommands.registerCommand("readyIntake", superstructure.intake()); + NamedCommands.registerCommand("intakeNote", superstructure.intake()); + NamedCommands.registerCommand("readyShoot", superstructure.shoot()); + NamedCommands.registerCommand("shootNote", superstructure.shoot()); + autoChooser = AutoBuilder.buildAutoChooser(); } From 856ffc6bf55a9d62c60fd9ef98f2f5b9e780eba6 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 19 Mar 2024 09:13:20 -0400 Subject: [PATCH 7/8] fix(superstructure): Add some intermediate rules. --- simgui-ds.json | 3 +- src/main/java/frc/robot/RobotContainer.java | 2 + .../robot/superstructure/Superstructure.java | 4 ++ .../superstructure/SuperstructureGoals.java | 62 ++++++++++++++++--- .../superstructure/SuperstructureState.java | 13 ++++ 5 files changed, 74 insertions(+), 10 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index af8e13e..acf3b7e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -101,8 +101,7 @@ "useGamepad": true }, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c309d79..2bdbd48 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -81,7 +81,9 @@ private void configureBindings() { operatorController.rightTrigger().onTrue(superstructure.shoot()); operatorController.a().onTrue(superstructure.amp()); + operatorController.b().onTrue(superstructure.intake()); operatorController.x().onTrue(superstructure.stow()); + operatorController.y().onTrue(superstructure.shoot()); } /** diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 960f1e6..49c163b 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -223,6 +223,10 @@ public Command to(SuperstructureState goal) { return new ToGoal(goal); } + public Command initial() { + return to(SuperstructureState.INITIAL); + } + public Command stow() { return to(SuperstructureState.STOW); } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java index 0180250..5442c1a 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java @@ -1,5 +1,7 @@ package frc.robot.superstructure; +import frc.robot.superstructure.SuperstructureConstants.PivotAngleConstants; +import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; import frc.robot.superstructure.SuperstructureConstants.WristAngleConstants; import java.util.LinkedList; import java.util.Queue; @@ -8,19 +10,63 @@ public class SuperstructureGoals { private final Queue goals; - public SuperstructureGoals(SuperstructureState start, SuperstructureState end) { - goals = new LinkedList(); + public static Queue merge( + Queue first, Queue second) { + Queue mergedGoals = new LinkedList(); + + while (!first.isEmpty()) { + mergedGoals.add(first.poll()); + } + + while (!second.isEmpty()) { + mergedGoals.add(second.poll()); + } + + return mergedGoals; + } + + public static Queue generate( + SuperstructureState start, SuperstructureState end) { + Queue goals = new LinkedList(); + + boolean shoulderMove = !start.atShoulderAngleGoal(end); - if (!start.atShoulderAngleGoal(end)) { + // Shoulder move: HOME -> STOW; ANY -> AMP + // Move wrist to STOW first, then move shoulder + if (shoulderMove) { goals.add(start.withWristAngle(WristAngleConstants.STOW)); - goals.add( - start - .withShoulderAngleOf(end) - .withWristAngle(WristAngleConstants.STOW) - .withPivotAngleOf(end)); + goals.add(end.withWristAngle(WristAngleConstants.STOW)); + goals.add(end); + return goals; + } + + boolean shoulderStowed = start.atShoulderAngle(ShoulderAngleConstants.STOW); + boolean wristMove = !start.atWristAngleGoal(end); + boolean pivotMove = !start.atPivotAngleGoal(end); + boolean pivotUp = start.pivotAngleRotations().position > PivotAngleConstants.DOWN.getRotations(); + + // Stow position movement & wrist-intake collision avoidance + if (shoulderStowed && wristMove && pivotMove) { + if (pivotUp) { + // Pivot is up and needs to go down; move it first + goals.add(start.withPivotAngleOf(end)); + } else { + // Pivot is down and needs to go up; move wrist out of way first + goals.add(start.withWristAngleOf(end)); + } } goals.add(end); + + return goals; + } + + public SuperstructureGoals(Queue goals) { + this.goals = goals; + } + + public SuperstructureGoals(SuperstructureState start, SuperstructureState end) { + this(generate(start, end)); } public SuperstructureState get() { diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 900af48..ef81d90 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -229,6 +229,19 @@ public boolean atShoulderAngleGoal(SuperstructureState goal) { SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations()); } + /** + * Returns true if at the shoulder angle. + * + * @param other + * @return true if at the shoulder angle. + */ + public boolean atShoulderAngle(Rotation2d other) { + return MathUtil.isNear( + this.shoulderAngleRotations().position, + other.getRotations(), + SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations()); + } + /** * Returns true if at the wrist angle goal. * From 6bef304f0d8e2ba19273ce276f1036ee5596dc99 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 19 Mar 2024 10:00:45 -0400 Subject: [PATCH 8/8] refactor(superstructure): Exit early. --- simgui.json | 3 --- .../superstructure/SuperstructureGoals.java | 26 +++++++------------ .../java/frc/robot/superstructure/ToGoal.java | 5 +++- 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/simgui.json b/simgui.json index 9189977..9f0a0e0 100644 --- a/simgui.json +++ b/simgui.json @@ -94,9 +94,6 @@ "Measurement": { "open": true }, - "Setpoint": { - "open": true - }, "open": true, "setpoint": { "open": true diff --git a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java index 5442c1a..e2652f3 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureGoals.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureGoals.java @@ -10,30 +10,18 @@ public class SuperstructureGoals { private final Queue goals; - public static Queue merge( - Queue first, Queue second) { - Queue mergedGoals = new LinkedList(); - - while (!first.isEmpty()) { - mergedGoals.add(first.poll()); - } - - while (!second.isEmpty()) { - mergedGoals.add(second.poll()); - } - - return mergedGoals; - } - public static Queue generate( SuperstructureState start, SuperstructureState end) { Queue goals = new LinkedList(); + // TOOD Bug where the shoulder "bounces" while the intake is pivoting down + boolean shoulderMove = !start.atShoulderAngleGoal(end); // Shoulder move: HOME -> STOW; ANY -> AMP // Move wrist to STOW first, then move shoulder if (shoulderMove) { + System.out.println("*** GENERATING SHOULDER MOVE ***"); goals.add(start.withWristAngle(WristAngleConstants.STOW)); goals.add(end.withWristAngle(WristAngleConstants.STOW)); goals.add(end); @@ -46,13 +34,19 @@ public static Queue generate( boolean pivotUp = start.pivotAngleRotations().position > PivotAngleConstants.DOWN.getRotations(); // Stow position movement & wrist-intake collision avoidance - if (shoulderStowed && wristMove && pivotMove) { + if (shoulderStowed && !shoulderMove && wristMove && pivotMove) { if (pivotUp) { // Pivot is up and needs to go down; move it first + System.out.println("*** GENERATING PIVOT DOWN ***"); goals.add(start.withPivotAngleOf(end)); + goals.add(end); + return goals; } else { + System.out.println("*** GENERATING WRIST UP ***"); // Pivot is down and needs to go up; move wrist out of way first goals.add(start.withWristAngleOf(end)); + goals.add(end); + return goals; } } diff --git a/src/main/java/frc/robot/superstructure/ToGoal.java b/src/main/java/frc/robot/superstructure/ToGoal.java index b78434d..3f42158 100644 --- a/src/main/java/frc/robot/superstructure/ToGoal.java +++ b/src/main/java/frc/robot/superstructure/ToGoal.java @@ -1,6 +1,8 @@ package frc.robot.superstructure; import edu.wpi.first.wpilibj2.command.Command; +import java.util.LinkedList; +import java.util.Queue; public class ToGoal extends Command { @@ -15,7 +17,8 @@ public ToGoal(SuperstructureState goal) { this.goal = goal; - goals = new SuperstructureGoals(superstructure.getState(), this.goal); + Queue empty = new LinkedList(); + goals = new SuperstructureGoals(empty); addRequirements(this.superstructure); }