diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 200d92a5..55670d70 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,7 @@ // GNU General Public License for more details. package frc.robot; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.robot.subsystems.drive.module.ModuleConstants; @@ -102,8 +103,17 @@ private DriveConstants() { ); } - private static class ArmConstants { + public static class ArmConstants { private ArmConstants(){} + + public static final double SHOULDER_BAR_LENGTH_METERS = 0.0; + public static final double SHOOTER_BAR_LENGTH_METERS = 0.0; + + public static final Translation3d PIVOT_TRANSLATION_METERS = new Translation3d( + 0.0, + 0.0, + 0.0 + ); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 937efb86..adf80960 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -54,7 +54,7 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs -// private final LoggedDashboardChooser autoChooser; + private final LoggedDashboardChooser autoChooser; private final LoggedDashboardNumber wristPower; private final LoggedDashboardNumber wristPosition; @@ -73,6 +73,7 @@ public RobotContainer() { new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS)); m_shooter = new ShooterSubsystem(new ShooterIntakeIOPrototype()); + m_armSubsystem = new ArmSubsystem(new ArmIOPrototype()); } case PROTO_SHOOTER -> { m_driveSubsystem = new DriveSubsystem( @@ -86,16 +87,16 @@ public RobotContainer() { m_armSubsystem = new ArmSubsystem(new ArmIOPrototype()); } case PROTO_ARM -> { -// m_driveSubsystem = new DriveSubsystem( -// new GyroIO() { -// }, -// new ModuleIO() {}, -// new ModuleIO() {}, -// new ModuleIO() {}, -// new ModuleIO() {}); + m_driveSubsystem = new DriveSubsystem( + new GyroIO() { + }, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); m_shooter = new ShooterSubsystem(new ShooterIO() {}); m_armSubsystem = new ArmSubsystem(new ArmIOPrototype() {}); - } + } case SIM -> { // Sim robot, instantiate physics sim IO implementations m_driveSubsystem = @@ -107,21 +108,17 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS), new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS)); m_shooter = new ShooterSubsystem(new ShooterIOSim()); + m_armSubsystem = new ArmSubsystem(new ArmIO() {}); } default -> { // Replayed robot, disable IO implementations m_driveSubsystem = new DriveSubsystem( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }); + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); m_shooter = new ShooterSubsystem(new ShooterIO() {}); m_armSubsystem = new ArmSubsystem(new ArmIO() {}); } @@ -130,6 +127,12 @@ public RobotContainer() { // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + // Set up dashboard tuners + wristPower = new LoggedDashboardNumber("Wrist Power", 0.0); + wristPosition = new LoggedDashboardNumber("Wrist Position", 0.0); + armPower = new LoggedDashboardNumber("Wrist Power", 0.0); + armPosition = new LoggedDashboardNumber("Wrist Position", 0.0); + // Set up feedforward characterization autoChooser.addOption( "Drive FF Characterization", @@ -176,6 +179,6 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand();//autoChooser.get(); + return autoChooser.get(); } } diff --git a/src/main/java/frc/robot/subsystems/Supersystem.java b/src/main/java/frc/robot/subsystems/Supersystem.java new file mode 100644 index 00000000..373d4c94 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Supersystem.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lib.utils.FieldConstants; +import lib.utils.AllianceFlipUtil; + +public class Supersystem extends SubsystemBase { + public Supersystem() { + + } + + public double calcShooterAngle(Pose2d robotPose) { + Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); + double groundDistance = Math.sqrt(Math.pow(speakerPose.getX(), 2) + Math.pow(speakerPose.getY(), 2)); + return Math.atan2(groundDistance, FieldConstants.CENTER_SPEAKER.getZ()); + } + + public double calcMaxShooterAngle(Pose2d robotPose, double shoulderAngle) { + return 0; + } +} + diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index d5e09725..568bde8b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -1,61 +1,58 @@ package frc.robot.subsystems.arm; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class ArmSubsystem extends SubsystemBase { - private final ArmIO m_io; - public ArmSubsystem(ArmIO io) { - m_io = io; - } - - @Override - public void periodic() { - m_io.updateInputs(new ArmIOInputsAutoLogged()); - -// m_io.setWristVoltage(0.0); -// m_io.setShoulderVoltage(0.0); - SmartDashboard.putNumber("Recorded Wrist Position", m_io.getWristPosition().getDegrees()); - SmartDashboard.putNumber("Recorded Shoulder Position", m_io.getShoulderPosition().getDegrees()); - } - - public void setShoulderPower(double power) { - m_io.setShoulderVoltage(power * 12.0); - } - - public void setShoulderPosition(double degrees) { - m_io.setShoulderAngle(degrees); - } - - public void setWristPower(double power) {m_io.setWristVoltage(power * 12.0);} - - public void setWristPosition(double degrees) { - m_io.setWristAngle(degrees); - } - - public Command setShoulderPowerFactory(double power) { - return runOnce(() -> setShoulderPower(power)); - } - - public Command setShoulderPositionFactory(double degrees) { - return runOnce(() -> setShoulderPosition(degrees)); - } - - public Command setWristPowerFactory(double power) { - return runOnce(() -> setWristPower(power)); - } - - public Command setWristPositionFactory(double degrees) { - return runOnce(() -> setWristPosition(degrees)); - } - - public Command stopArmFactory() { - return runOnce(() -> { - setWristPower(0.0); - setShoulderPower(0.0); - }); - } + public enum ArmStates { + STOW, + AIM, + AIM_BLOCKED, + AMP, + TRAP, + MOVING + } + + private final ArmIO m_io; + public ArmSubsystem(ArmIO io) { + m_io = io; + } + + @Override + public void periodic() { + m_io.updateInputs(new ArmIOInputsAutoLogged()); + } + + public void setShoulderPower(double power) { + m_io.setShoulderVoltage(power * 12.0); + } + + public void setShoulderPosition(double degrees) { + m_io.setShoulderAngle(degrees); + } + + public void setWristPower(double power) { + m_io.setWristVoltage(power * 12.0); + } + + public void setWristPosition(double degrees) { + m_io.setWristAngle(degrees); + } + + public Translation3d getShooterTranslation(Rotation2d shoulderRotation, Rotation2d shooterRotation) { + return Constants.ArmConstants.PIVOT_TRANSLATION_METERS.plus( + new Translation3d( + Constants.ArmConstants.SHOULDER_BAR_LENGTH_METERS, + new Rotation3d(0.0, shoulderRotation.getDegrees(), 0.0))) + .plus( + new Translation3d( + Constants.ArmConstants.SHOOTER_BAR_LENGTH_METERS, + new Rotation3d(0.0, shooterRotation.getDegrees(), 0.0)) + ); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b4d2deb4..d1935dec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -6,6 +6,7 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class ShooterSubsystem extends SubsystemBase { + private final ShooterIO m_io; private final ShooterIOInputsAutoLogged m_inputs; @@ -47,7 +48,6 @@ public void setIntakePower(double power) { public void runShooterVelocity() { m_io.setLeftVelocityRpm(m_leftSetpoint.get()); m_io.setRightVelocityRpm(m_rightSetpoint.get()); -// m_io.setKickerVoltage(9.0); } public Command setShooterPowerFactory(double left, double right) { diff --git a/src/main/java/lib/utils/AllianceFlipUtil.java b/src/main/java/lib/utils/AllianceFlipUtil.java new file mode 100644 index 00000000..fc533eef --- /dev/null +++ b/src/main/java/lib/utils/AllianceFlipUtil.java @@ -0,0 +1,98 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package lib.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** + * Utility functions for flipping from the blue to red alliance. By default, all translations and + * poses in {@link FieldConstants} are stored with the origin at the rightmost point on the blue + * alliance wall. + */ +public class AllianceFlipUtil { + private AllianceFlipUtil() {} + /** Flips a translation to the correct side of the field based on the current alliance color. */ + public static Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(FieldConstants.FIELD_LENGTH - translation.getX(), translation.getY()); + } else { + return translation; + } + } + + public static Translation3d apply(Translation3d translation) { + if (shouldFlip()) { + return new Translation3d(FieldConstants.FIELD_LENGTH - translation.getX(), + translation.getY(), + translation.getZ()); + } else { + return translation; + } + } + + /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ + public static double apply(double xCoordinate) { + if (shouldFlip()) { + return FieldConstants.FIELD_LENGTH - xCoordinate; + } else { + return xCoordinate; + } + } + + /** Flips a rotation based on the current alliance color. */ + public static Rotation2d apply(Rotation2d rotation) { + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else { + return rotation; + } + } + + /** Flips a pose to the correct side of the field based on the current alliance color. */ + public static Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d( + FieldConstants.FIELD_LENGTH - pose.getX(), + pose.getY(), + new Rotation2d(-pose.getRotation().getCos(), pose.getRotation().getSin())); + } else { + return pose; + } + } + + /** + * Flips a trajectory state to the correct side of the field based on the current alliance color. + */ + public static Trajectory.State apply(Trajectory.State state) { + if (shouldFlip()) { + return new Trajectory.State( + state.timeSeconds, + state.velocityMetersPerSecond, + state.accelerationMetersPerSecondSq, + new Pose2d( + FieldConstants.FIELD_LENGTH - state.poseMeters.getX(), + state.poseMeters.getY(), + new Rotation2d( + -state.poseMeters.getRotation().getCos(), + state.poseMeters.getRotation().getSin())), + -state.curvatureRadPerMeter); + } else { + return state; + } + } + + private static boolean shouldFlip() { + return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue) == Alliance.Red; + } +} diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java new file mode 100644 index 00000000..bbcef154 --- /dev/null +++ b/src/main/java/lib/utils/FieldConstants.java @@ -0,0 +1,103 @@ +package lib.utils; + +import static edu.wpi.first.apriltag.AprilTagFields.k2024Crescendo; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import java.io.IOException; + +/** + * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets + * of corners start in the lower left moving clockwise. All units in Meters
+ *
+ * + *

