From b03c055c6a5b658c045fd7629e3702150e005736 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 9 Feb 2024 22:37:20 -0500 Subject: [PATCH] Add Speaker to constants Finish devbot superstructure --- .../frc2024/FieldConstants.java | 10 +++--- .../frc2024/RobotContainer.java | 25 ++++++--------- .../littletonrobotics/frc2024/RobotState.java | 3 +- .../frc2024/subsystems/drive/Drive.java | 1 - .../superstructure/DevBotSuperstructure.java | 31 ++++++++++++++++--- .../frc2024/util/AllianceFlipUtil.java | 10 ++++++ 6 files changed, 52 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java b/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java index 56b7147a..6d5c1005 100644 --- a/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java @@ -79,11 +79,11 @@ public static final class Speaker { new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); /** Center of the speaker opening (blue alliance) */ - // public static Translation2d centerSpeakerOpening = - // new Translation2d(topLeftSpeaker.getX() / 2.0, Units.inchesToMeters(241.56)); - - public static Translation2d centerSpeakerOpening = - new Translation2d(topLeftSpeaker.getX() / 2, fieldWidth - Units.inchesToMeters(104.0)); + public static Translation3d centerSpeakerOpening = + new Translation3d( + topLeftSpeaker.getX() / 2.0, + fieldWidth - Units.inchesToMeters(104.0), + (bottomLeftSpeaker.getZ() + bottomRightSpeaker.getZ()) / 2.0); } public static double aprilTagWidth = Units.inchesToMeters(6.50); diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 6a24b45a..37965935 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -35,6 +35,9 @@ import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim; +import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder; +import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSim; +import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim; @@ -61,6 +64,7 @@ public class RobotContainer { private Drive drive; private AprilTagVision aprilTagVision; private Flywheels flywheels; + private Feeder feeder; private Intake intake; private Arm arm; private DevBotSuperstructure superstructure; @@ -84,8 +88,8 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); arm = new Arm(new ArmIOKrakenFOC()); flywheels = new Flywheels(new FlywheelsIOSparkFlex()); - // intake = new Intake(new IntakeIOSparkMax()); - superstructure = new DevBotSuperstructure(arm, flywheels); + feeder = new Feeder(new FeederIOSparkFlex()); + superstructure = new DevBotSuperstructure(arm, flywheels, feeder); } case SIMBOT -> { drive = @@ -97,8 +101,9 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.moduleConfigs[3])); arm = new Arm(new ArmIOSim()); flywheels = new Flywheels(new FlywheelsIOSim()); + feeder = new Feeder(new FeederIOSim()); intake = new Intake(new IntakeIOSim()); - superstructure = new DevBotSuperstructure(arm, flywheels); + superstructure = new DevBotSuperstructure(arm, flywheels, feeder); } case COMPBOT -> { // No impl yet @@ -208,24 +213,12 @@ private void configureButtonBindings() { Trigger readyToShootTrigger = new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); - readyToShootTrigger - .whileTrue( - Commands.run( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) - .whileFalse( - Commands.run( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); controller .rightTrigger() .and(readyToShootTrigger) .onTrue( Commands.runOnce( - () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT)) - .andThen(Commands.waitSeconds(0.5)) - .andThen( - Commands.runOnce( - () -> - superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE)))); + () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT))); controller .leftBumper() diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index bc1d4f90..d0d9e569 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -149,8 +149,9 @@ public AimingParameters getAimingParameters() { Pose2d robot = getEstimatedPose(); Twist2d fieldVelocity = fieldVelocity(); - Translation2d originToGoal = + Translation3d originToGoal3d = AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening); + Translation2d originToGoal = new Translation2d(originToGoal3d.getX(), originToGoal3d.getY()); Translation2d originToRobot = robot.getTranslation(); // Get robot to goal angle but limit to reasonable range diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index 55d311c0..bf66b29f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -237,7 +237,6 @@ public void periodic() { currentSetpoint = setpointGenerator.generateSetpoint( currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); - // run modules SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) { diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java index 392a1b85..6d9e09d0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java @@ -1,11 +1,15 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; import lombok.Setter; +import org.littletonrobotics.frc2024.FieldConstants; +import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; +import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; @@ -40,13 +44,15 @@ public enum SystemState { @Getter @Setter private SystemState goalState = SystemState.IDLE; private final Arm arm; private final Flywheels flywheels; + private final Feeder feeder; private final Timer followThroughTimer = new Timer(); private double followThroughArmAngle = 0.0; - public DevBotSuperstructure(Arm arm, Flywheels flywheels) { + public DevBotSuperstructure(Arm arm, Flywheels flywheels, Feeder feeder) { this.arm = arm; this.flywheels = flywheels; + this.feeder = feeder; } @Override @@ -80,11 +86,26 @@ public void periodic() { switch (currentState) { case IDLE -> { arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get())); - flywheels.runVolts(0.0, 0.0); + flywheels.runVolts(4.0, 4.0); + feeder.stop(); + } + case INTAKE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); + flywheels.runVolts(-2.0, -2.0); + feeder.runVolts(-0.5); + } + case PREPARE_SHOOT -> { + var aimingParams = RobotState.getInstance().getAimingParameters(); + arm.setSetpoint( + new Rotation2d( + aimingParams.effectiveDistance(), + FieldConstants.Speaker.centerSpeakerOpening.getZ() + + Units.inchesToMeters(yCompensation.get()))); + flywheels.setSetpointRpm(shootingLeftRPM.get(), shootingRightRPM.get()); + } + case SHOOT -> { + feeder.runVolts(2.0); } - case INTAKE -> {} - case PREPARE_SHOOT -> {} - case SHOOT -> {} } Logger.recordOutput("DevBotSuperstructure/GoalState", goalState); diff --git a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java index 1aa2b36b..f4efb69f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java @@ -3,6 +3,7 @@ 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.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import org.littletonrobotics.frc2024.FieldConstants; @@ -47,6 +48,15 @@ public static Pose2d apply(Pose2d pose) { } } + public static Translation3d apply(Translation3d translation3d) { + if (shouldFlip()) { + return new Translation3d( + apply(translation3d.getX()), translation3d.getY(), translation3d.getZ()); + } else { + return translation3d; + } + } + /** * Flips a trajectory state to the correct side of the field based on the current alliance color. */