All translations and poses are stored with the origin at the rightmost point on the BLUE + * ALLIANCE wall.
+ *
+ * Length refers to the x direction (as described by wpilib)
+ * Width refers to the y direction (as described by wpilib) + */ +public class FieldConstants { + private FieldConstants() {} + public static final double FIELD_LENGTH = Units.inchesToMeters(651.223); + public static final double FIELD_WIDTH = Units.inchesToMeters(323.277); + public static final double WING_X = Units.inchesToMeters(229.201); + public static final double PODIUM_X = Units.inchesToMeters(126.75); + public static final double STARTING_LINE_X = Units.inchesToMeters(74.111); + + public static final Translation2d AMP_CENTER = + new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + + /** Staging locations for each note */ + public static final class StagingLocations { + public static final double CENTERLINE_X = FIELD_LENGTH / 2.0; + + // need to update + public static final double CENTERLINE_FIRST_Y = Units.inchesToMeters(29.638); + public static final double CENTERLINE_SEPARATION_Y = Units.inchesToMeters(66); + public static final double SPIKE_X = Units.inchesToMeters(114); + // need + public static final double SPIKE_FIRST_Y = Units.inchesToMeters(161.638); + public static final double SPIKE_SEPARATION_Y = Units.inchesToMeters(57); + + public static final Translation2d[] CENTERLINE_TRANSLATIONS = new Translation2d[5]; + public static final Translation2d[] SPIKE_TRANSLATIONS = new Translation2d[3]; + + static { + for (int i = 0; i < CENTERLINE_TRANSLATIONS.length; i++) { + CENTERLINE_TRANSLATIONS[i] = + new Translation2d(CENTERLINE_X, CENTERLINE_FIRST_Y + (i * CENTERLINE_SEPARATION_Y)); + } + } + + static { + for (int i = 0; i < SPIKE_TRANSLATIONS.length; i++) { + SPIKE_TRANSLATIONS[i] = new Translation2d(SPIKE_X, SPIKE_FIRST_Y + (i * SPIKE_SEPARATION_Y)); + } + } + } + + /** Each corner of the speaker * */ + public static final class Speaker { + + /** Center of the speaker opening (blue alliance) */ + public static final Pose2d CENTER_SPEAKER_OPENING = + new Pose2d(0.0, FIELD_WIDTH - Units.inchesToMeters(104.0), new Rotation2d()); + } + + // corners (blue alliance origin) + public static final Translation3d TOP_RIGHT_SPEAKER = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(83.091)); + + public static final Translation3d TOP_LEFT_SPEAKER = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(197.765), + Units.inchesToMeters(83.091)); + + public static final Translation3d BOTTOM_RIGHT_SPEAKER = + new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); + public static final Translation3d BOTTOM_LEFT_SPEAKER = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + + public static final Translation3d CENTER_SPEAKER = + new Translation3d( + Units.inchesToMeters(18.055) / 2.0, + Units.inchesToMeters((238.815 + 197.765) / 2.0), + Units.inchesToMeters(83.091 + 78.324) / 2.0); + + public static final double APRIL_TAG_WIDTH = Units.inchesToMeters(6.50); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; + + static { + try { + APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); + } + } +}