From 07f0572fa6d50479eabb95877a884b9e66dc0a4e Mon Sep 17 00:00:00 2001 From: boomermath Date: Sun, 22 Sep 2024 20:41:40 -0400 Subject: [PATCH 1/8] Add feeder subsystem for shooter --- .../robot/subsystems/ShooterSubsystem.java | 40 ++++++++++--------- 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3bb5e4c..f2ead9f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -14,7 +15,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.RobotDataPublisher; @@ -25,8 +25,8 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX leftKraken; private final TalonFX rightKraken; - private final TalonFX neo; private final DigitalInput beamBreak; + private final FeederSubsystem feederSubsystem; private final StatusSignal leftVel; private final StatusSignal rightVel; @@ -36,7 +36,7 @@ public class ShooterSubsystem extends SubsystemBase { public static class ShooterConstants { public static ShooterModes shooterModes; public static ShooterStatus shooterStatus; - public static double velocity = 0; + public static final int SHOOTER_LEFT_MOTOR = 5; //id public static final int SHOOTER_RIGHT_MOTOR = 15; //id @@ -60,13 +60,11 @@ public static class ShooterConstants { withKA(MOTION_MAGIC_ACCELERATION). withKV(SHOOTER_V); - - public static final int SHOOTER_NEO = 16; public static final int BEAM_BREAK = 0; public static InterpolatingTreeMap SHOOTER_MAP() { InterpolatingTreeMap map = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShooterStateData.interpolator); - // TODO: decrease angles by aroun 0.05 to tune + // TODO: decrease angles by around 0.05 to tune map.put(1.375, new ShooterStateData(0.5, 125)); map.put(1.7, new ShooterStateData(.485, 125)); map.put(2.0, new ShooterStateData(0.478, 125)); @@ -85,6 +83,20 @@ public static InterpolatingTreeMap SHUTTLE_MAP() { } } + + private static class FeederSubsystem extends SubsystemBase { + private final TalonFX feederMotor; + public static final int SHOOTER_NEO = 16; + + public FeederSubsystem() { + feederMotor = new TalonFX(SHOOTER_NEO, "canivoreBus"); + } + + protected Command spinFeederAndStop(double speed) { + return startEnd(() -> feederMotor.set(speed), feederMotor::stopMotor); + } + } + public ShooterSubsystem() { leftKraken = new TalonFX(ShooterConstants.SHOOTER_LEFT_MOTOR, Constants.TalonFXConstants.CANIVORE_NAME); rightKraken = new TalonFX(ShooterConstants.SHOOTER_RIGHT_MOTOR, Constants.TalonFXConstants.CANIVORE_NAME); @@ -104,8 +116,6 @@ public ShooterSubsystem() { leftVel = leftKraken.getVelocity(); rightVel = rightKraken.getVelocity(); - neo = new TalonFX(ShooterConstants.SHOOTER_NEO, "canivoreBus"); - beamBreak = new DigitalInput(ShooterConstants.BEAM_BREAK); motionMagicVelocityVoltage = new MotionMagicVelocityVoltage(0); @@ -115,6 +125,8 @@ public ShooterSubsystem() { motionMagicConfigs.MotionMagicJerk = 4000; rightMotorConfigurator.apply(rightMotorConfiguration); leftMotorConfigurator.apply(rightMotorConfiguration); + + feederSubsystem = new FeederSubsystem(); } @Override @@ -131,22 +143,14 @@ public boolean getBeamBreak() { return !beamBreak.get(); } - private void stopFeeder() { - neo.stopMotor(); - } - - private void spinFeeder(double speed) { - neo.set(speed); - } - public void stopShooter() { rightKraken.stopMotor(); leftKraken.stopMotor(); - neo.stopMotor(); ShooterConstants.shooterStatus = ShooterStatus.OFF; } public void setDualVelocity(double leftVel, double rightVel) { + leftKraken.setControl(new VelocityVoltage(leftVel)); leftKraken.setControl(motionMagicVelocityVoltage.withVelocity(leftVel)); rightKraken.setControl(motionMagicVelocityVoltage.withVelocity(rightVel)); } @@ -190,7 +194,7 @@ public Command setVelocityContinuous(double vel) { } public Command spinFeederAndStop(double vel) { - return startEnd(() -> spinFeeder(vel), this::stopFeeder); + return feederSubsystem.spinFeederAndStop(vel); } public Command spinFeederMaxAndStop() { From 90c76d641365f004a42eab383557efc3407c4242 Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 02:14:04 -0400 Subject: [PATCH 2/8] Refactor shooter into subclasses --- src/main/java/frc/robot/Constants.java | 21 +- src/main/java/frc/robot/RobotCommands.java | 88 +++---- src/main/java/frc/robot/RobotContainer.java | 38 ++- .../commands/auto/AutoNamedCommands.java | 92 ++----- .../frc/robot/subsystems/PivotSubsystem.java | 230 ------------------ .../robot/subsystems/ShooterSubsystem.java | 217 ----------------- .../java/frc/robot/util/ShooterStateData.java | 30 --- 7 files changed, 78 insertions(+), 638 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/PivotSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java delete mode 100644 src/main/java/frc/robot/util/ShooterStateData.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c7ad75..8a439e5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,19 +1,9 @@ package frc.robot; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.interpolation.InterpolatingTreeMap; -import edu.wpi.first.math.interpolation.InverseInterpolator; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; -import frc.robot.util.ShooterStateData; import java.io.IOException; import java.util.Map; @@ -22,7 +12,7 @@ public class Constants { - public class FieldConstants{ + public static class FieldConstants { public static final double fieldLength = Units.inchesToMeters(651.223); public static final class Speaker { @@ -39,6 +29,7 @@ public static final class Speaker { public static Translation3d centerSpeakerOpening = bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5); public static final AprilTagFieldLayout aprilTags; + static { try { aprilTags = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile); @@ -50,7 +41,7 @@ public static final class Speaker { public static final class Shuttle { public static Translation2d closeShuttleTargetCorner = - new Translation2d(0.0,7.0); + new Translation2d(0.0, 7.0); public static Translation2d farShuttleTargetCorner = new Translation2d(2.5, 7.0); @@ -60,12 +51,12 @@ public static final class Shuttle { } } - public class CANCoderConstants{ + public static class CANCoderConstants { public static final int COUNTS_PER_REV = 4096; public static final double COUNTS_PER_DEG = COUNTS_PER_REV / 360.0; } - public class OIConstants{ + public static class OIConstants { public static final Map CONTROLLERS = Map.of( 0, ControllerType.CUSTOM, 1, ControllerType.XBOX ); @@ -79,7 +70,7 @@ public enum ControllerType { } } - public class TalonFXConstants{ + public static class TalonFXConstants { public static final boolean TALON_FUTURE_PROOF = true; public static final String CANIVORE_NAME = "canivoreBus"; } diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index 2ca8f74..074702d 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -2,33 +2,52 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; -import frc.robot.subsystems.*; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; -import frc.robot.util.RobotDataPublisher; +import frc.robot.subsystems.shooter.*; import frc.robot.util.AllianceFlipUtil; -import frc.robot.util.ShooterStateData; -import frc.robot.util.controllerUtils.MultiButton; + +import java.util.function.Supplier; public class RobotCommands { - private final IntakeSubsystem intake; private final PivotSubsystem pivot; - private final ShooterSubsystem shooter; private final CommandSwerveDrivetrain commandSwerveDrivetrain; private final ArmSubsystem arm; private final ElevatorSubsystem elevator; + private final FeederSubsystem feeder; + private final FlywheelSubsystem flywheel; - public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, ShooterSubsystem shooter, CommandSwerveDrivetrain commandSwerveDrivetrain, ArmSubsystem arm, ElevatorSubsystem elevator1) { - this.intake = intake; + public RobotCommands(PivotSubsystem pivot, CommandSwerveDrivetrain commandSwerveDrivetrain, ArmSubsystem arm, ElevatorSubsystem elevator, FeederSubsystem feeder, FlywheelSubsystem flywheel) { this.pivot = pivot; - this.shooter = shooter; this.commandSwerveDrivetrain = commandSwerveDrivetrain; this.arm = arm; - this.elevator = elevator1; + this.elevator = elevator; + this.feeder = feeder; + this.flywheel = flywheel; + } + + private Command updateTarget(Supplier shooterTarget) { + return pivot.updatePivotPositionWith(() -> shooterTarget.get().angle) + .alongWith(flywheel.reachVelocity(() -> shooterTarget.get().rps)); + } + + public Command setTarget(double angle, double velocity) { + return pivot.adjustPivotPositionTo(angle) + .alongWith(flywheel.reachVelocity(velocity)); + } + + public Command setTargetAndShoot(double angle, double velocity) { + return setTarget(angle, velocity).andThen(feeder.feed(-1)); + } + + + private Command createShooterTargetCommand(Pose2d target, InterpolatingTreeMap targetMap) { + Supplier shooterTargetSupplier = () -> targetMap.get(commandSwerveDrivetrain.getState().Pose.relativeTo(target).getTranslation().getNorm()); + return updateTarget(shooterTargetSupplier); } /** @@ -38,57 +57,30 @@ public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, ShooterSubsys * @return {@code Command} instance */ public Command setShooterState() { - RobotDataPublisher shooterStatePublisher = commandSwerveDrivetrain.observablePose().map(robotPose -> { - final Pose2d speakerPose = AllianceFlipUtil.apply(new Pose2d(0.0, 5.5, Rotation2d.fromDegrees(0))); - Pose2d relativeSpeaker = robotPose.relativeTo(speakerPose); - double distance = relativeSpeaker.getTranslation().getNorm(); - return ShooterSubsystem.ShooterConstants.SHOOTER_MAP().get(distance); - }); - - return pivot.updatePivotPositionWith(shooterStatePublisher) - .alongWith(shooter.updateVelocityWith(shooterStatePublisher)); + return createShooterTargetCommand(AllianceFlipUtil.apply(new Pose2d(0.0, 5.5, Rotation2d.fromDegrees(0))), ShooterValues.SHOOTER_MAP); } public Command setShuttleState() { - RobotDataPublisher shuttleStatePublisher = commandSwerveDrivetrain.observablePose().map(robotPose -> { - final Pose2d shuttleTarget = AllianceFlipUtil.apply(new Pose2d(2.5, 7.0, Rotation2d.fromDegrees(0))); - Pose2d relativeShuttleTarget = robotPose.relativeTo(shuttleTarget); - double distance = relativeShuttleTarget.getTranslation().getNorm(); - return ShooterSubsystem.ShooterConstants.SHUTTLE_MAP().get(distance); - }); - return pivot.updatePivotPositionWith(shuttleStatePublisher).alongWith(shooter.updateVelocityWith(shuttleStatePublisher)); + return createShooterTargetCommand(AllianceFlipUtil.apply(new Pose2d(0.0, 5.5, Rotation2d.fromDegrees(0))), ShooterValues.SHUTTLE_MAP); } public Command handoff() { - return pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen( - arm.moveUpAndStop(.5).until(() -> - arm.getEnc() >= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION).andThen( - shooter.spinFeederAndStop(-0.3).alongWith(intake.rollOut(-0.15)) - ).until(shooter::getBeamBreak) - ); + return arm.moveUpAndStop(.5).until(() -> + arm.getEnc() >= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION) + .andThen(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)) + .andThen(feeder.ingest(-1)); } public Command setAmp() { return elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.AMP).andThen(pivot.moveDown(-0.25).unless( - () -> pivot.getEncoderAngle() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))); + () -> pivot.getPosition() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))); } public Command stowAmp() { return pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen(elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN))); } - public Command bumpFire(){ - return pivot.adjustPivotPositionTo(.53).andThen( - new StartEndCommand(() -> arm.moveUp(.7), arm::stop).until(() -> - arm.getEnc() <= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION).andThen( - shooter.spinFeederAndStop(-0.3).alongWith(intake.rollOut(-.2)) - ).until(shooter::getBeamBreak) - ).andThen( - shooter.setVelocityAndStop(100)).andThen( - intake.rollOut(-.1).withTimeout(0.2)).andThen( - Commands.waitSeconds(.45)).andThen( - shooter.spinFeederMaxAndStop().alongWith(intake.rollOut(-1).withTimeout(1)).andThen( - this.handoff().withTimeout(2))); + return setTargetAndShoot(0.53, 100); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 531a6d0..e252d32 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -29,20 +28,22 @@ import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.drivetrain.Telemetry; import frc.robot.subsystems.drivetrain.generated.TunerConstants; +import frc.robot.subsystems.shooter.FeederSubsystem; +import frc.robot.subsystems.shooter.FlywheelSubsystem; +import frc.robot.subsystems.shooter.PivotSubsystem; import frc.robot.util.controllerUtils.ButtonHelper; import frc.robot.util.controllerUtils.ControllerContainer; import frc.robot.util.controllerUtils.MultiButton; public class RobotContainer { - - public final ShooterSubsystem shooter = new ShooterSubsystem(); - public final PivotSubsystem pivot = new PivotSubsystem(); public final ElevatorSubsystem elevator = new ElevatorSubsystem(); public final IntakeSubsystem intake = new IntakeSubsystem(); public final ArmSubsystem arm = new ArmSubsystem(); - public final CommandXboxController joystick = new CommandXboxController(1); // My joystick public final VisionSubsystem vision = new VisionSubsystem(); + public final FeederSubsystem feeder = new FeederSubsystem(); + public final FlywheelSubsystem flywheel = new FlywheelSubsystem(); + public final PivotSubsystem pivot = new PivotSubsystem(); final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() @@ -50,23 +51,16 @@ public class RobotContainer { .withRotationalDeadband(CommandSwerveDrivetrain.MaFxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); public ControllerContainer controllerContainer = new ControllerContainer(); public SendableChooser autoChooser; - ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers()); + private final ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers()); + public final CommandXboxController joystick = new CommandXboxController(1); // My joystick private boolean autoNeedsRebuild = true; private Command auto; - private final RobotCommands robotCommands = new RobotCommands(intake, pivot, shooter, drivetrain, arm, elevator); + private final RobotCommands robotCommands = new RobotCommands(pivot, drivetrain, arm, elevator, feeder, flywheel); public RobotContainer() { - NamedCommands.registerCommand("shootBump", Commands.sequence( - pivot.adjustPivotPositionTo(0.55), - shooter.shooterBumpFire(), - Commands.waitSeconds(.75), // is this waiting for a specific speed or something? should prob be replaced - shooter.spinFeederMaxAndStop().alongWith(intake.rollOut(-1).withTimeout(1)) - )); - var field = new Field2d(); SmartDashboard.putData("Field", field); @@ -80,7 +74,6 @@ public RobotContainer() { field.getObject("path").setPoses(poses) ); - if (Utils.isSimulation()) { drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } @@ -103,19 +96,18 @@ public RobotContainer() { private void configureBindings() { buttonHelper.createButton(1, 0, robotCommands.setShuttleState().alongWith(new ShuttleAimCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())), MultiButton.RunCondition.WHILE_HELD); - buttonHelper.createButton(8, 0, shooter.setVelocityAndStop(-70), MultiButton.RunCondition.WHILE_HELD); - buttonHelper.createButton(7, 0, shooter.setVelocityAndStop(15), MultiButton.RunCondition.WHILE_HELD); + buttonHelper.createButton(8, 0, flywheel.reachVelocity(-70), MultiButton.RunCondition.WHILE_HELD); + buttonHelper.createButton(7, 0, flywheel.reachVelocity(15), MultiButton.RunCondition.WHILE_HELD); buttonHelper.createButton(5, 0, robotCommands.bumpFire(), MultiButton.RunCondition.WHEN_PRESSED); buttonHelper.createButton(10, 0, robotCommands.handoff().withTimeout(2), MultiButton.RunCondition.WHEN_PRESSED); buttonHelper.createButton(2, 0, intake.rollOut(-.65), MultiButton.RunCondition.WHILE_HELD); buttonHelper.createButton(4, 0, elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)).andThen(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)), MultiButton.RunCondition.WHEN_PRESSED); - buttonHelper.createButton(6, 0, shooter.spinFeederAndStop(-.1).alongWith(intake.rollIn(0.5)), MultiButton.RunCondition.WHILE_HELD); + buttonHelper.createButton(6, 0, feeder.feed(-.1).alongWith(intake.rollIn(0.5)), MultiButton.RunCondition.WHILE_HELD); buttonHelper.createButton(9, 0, elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.AMP).andThen(pivot.moveDown(-0.25).unless( - () -> pivot.getEncoderAngle() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))), MultiButton.RunCondition.WHEN_PRESSED); + () -> pivot.getPosition() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))), MultiButton.RunCondition.WHEN_PRESSED); intake.intakeOccupiedTrigger.onTrue(vision.blinkLimelight().alongWith(successfulIntakeRumble())); intake.intakeOccupiedTrigger.and(joystick.rightBumper().negate()).onTrue(robotCommands.handoff().withTimeout(2)); - buttonHelper.createButton(11, 0, shooter.shooterTrap(), MultiButton.RunCondition.WHILE_HELD); drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest( @@ -152,7 +144,7 @@ private void configureBindings() { joystick.a().onTrue(robotCommands.setAmp()); // Shoot - joystick.rightTrigger().whileTrue(shooter.spinFeederMaxAndStop().alongWith(intake.rollOut(-1))); + joystick.rightTrigger().whileTrue(feeder.feed(-1).alongWith(intake.rollOut(-1))); // Handoff joystick.povUp().onTrue(robotCommands.handoff().withTimeout(2)); // Move elevator down @@ -184,7 +176,7 @@ public void updateOdometryVision() { } public void buildAutoChooser() { - var namedCommands = new AutoNamedCommands(intake, shooter, pivot, arm, robotCommands); + var namedCommands = new AutoNamedCommands(intake, pivot, arm, feeder, robotCommands); namedCommands.registerCommands(); autoChooser = new SendableChooser<>(); diff --git a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java index 938cc6e..6a582f3 100644 --- a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java @@ -1,27 +1,26 @@ package frc.robot.commands.auto; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.RobotCommands; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.shooter.FeederSubsystem; +import frc.robot.subsystems.shooter.PivotSubsystem; public class AutoNamedCommands { private final IntakeSubsystem intakeSubsystem; - private final ShooterSubsystem shooterSubsystem; private final PivotSubsystem pivotSubsystem; private final ArmSubsystem armSubsystem; + private final FeederSubsystem feederSubsystem; + private final RobotCommands robotCommands; - public AutoNamedCommands(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem, ArmSubsystem armSubsystem, RobotCommands robotCommands) { + + public AutoNamedCommands(IntakeSubsystem intakeSubsystem, PivotSubsystem pivotSubsystem, ArmSubsystem armSubsystem, FeederSubsystem feederSubsystem, RobotCommands robotCommands) { this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; this.pivotSubsystem = pivotSubsystem; this.armSubsystem = armSubsystem; + this.feederSubsystem = feederSubsystem; this.robotCommands = robotCommands; registerCommands(); } @@ -32,76 +31,19 @@ public void registerCommands() { NamedCommands.registerCommand("armUp", armSubsystem.moveUpAndStop(.5).until(() -> armSubsystem.getEnc() >= 0.75)); NamedCommands.registerCommand("armDown", armSubsystem.moveDownAndStop(.5).until( () -> armSubsystem.getEnc() <= .5 && armSubsystem.getEnc() >= .44).alongWith(pivotSubsystem.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF))); - NamedCommands.registerCommand("shootBump", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(0.53), - shooterSubsystem.shooterBumpFire(), - new WaitCommand(.75), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)), - new InstantCommand(shooterSubsystem::stopShooter) - )); - - NamedCommands.registerCommand("shootLine", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.47), - shooterSubsystem.setVelocityContinuous(100), - intakeSubsystem.rollOut(-0.1).withTimeout(0.25), - new WaitCommand(.45), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1.7)) - )); - - NamedCommands.registerCommand("shootLine2", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.39), - shooterSubsystem.setVelocityContinuous(100), - intakeSubsystem.rollOut(-0.1).withTimeout(0.2), - new WaitCommand(.45), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)) - )); - - NamedCommands.registerCommand("shootNearSource", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.48), - shooterSubsystem.setVelocityContinuous(90), - intakeSubsystem.rollOut(-0.1).withTimeout(0.2), - new WaitCommand(.45), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)) - )); - - NamedCommands.registerCommand("shuttleMid", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.535), - shooterSubsystem.setVelocityContinuous(100), - intakeSubsystem.rollOut(-0.1).withTimeout(0.2), - new WaitCommand(.45), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)) - )); - - - NamedCommands.registerCommand("shootBumpCorner", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.55), - shooterSubsystem.shooterBumpFire(), - new WaitCommand(.75), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)), - new InstantCommand(shooterSubsystem::stopShooter) - )); - - NamedCommands.registerCommand("shootBumpLast", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.55), - shooterSubsystem.shooterBumpFire(), - new WaitCommand(.25), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)) - )); - - - NamedCommands.registerCommand("shootFar", new SequentialCommandGroup( - pivotSubsystem.adjustPivotPositionTo(.45), - shooterSubsystem.setVelocityContinuous(100), - new WaitCommand(.75), - shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1)) - )); + NamedCommands.registerCommand("shootBump", robotCommands.setTargetAndShoot(0.53, 100)); + NamedCommands.registerCommand("shootLine", robotCommands.setTargetAndShoot(0.47, 100)); + NamedCommands.registerCommand("shootLine2", robotCommands.setTargetAndShoot(0.39, 100)); + NamedCommands.registerCommand("shootNearSource", robotCommands.setTargetAndShoot(0.48, 90)); + NamedCommands.registerCommand("shuttleMid", robotCommands.setTargetAndShoot(0.535, 100)); + NamedCommands.registerCommand("shootBumpCorner", robotCommands.setTargetAndShoot(0.55, 100)); + NamedCommands.registerCommand("shootBumpLast", robotCommands.setTargetAndShoot(0.55, 100)); + NamedCommands.registerCommand("shootBumpLast", robotCommands.setTargetAndShoot(0.45, 100)); - NamedCommands.registerCommand("shootOut", shooterSubsystem.spinFeederMaxAndStop().alongWith(intakeSubsystem.rollOut(-1).withTimeout(1))); + NamedCommands.registerCommand("shootOut", feederSubsystem.feed(-1).alongWith(intakeSubsystem.rollOut(-1).withTimeout(1))); NamedCommands.registerCommand("handoff", robotCommands.handoff().withTimeout(1.5)); - NamedCommands.registerCommand("handoffMidLine", intakeSubsystem.rollIn(.275) - .withTimeout(0.5).andThen( - robotCommands.handoff().withTimeout(1.5))); + NamedCommands.registerCommand("handoffMidLine", intakeSubsystem.rollIn(.275).withTimeout(0.5).andThen(robotCommands.handoff().withTimeout(1.5))); NamedCommands.registerCommand("pivotHandoff", pivotSubsystem.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)); } diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java deleted file mode 100644 index ba64dd7..0000000 --- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java +++ /dev/null @@ -1,230 +0,0 @@ -package frc.robot.subsystems; - - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.*; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants; -import frc.robot.util.RobotDataPublisher; -import frc.robot.util.RobotDataPublisher.RobotDataSubscription; -import frc.robot.util.ShooterStateData; - -public class PivotSubsystem extends SubsystemBase { - - public final TalonFX pivotMotor; - public final CANcoder pivotEncoder; - private final StatusSignal pivotPositionStatusSignal; - public DigitalInput forwardLimitSwitch; - public DigitalInput reverseLimitSwitch; - - public class PivotConstants { - - public static final int PIVOT_LIMIT_SWITCH_FORWARD = 7; - public static final int PIVOT_LIMIT_SWITCH_REVERSE = 6; - public static final int PIVOT_ONE_MOTOR_ID = 29; //placeholder - public static final int PIVOT_ONE_CANCODER_ID = 16; //placeholder - public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake; - - public static final double PIVOT_POSITION_STATUS_FRAME = 0.05; - - public static final double PIVOT_P = 50; //1 //350.0 /11.7 - public static final double PIVOT_I = 0; - public static final double PIVOT_D = 2; //0 //45.0 - public static final double PIVOT_V = 0.12000000149011612; // 65 - public static final double PIVOT_A = 0.009999999776482582; // 0.7 - public static final double PIVOT_G = 0.02; // 0.35 - public static final double PROFILE_V = 0.000000001; - public static final double PROFILE_A = 0.000000001; - - public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs().withKP(PIVOT_P).withKI(PIVOT_I).withKD(PIVOT_D). - withKA(PIVOT_A).withKV(PIVOT_V).withKG(PIVOT_G).withGravityType(GravityTypeValue.Arm_Cosine); - - public static final CurrentLimitsConfigs PIVOT_CURRENT_LIMIT = new CurrentLimitsConfigs().withSupplyCurrentLimitEnable(true).withSupplyCurrentThreshold(55). - withSupplyCurrentLimit(65).withSupplyTimeThreshold(0.1).withStatorCurrentLimitEnable(true).withStatorCurrentLimit(65); - - public static final SoftwareLimitSwitchConfigs PIVOT_SOFT_LIMIT = new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(true).withForwardSoftLimitThreshold( - .53 - ).withReverseSoftLimitEnable(true).withReverseSoftLimitThreshold( - .31 - ); - - public static final double MAGNET_OFFSET = 0.433838; //placeholder - - public enum PivotPosition { - CUSTOM(-1) { - @Override - public double getPosition() { - if (super.getPosition() == -1) { - throw new IllegalArgumentException("Custom pivot position was not set"); - } - - return super.getPosition(); - } - }, - HANDOFF(0.5); - //placeholder - private double position; - - PivotPosition(double position) { - this.position = position; - } - - public double getPosition() { - return position; - } - - public static PivotPosition CUSTOM(double position) { - PivotPosition pivotPosition = PivotPosition.CUSTOM; - pivotPosition.position = position; - return pivotPosition; - } - } - - private static PivotPosition pivotSetPosition = PivotPosition.CUSTOM(0.45); - } - - public PivotSubsystem() { - reverseLimitSwitch = new DigitalInput(PivotConstants.PIVOT_LIMIT_SWITCH_REVERSE); - forwardLimitSwitch = new DigitalInput(PivotConstants.PIVOT_LIMIT_SWITCH_FORWARD); - pivotMotor = new TalonFX(PivotConstants.PIVOT_ONE_MOTOR_ID, Constants.TalonFXConstants.CANIVORE_NAME); - pivotEncoder = new CANcoder(PivotConstants.PIVOT_ONE_CANCODER_ID, Constants.TalonFXConstants.CANIVORE_NAME); - - pivotMotor.setInverted(true); - pivotMotor.setNeutralMode(NeutralModeValue.Brake); - pivotPositionStatusSignal = pivotMotor.getPosition(); - - new Trigger(this::getForwardLimitSwitch).whileTrue(moveDown(0.05)); - new Trigger(this::getReverseLimitSwitch).whileTrue(moveUp(0.05)); - - var pivotMotor1Configurator = pivotMotor.getConfigurator(); - var talonFXConfiguration = new TalonFXConfiguration(); - - talonFXConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - talonFXConfiguration.Feedback.FeedbackRemoteSensorID = pivotEncoder.getDeviceID(); - talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - talonFXConfiguration.MotorOutput.NeutralMode = PivotConstants.PIVOT_NEUTRAL_MODE; - talonFXConfiguration.FutureProofConfigs = true; - talonFXConfiguration.Feedback.SensorToMechanismRatio = 1; - talonFXConfiguration.Feedback.RotorToSensorRatio = 83.79; - talonFXConfiguration.CurrentLimits = PivotConstants.PIVOT_CURRENT_LIMIT; - talonFXConfiguration.SoftwareLimitSwitch = PivotConstants.PIVOT_SOFT_LIMIT; - talonFXConfiguration.Slot0 = PivotConstants.SLOT_0_CONFIGS; - talonFXConfiguration.MotionMagic.MotionMagicCruiseVelocity = 2; - talonFXConfiguration.MotionMagic.MotionMagicAcceleration = 2; - talonFXConfiguration.MotionMagic.MotionMagicExpo_kA = PivotConstants.PROFILE_A; - talonFXConfiguration.MotionMagic.MotionMagicExpo_kV = PivotConstants.PROFILE_V; - - pivotMotor1Configurator.apply(talonFXConfiguration); - - var pivotEncoder1Configurator = pivotEncoder.getConfigurator(); - var cancoderConfiguration = new CANcoderConfiguration(); - - cancoderConfiguration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; - cancoderConfiguration.MagnetSensor.MagnetOffset = PivotConstants.MAGNET_OFFSET; - cancoderConfiguration.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - pivotEncoder1Configurator.apply(cancoderConfiguration); - } - - @Override - public void periodic() { - pivotPositionStatusSignal.refresh(); - - Sendable pivotPositionSendable = sendableBuilder -> { - sendableBuilder.addStringProperty("Type", PivotConstants.pivotSetPosition::toString, null); - sendableBuilder.addDoubleProperty("Position", PivotConstants.pivotSetPosition::getPosition, null); - }; - - SmartDashboard.putNumber("Pivot pos:", getPivotPosition()); - - } - - public double getPivotPosition() { - if (!pivotPositionStatusSignal.hasUpdated()) { - pivotPositionStatusSignal.waitForUpdate(PivotConstants.PIVOT_POSITION_STATUS_FRAME); - } - - return pivotPositionStatusSignal.getValue(); - } - - private void setPivotPosition(PivotConstants.PivotPosition pivotPosition) { - PivotConstants.pivotSetPosition = pivotPosition; - - MotionMagicVoltage motionMagicControlRequest = new MotionMagicVoltage( - pivotPosition.getPosition(), true, 0, 0, - false, false, false) - .withLimitForwardMotion(getForwardLimitSwitch()) - .withLimitReverseMotion(getReverseLimitSwitch()) - .withSlot(0).withUpdateFreqHz(200); - - pivotMotor.setControl(motionMagicControlRequest); - } - - public Command movePivotPositionTo(PivotConstants.PivotPosition pivotPosition) { - return runOnce(() -> setPivotPosition(pivotPosition)); - } - - public Command adjustPivotPositionTo(double angle) { - return runOnce(() -> setPivotPosition(PivotConstants.PivotPosition.CUSTOM(angle))); - } - - public Command updatePivotPositionWith(RobotDataPublisher shooterDataPublisher) { - RobotDataSubscription shooterStateSubscription = shooterDataPublisher.subscribeWith(data -> setPivotPosition(PivotConstants.PivotPosition.CUSTOM(data.angle))); - - return startEnd(shooterStateSubscription::start, shooterStateSubscription::cancel); - } - - public double getEncoderAngle() { - return pivotEncoder.getAbsolutePosition().getValue(); - } - - public Command moveUp(double speed) { - return startEnd(() -> movePivotMotorUp(speed), this::stopPivotMotor); - } - - public Command moveDown(double speed) { - return startEnd(() -> movePivotMotorDown(speed), this::stopPivotMotor); - } - - public Command moveDownWithBrake(double moveSpeed, double brakeSpeed) { - return startEnd(() -> movePivotMotorDown(moveSpeed), () -> movePivotMotorUp(brakeSpeed)); - } - - public Command moveUpWithBrake(double moveSpeed, double brakeSpeed) { - return startEnd(() -> movePivotMotorUp(moveSpeed), () -> movePivotMotorDown(brakeSpeed)); - } - - private void movePivotMotorUp(double speed) { - if (!getForwardLimitSwitch()) { - pivotMotor.set(Math.abs(speed)); - } - } - - private void movePivotMotorDown(double speed) { - if (!getReverseLimitSwitch()) { - pivotMotor.set(-Math.abs(speed)); - } - } - - public void stopPivotMotor() { - pivotMotor.stopMotor(); - } - - public boolean getForwardLimitSwitch() { - return !forwardLimitSwitch.get(); - } - - public boolean getReverseLimitSwitch() { - return !reverseLimitSwitch.get(); - } -} - diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index f2ead9f..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,217 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.interpolation.InterpolatingTreeMap; -import edu.wpi.first.math.interpolation.InverseInterpolator; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.util.RobotDataPublisher; -import frc.robot.util.RobotDataPublisher.RobotDataSubscription; -import frc.robot.util.ShooterStateData; - -public class ShooterSubsystem extends SubsystemBase { - - private final TalonFX leftKraken; - private final TalonFX rightKraken; - private final DigitalInput beamBreak; - private final FeederSubsystem feederSubsystem; - - private final StatusSignal leftVel; - private final StatusSignal rightVel; - - MotionMagicVelocityVoltage motionMagicVelocityVoltage; - - public static class ShooterConstants { - public static ShooterModes shooterModes; - public static ShooterStatus shooterStatus; - - - public static final int SHOOTER_LEFT_MOTOR = 5; //id - public static final int SHOOTER_RIGHT_MOTOR = 15; //id - public static final CurrentLimitsConfigs SHOOTER_CURRENT_LIMIT = new CurrentLimitsConfigs().withSupplyCurrentLimitEnable(true). - withSupplyCurrentThreshold(55).withSupplyCurrentLimit(65).withSupplyTimeThreshold(0.1).withStatorCurrentLimitEnable(true).withStatorCurrentLimit(65); - public static final InvertedValue SHOOTER_INVERSION = InvertedValue.CounterClockwise_Positive; - - public static final double SHOOTER_P = 0.11;//0.043315 - public static final double SHOOTER_S = 0.25;//0.043315 - public static final double SHOOTER_I = 0.0; - public static final double SHOOTER_D = 0.0; - public static final double SHOOTER_V = 0.12; - public static final double MOTION_MAGIC_ACCELERATION = 0.1; - public static final double SHOOTER_STATUS_FRAME_SECONDS = 0.01; - - public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs(). - withKS(SHOOTER_S). - withKP(SHOOTER_P). - withKI(SHOOTER_I). - withKD(SHOOTER_D). - withKA(MOTION_MAGIC_ACCELERATION). - withKV(SHOOTER_V); - - public static final int BEAM_BREAK = 0; - - public static InterpolatingTreeMap SHOOTER_MAP() { - InterpolatingTreeMap map = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShooterStateData.interpolator); - // TODO: decrease angles by around 0.05 to tune - map.put(1.375, new ShooterStateData(0.5, 125)); - map.put(1.7, new ShooterStateData(.485, 125)); - map.put(2.0, new ShooterStateData(0.478, 125)); - map.put(2.3, new ShooterStateData(0.47, 125)); - map.put(2.65, new ShooterStateData(.465, 125)); - map.put(2.8, new ShooterStateData(.4625, 125)); - map.put(3.0, new ShooterStateData(0.46, 125)); - map.put(3.8, new ShooterStateData(0.443, 125)); - return map; - } - - public static InterpolatingTreeMap SHUTTLE_MAP() { - InterpolatingTreeMap map = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShooterStateData.interpolator); - map.put(8.0, new ShooterStateData(0.5, 125)); - return map; - } - } - - - private static class FeederSubsystem extends SubsystemBase { - private final TalonFX feederMotor; - public static final int SHOOTER_NEO = 16; - - public FeederSubsystem() { - feederMotor = new TalonFX(SHOOTER_NEO, "canivoreBus"); - } - - protected Command spinFeederAndStop(double speed) { - return startEnd(() -> feederMotor.set(speed), feederMotor::stopMotor); - } - } - - public ShooterSubsystem() { - leftKraken = new TalonFX(ShooterConstants.SHOOTER_LEFT_MOTOR, Constants.TalonFXConstants.CANIVORE_NAME); - rightKraken = new TalonFX(ShooterConstants.SHOOTER_RIGHT_MOTOR, Constants.TalonFXConstants.CANIVORE_NAME); - var rightMotorConfigurator = rightKraken.getConfigurator(); - var leftMotorConfigurator = leftKraken.getConfigurator(); - var rightMotorConfiguration = new TalonFXConfiguration(); - - rightMotorConfiguration.MotorOutput.Inverted = ShooterConstants.SHOOTER_INVERSION; - rightMotorConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast; - rightMotorConfiguration.CurrentLimits = ShooterConstants.SHOOTER_CURRENT_LIMIT; - rightMotorConfiguration.Slot0 = ShooterConstants.SLOT_0_CONFIGS; - leftKraken.setControl(new Follower(rightKraken.getDeviceID(), false)); - - ShooterConstants.shooterStatus = ShooterStatus.OFF; - ShooterConstants.shooterModes = ShooterModes.TRAP; - - leftVel = leftKraken.getVelocity(); - rightVel = rightKraken.getVelocity(); - - beamBreak = new DigitalInput(ShooterConstants.BEAM_BREAK); - - motionMagicVelocityVoltage = new MotionMagicVelocityVoltage(0); - - var motionMagicConfigs = rightMotorConfiguration.MotionMagic; - motionMagicConfigs.MotionMagicAcceleration = 400; - motionMagicConfigs.MotionMagicJerk = 4000; - rightMotorConfigurator.apply(rightMotorConfiguration); - leftMotorConfigurator.apply(rightMotorConfiguration); - - feederSubsystem = new FeederSubsystem(); - } - - @Override - public void periodic() { - leftVel.refresh(); - rightVel.refresh(); - - SmartDashboard.putData("shooter beam break", beamBreak); - SmartDashboard.putNumber("left rps:", leftVel.getValue()); - SmartDashboard.putNumber("right rps:", rightVel.getValue()); - } - - public boolean getBeamBreak() { - return !beamBreak.get(); - } - - public void stopShooter() { - rightKraken.stopMotor(); - leftKraken.stopMotor(); - ShooterConstants.shooterStatus = ShooterStatus.OFF; - } - - public void setDualVelocity(double leftVel, double rightVel) { - leftKraken.setControl(new VelocityVoltage(leftVel)); - leftKraken.setControl(motionMagicVelocityVoltage.withVelocity(leftVel)); - rightKraken.setControl(motionMagicVelocityVoltage.withVelocity(rightVel)); - } - - private void setVelocity(double velocity) { - setDualVelocity(velocity, velocity); - } - - public double getVelocity() { - if (!leftVel.hasUpdated() || !rightVel.hasUpdated()) { - leftVel.waitForUpdate(ShooterConstants.SHOOTER_STATUS_FRAME_SECONDS); - rightVel.waitForUpdate(ShooterConstants.SHOOTER_STATUS_FRAME_SECONDS); - } - - return (leftVel.getValue() + rightVel.getValue()) / 2; - } - - public Command updateVelocityWith(RobotDataPublisher shooterStateDataPublisher) { - RobotDataSubscription shooterStateDataSubscription = shooterStateDataPublisher.subscribeWith(data -> setVelocity(data.rps)); - - return startEnd(shooterStateDataSubscription::start, () -> { - shooterStateDataSubscription.cancel(); - stopShooter(); - }); - } - - public Command shooterBumpFire() { - return runOnce(() -> setDualVelocity(60, 100)); - } - - public Command shooterTrap() { - return startEnd(() -> setDualVelocity(35, 80), this::stopShooter); - } - - public Command setVelocityAndStop(double vel) { - return startEnd(() -> setVelocity(vel), this::stopShooter); - } - - public Command setVelocityContinuous(double vel) { - return runOnce(() -> setVelocity(vel)); - } - - public Command spinFeederAndStop(double vel) { - return feederSubsystem.spinFeederAndStop(vel); - } - - public Command spinFeederMaxAndStop() { - return spinFeederAndStop(-1); - } - - public enum ShooterModes { - DEFAULT, - SPEAKER, - AMP, - TRAP, - BUMP - } - - public enum ShooterStatus { - FORWARD, - BACKWARD, - OFF - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/ShooterStateData.java b/src/main/java/frc/robot/util/ShooterStateData.java deleted file mode 100644 index 071edd6..0000000 --- a/src/main/java/frc/robot/util/ShooterStateData.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.util; - -import edu.wpi.first.math.interpolation.Interpolator; - -public class ShooterStateData { - - public static final Interpolator interpolator = new Interpolator() { - @Override - public ShooterStateData interpolate( - ShooterStateData initialValue, ShooterStateData finalValue, double t) { - - double initialAngle = initialValue.angle; - double finalAngle = finalValue.angle; - double initialRPS = initialValue.rps; - double finalRPS = finalValue.rps; - - return new ShooterStateData( - initialAngle + t * (finalAngle - initialAngle), initialRPS + t * (finalRPS - initialRPS)); - - } - }; - - public final double angle; - public final double rps; - - public ShooterStateData(double angle, double rps) { - this.angle = angle; - this.rps = rps; - } -} \ No newline at end of file From 2766a6e53e2914d30d1bc175ee189e3607fb5d27 Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 13:13:49 +0000 Subject: [PATCH 3/8] Add intake back to handoff --- src/main/java/frc/robot/RobotCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index 074702d..c31411e 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -68,7 +68,7 @@ public Command handoff() { return arm.moveUpAndStop(.5).until(() -> arm.getEnc() >= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION) .andThen(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)) - .andThen(feeder.ingest(-1)); + .andThen(feeder.ingest(-0.3).alongWith(intake.rollOut(-0.15))); } public Command setAmp() { From 13da421dcd7e2dc3208734b306c7ebba28391809 Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 17:03:03 -0400 Subject: [PATCH 4/8] Add shooter classes (oops) --- .../subsystems/shooter/FeederSubsystem.java | 42 ++++ .../subsystems/shooter/FlywheelSubsystem.java | 138 +++++++++++ .../subsystems/shooter/PivotSubsystem.java | 225 ++++++++++++++++++ .../subsystems/shooter/ShooterTarget.java | 11 + .../subsystems/shooter/ShooterValues.java | 34 +++ 5 files changed, 450 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/FeederSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/FlywheelSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterTarget.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterValues.java diff --git a/src/main/java/frc/robot/subsystems/shooter/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/FeederSubsystem.java new file mode 100644 index 0000000..de1b5fe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/FeederSubsystem.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class FeederSubsystem extends SubsystemBase { + public static final int SHOOTER_NEO = 16; + public static final int BEAM_BREAK = 0; + + private final TalonFX feederMotor; + private final DigitalInput beamBreak; + + public FeederSubsystem() { + feederMotor = new TalonFX(SHOOTER_NEO, "canivoreBus"); + beamBreak = new DigitalInput(BEAM_BREAK); + } + + @Override + public void periodic() { + SmartDashboard.putData("shooter beam break", beamBreak); + } + + private boolean isBeamBroken() { + return !beamBreak.get(); + } + + private Command setSpeed(double speed) { + return startEnd(() -> feederMotor.set(speed), feederMotor::stopMotor); + } + + public Command ingest(double speed) { + return setSpeed(speed).until(this::isBeamBroken); + } + + public Command feed(double speed) { + return setSpeed(speed).until(() -> !isBeamBroken()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelSubsystem.java new file mode 100644 index 0000000..174ac67 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelSubsystem.java @@ -0,0 +1,138 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.util.RobotDataPublisher; +import frc.robot.util.RobotDataPublisher.RobotDataSubscription; + +import java.util.function.Supplier; + +public class FlywheelSubsystem extends SubsystemBase { + private final TalonFX leftKraken; + private final TalonFX rightKraken; + + private final StatusSignal leftVelSignal; + private final StatusSignal rightVelSignal; + + private static final MotionMagicVelocityVoltage motionMagicVelocityRequest = new MotionMagicVelocityVoltage(0); + + public static class ShooterConstants { + public static final int SHOOTER_LEFT_MOTOR = 5; //id + public static final int SHOOTER_RIGHT_MOTOR = 15; //id + public static final CurrentLimitsConfigs SHOOTER_CURRENT_LIMIT = new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentThreshold(55) + .withSupplyCurrentLimit(65) + .withSupplyTimeThreshold(0.1) + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(65); + public static final InvertedValue SHOOTER_INVERSION = InvertedValue.CounterClockwise_Positive; + + public static final double SHOOTER_P = 0.11;//0.043315 + public static final double SHOOTER_S = 0.25;//0.043315 + public static final double SHOOTER_I = 0.0; + public static final double SHOOTER_D = 0.0; + public static final double SHOOTER_V = 0.12; + public static final double MOTION_MAGIC_ACCELERATION = 0.1; + + public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs(). + withKS(SHOOTER_S). + withKP(SHOOTER_P). + withKI(SHOOTER_I). + withKD(SHOOTER_D). + withKA(MOTION_MAGIC_ACCELERATION). + withKV(SHOOTER_V); + + public static final int VELOCITY_THRESHOLD = 5; + } + + public FlywheelSubsystem() { + leftKraken = new TalonFX(ShooterConstants.SHOOTER_LEFT_MOTOR, Constants.TalonFXConstants.CANIVORE_NAME); + rightKraken = new TalonFX(ShooterConstants.SHOOTER_RIGHT_MOTOR, Constants.TalonFXConstants.CANIVORE_NAME); + + TalonFXConfigurator rightMotorConfigurator = rightKraken.getConfigurator(); + TalonFXConfigurator leftMotorConfigurator = leftKraken.getConfigurator(); + + TalonFXConfiguration talonConfig = new TalonFXConfiguration(); + talonConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + talonConfig.CurrentLimits = ShooterConstants.SHOOTER_CURRENT_LIMIT; + talonConfig.Slot0 = ShooterConstants.SLOT_0_CONFIGS; + talonConfig.MotorOutput.Inverted = ShooterConstants.SHOOTER_INVERSION; + + MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); + motionMagicConfigs.MotionMagicAcceleration = 400; + motionMagicConfigs.MotionMagicJerk = 4000; + + talonConfig.MotionMagic = motionMagicConfigs; + + leftMotorConfigurator.apply(talonConfig); + rightMotorConfigurator.apply(talonConfig); + + leftKraken.setControl(new Follower(rightKraken.getDeviceID(), false)); + + leftVelSignal = leftKraken.getVelocity(); + rightVelSignal = rightKraken.getVelocity(); + } + + @Override + public void periodic() { + leftVelSignal.refresh(); + rightVelSignal.refresh(); + + SmartDashboard.putNumber("left rps:", leftVelSignal.getValue()); + SmartDashboard.putNumber("right rps:", rightVelSignal.getValue()); + } + + private void stopShooter() { + rightKraken.stopMotor(); + } + + private void setVelocity(double velocity) { + rightKraken.setControl(motionMagicVelocityRequest.withVelocity(velocity)); + } + + private double getVelocity() { + return (leftVelSignal.getValue() + rightVelSignal.getValue()) / 2; + } + + private boolean reachedVelocity(double vel) { + return Math.abs(getVelocity() - vel) < ShooterConstants.VELOCITY_THRESHOLD; + } + + public Command reachVelocity(double vel) { + return startEnd(() -> setVelocity(vel), this::stopShooter) + .until(() -> reachedVelocity(vel)); + } + + public Command reachVelocity(Supplier vel) { + return runEnd(() -> setVelocity(vel.get()), this::stopShooter) + .until(() -> reachedVelocity(vel.get())); + } + + public enum ShooterModes { + DEFAULT, + SPEAKER, + AMP, + TRAP, + BUMP + } + + public enum ShooterStatus { + FORWARD, + BACKWARD, + OFF + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java new file mode 100644 index 0000000..9744eba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java @@ -0,0 +1,225 @@ +package frc.robot.subsystems.shooter; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.*; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; + +public class PivotSubsystem extends SubsystemBase { + public final TalonFX pivotMotor; + public final CANcoder pivotEncoder; + public DigitalInput forwardLimitSwitch; + public DigitalInput reverseLimitSwitch; + + private final StatusSignal pivotPositionStatusSignal; + private PivotConstants.PivotPosition setPosition; + + public static class PivotConstants { + public static final int PIVOT_LIMIT_SWITCH_FORWARD = 7; + public static final int PIVOT_LIMIT_SWITCH_REVERSE = 6; + public static final int PIVOT_ONE_MOTOR_ID = 29; // placeholder + public static final int PIVOT_ONE_CANCODER_ID = 16; // placeholder + public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake; + + public static final double PIVOT_POSITION_STATUS_FRAME = 0.05; + + public static final double PIVOT_P = 50; // 1 //350.0 /11.7 + public static final double PIVOT_I = 0; + public static final double PIVOT_D = 2; // 0 //45.0 + public static final double PIVOT_V = 0.12000000149011612; // 65 + public static final double PIVOT_A = 0.009999999776482582; // 0.7 + public static final double PIVOT_G = 0.02; // 0.35 + public static final double PROFILE_V = 0.000000001; + public static final double PROFILE_A = 0.000000001; + + public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs().withKP(PIVOT_P).withKI(PIVOT_I) + .withKD(PIVOT_D).withKA(PIVOT_A).withKV(PIVOT_V).withKG(PIVOT_G) + .withGravityType(GravityTypeValue.Arm_Cosine); + + public static final CurrentLimitsConfigs PIVOT_CURRENT_LIMIT = new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true).withSupplyCurrentThreshold(55).withSupplyCurrentLimit(65) + .withSupplyTimeThreshold(0.1).withStatorCurrentLimitEnable(true).withStatorCurrentLimit(65); + + public static final SoftwareLimitSwitchConfigs PIVOT_SOFT_LIMIT = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true).withForwardSoftLimitThreshold( + .53) + .withReverseSoftLimitEnable(true).withReverseSoftLimitThreshold( + .31); + + public static final double MAGNET_OFFSET = 0.433838; // placeholder + public static final double ANGLE_THRESHOLD = 0.01; + + public enum PivotPosition { + CUSTOM(-1) { + @Override + public double getPosition() { + if (super.getPosition() == -1) { + throw new IllegalArgumentException("Custom pivot position was not set"); + } + + return super.getPosition(); + } + }, + HANDOFF(0.5); + + // placeholder + private double position; + + PivotPosition(double position) { + this.position = position; + } + + public double getPosition() { + return position; + } + + public static PivotPosition CUSTOM(double position) { + PivotPosition pivotPosition = PivotPosition.CUSTOM; + pivotPosition.position = position; + return pivotPosition; + } + } + } + + public PivotSubsystem() { + reverseLimitSwitch = new DigitalInput(PivotConstants.PIVOT_LIMIT_SWITCH_REVERSE); + forwardLimitSwitch = new DigitalInput(PivotConstants.PIVOT_LIMIT_SWITCH_FORWARD); + pivotMotor = new TalonFX(PivotConstants.PIVOT_ONE_MOTOR_ID, Constants.TalonFXConstants.CANIVORE_NAME); + pivotEncoder = new CANcoder(PivotConstants.PIVOT_ONE_CANCODER_ID, Constants.TalonFXConstants.CANIVORE_NAME); + + pivotMotor.setInverted(true); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); + pivotPositionStatusSignal = pivotMotor.getPosition(); + + setPosition = PivotConstants.PivotPosition.CUSTOM(pivotPositionStatusSignal.getValue()); + + new Trigger(this::getForwardLimitSwitch).whileTrue(moveDown(0.05)); + new Trigger(this::getReverseLimitSwitch).whileTrue(moveUp(0.05)); + + var pivotMotorConfigurator = pivotMotor.getConfigurator(); + var talonFXConfiguration = new TalonFXConfiguration(); + + talonFXConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + talonFXConfiguration.Feedback.FeedbackRemoteSensorID = pivotEncoder.getDeviceID(); + talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + talonFXConfiguration.MotorOutput.NeutralMode = PivotConstants.PIVOT_NEUTRAL_MODE; + talonFXConfiguration.FutureProofConfigs = true; + talonFXConfiguration.Feedback.SensorToMechanismRatio = 1; + talonFXConfiguration.Feedback.RotorToSensorRatio = 83.79; + talonFXConfiguration.CurrentLimits = PivotConstants.PIVOT_CURRENT_LIMIT; + talonFXConfiguration.SoftwareLimitSwitch = PivotConstants.PIVOT_SOFT_LIMIT; + talonFXConfiguration.Slot0 = PivotConstants.SLOT_0_CONFIGS; + talonFXConfiguration.MotionMagic.MotionMagicCruiseVelocity = 2; + talonFXConfiguration.MotionMagic.MotionMagicAcceleration = 2; + talonFXConfiguration.MotionMagic.MotionMagicExpo_kA = PivotConstants.PROFILE_A; + talonFXConfiguration.MotionMagic.MotionMagicExpo_kV = PivotConstants.PROFILE_V; + + pivotMotorConfigurator.apply(talonFXConfiguration); + + var pivotEncoderConfigurator = pivotEncoder.getConfigurator(); + var cancoderConfiguration = new CANcoderConfiguration(); + + cancoderConfiguration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; + cancoderConfiguration.MagnetSensor.MagnetOffset = PivotConstants.MAGNET_OFFSET; + cancoderConfiguration.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + pivotEncoderConfigurator.apply(cancoderConfiguration); + } + + @Override + public void periodic() { + pivotPositionStatusSignal.refresh(); + + SmartDashboard.putNumber("Pivot pos:", getPosition()); + } + + public double getPosition() { + return pivotPositionStatusSignal.getValueAsDouble(); + } + + private boolean getForwardLimitSwitch() { + return !forwardLimitSwitch.get(); + } + + private boolean getReverseLimitSwitch() { + return !reverseLimitSwitch.get(); + } + + private void setPosition(PivotConstants.PivotPosition pivotPosition) { + setPosition = pivotPosition; + + MotionMagicVoltage motionMagicControlRequest = new MotionMagicVoltage( + pivotPosition.getPosition(), true, 0, 0, false, false, false) + .withLimitForwardMotion(getForwardLimitSwitch()) + .withLimitReverseMotion(getReverseLimitSwitch()) + .withSlot(0) + .withUpdateFreqHz(200); + + pivotMotor.setControl(motionMagicControlRequest); + } + + private void movePivotMotorUp(double speed) { + if (!getForwardLimitSwitch()) { + pivotMotor.set(Math.abs(speed)); + } + } + + private void movePivotMotorDown(double speed) { + if (!getReverseLimitSwitch()) { + pivotMotor.set(-Math.abs(speed)); + } + } + + private void stopPivotMotor() { + pivotMotor.stopMotor(); + } + + private boolean isAtSetPoint() { + return Math.abs(setPosition.getPosition() - getPosition()) < PivotConstants.ANGLE_THRESHOLD; + } + + private Command waitUntilSetPointReached() { + return Commands.waitUntil(this::isAtSetPoint); + } + + public Command movePivotPositionTo(PivotConstants.PivotPosition pivotPosition) { + return runOnce(() -> setPosition(pivotPosition)) + .andThen(waitUntilSetPointReached()); + } + + public Command updatePivotPositionWith(Supplier positionSupplier) { + return runEnd(() -> setPosition(PivotConstants.PivotPosition.CUSTOM(positionSupplier.get())), this::stopPivotMotor) + .andThen(waitUntilSetPointReached()); + } + + public Command adjustPivotPositionTo(double angle) { + return movePivotPositionTo(PivotConstants.PivotPosition.CUSTOM(angle)); + } + + public Command moveUp(double speed) { + return startEnd(() -> movePivotMotorUp(speed), this::stopPivotMotor); + } + + public Command moveDown(double speed) { + return startEnd(() -> movePivotMotorDown(speed), this::stopPivotMotor); + } + + public Command moveDownWithBrake(double moveSpeed, double brakeSpeed) { + return startEnd(() -> movePivotMotorDown(moveSpeed), () -> movePivotMotorUp(brakeSpeed)); + } + + public Command moveUpWithBrake(double moveSpeed, double brakeSpeed) { + return startEnd(() -> movePivotMotorUp(moveSpeed), () -> movePivotMotorDown(brakeSpeed)); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTarget.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTarget.java new file mode 100644 index 0000000..49fab70 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTarget.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.shooter; + +public class ShooterTarget { + public final double angle; + public final double rps; + + public ShooterTarget(double angle, double rps) { + this.angle = angle; + this.rps = rps; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterValues.java b/src/main/java/frc/robot/subsystems/shooter/ShooterValues.java new file mode 100644 index 0000000..63c9840 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterValues.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.Interpolator; +import edu.wpi.first.math.interpolation.InverseInterpolator; + +public class ShooterValues { + public static final Interpolator interpolator = (initialValue, finalValue, t) -> { + double initialAngle = initialValue.angle; + double finalAngle = finalValue.angle; + double initialRPS = initialValue.rps; + double finalRPS = finalValue.rps; + + return new ShooterTarget( + initialAngle + t * (finalAngle - initialAngle), initialRPS + t * (finalRPS - initialRPS)); + }; + + public static InterpolatingTreeMap SHOOTER_MAP = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), interpolator); + public static InterpolatingTreeMap SHUTTLE_MAP = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), interpolator); + + static { + // TODO: decrease angles by around 0.05 to tune + SHOOTER_MAP.put(1.375, new ShooterTarget(0.5, 125)); + SHOOTER_MAP.put(1.7, new ShooterTarget(.485, 125)); + SHOOTER_MAP.put(2.0, new ShooterTarget(0.478, 125)); + SHOOTER_MAP.put(2.3, new ShooterTarget(0.47, 125)); + SHOOTER_MAP.put(2.65, new ShooterTarget(.465, 125)); + SHOOTER_MAP.put(2.8, new ShooterTarget(.4625, 125)); + SHOOTER_MAP.put(3.0, new ShooterTarget(0.46, 125)); + SHOOTER_MAP.put(3.8, new ShooterTarget(0.443, 125)); + + SHUTTLE_MAP.put(8.0, new ShooterTarget(0.5, 125)); + } +} From 3ac890cecf0ab8432c687af54f6e8f025e6a2380 Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 17:03:20 -0400 Subject: [PATCH 5/8] Add shooter classes (oops) --- src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java index 9744eba..a919c0b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java @@ -32,8 +32,6 @@ public static class PivotConstants { public static final int PIVOT_ONE_CANCODER_ID = 16; // placeholder public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake; - public static final double PIVOT_POSITION_STATUS_FRAME = 0.05; - public static final double PIVOT_P = 50; // 1 //350.0 /11.7 public static final double PIVOT_I = 0; public static final double PIVOT_D = 2; // 0 //45.0 From d796b4c4550ec2dc1ef43bc00e4dae4d90d479a2 Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 19:58:37 -0400 Subject: [PATCH 6/8] Add back IntakeSubsystem --- src/main/java/frc/robot/RobotCommands.java | 5 ++++- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index c31411e..2999814 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.shooter.*; import frc.robot.util.AllianceFlipUtil; @@ -14,6 +15,7 @@ public class RobotCommands { + private final IntakeSubsystem intake; private final PivotSubsystem pivot; private final CommandSwerveDrivetrain commandSwerveDrivetrain; private final ArmSubsystem arm; @@ -21,7 +23,8 @@ public class RobotCommands { private final FeederSubsystem feeder; private final FlywheelSubsystem flywheel; - public RobotCommands(PivotSubsystem pivot, CommandSwerveDrivetrain commandSwerveDrivetrain, ArmSubsystem arm, ElevatorSubsystem elevator, FeederSubsystem feeder, FlywheelSubsystem flywheel) { + public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, CommandSwerveDrivetrain commandSwerveDrivetrain, ArmSubsystem arm, ElevatorSubsystem elevator, FeederSubsystem feeder, FlywheelSubsystem flywheel) { + this.intake = intake; this.pivot = pivot; this.commandSwerveDrivetrain = commandSwerveDrivetrain; this.arm = arm; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e252d32..67fc50d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,7 +58,7 @@ public class RobotContainer { private boolean autoNeedsRebuild = true; private Command auto; - private final RobotCommands robotCommands = new RobotCommands(pivot, drivetrain, arm, elevator, feeder, flywheel); + private final RobotCommands robotCommands = new RobotCommands(intake, pivot, drivetrain, arm, elevator, feeder, flywheel); public RobotContainer() { var field = new Field2d(); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index b1e855c..2cee82d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -19,7 +19,7 @@ public class IntakeSubsystem extends SubsystemBase { public class IntakeConstants { public static final int INTAKE_KRAKEN_ID = 8; - + public static final InvertedValue INTAKE_INVERSION = InvertedValue.Clockwise_Positive; public static final NeutralModeValue INTAKE_NEUTRAL_MODE = NeutralModeValue.Brake; public static final double INTAKE_POSITION_STATUS_FRAME = 0.05; From 2a1517e96c30c44c7704e0a53892b9b4c885dfd2 Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 20:22:39 -0400 Subject: [PATCH 7/8] Add robot commands --- src/main/java/frc/robot/RobotCommands.java | 6 +-- src/main/java/frc/robot/RobotContainer.java | 4 +- .../commands/auto/AutoNamedCommands.java | 4 +- .../subsystems/shooter/PivotSubsystem.java | 49 ++++--------------- 4 files changed, 16 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index 2999814..bea8510 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -34,7 +34,7 @@ public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, CommandSwerve } private Command updateTarget(Supplier shooterTarget) { - return pivot.updatePivotPositionWith(() -> shooterTarget.get().angle) + return pivot.adjustPivotPositionTo(() -> shooterTarget.get().angle) .alongWith(flywheel.reachVelocity(() -> shooterTarget.get().rps)); } @@ -70,7 +70,7 @@ public Command setShuttleState() { public Command handoff() { return arm.moveUpAndStop(.5).until(() -> arm.getEnc() >= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION) - .andThen(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)) + .andThen(pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)) .andThen(feeder.ingest(-0.3).alongWith(intake.rollOut(-0.15))); } @@ -80,7 +80,7 @@ public Command setAmp() { } public Command stowAmp() { - return pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen(elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN))); + return pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen(elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN))); } public Command bumpFire(){ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 67fc50d..dce8412 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,7 +102,7 @@ private void configureBindings() { buttonHelper.createButton(10, 0, robotCommands.handoff().withTimeout(2), MultiButton.RunCondition.WHEN_PRESSED); buttonHelper.createButton(2, 0, intake.rollOut(-.65), MultiButton.RunCondition.WHILE_HELD); - buttonHelper.createButton(4, 0, elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)).andThen(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)), MultiButton.RunCondition.WHEN_PRESSED); + buttonHelper.createButton(4, 0, elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)).andThen(pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)), MultiButton.RunCondition.WHEN_PRESSED); buttonHelper.createButton(6, 0, feeder.feed(-.1).alongWith(intake.rollIn(0.5)), MultiButton.RunCondition.WHILE_HELD); buttonHelper.createButton(9, 0, elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.AMP).andThen(pivot.moveDown(-0.25).unless( () -> pivot.getPosition() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))), MultiButton.RunCondition.WHEN_PRESSED); @@ -138,7 +138,7 @@ private void configureBindings() { joystick.rightBumper().whileTrue(intake.rollIn(.7)); // Arm down - joystick.leftBumper().onTrue(arm.deployArm(0.5).alongWith(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF))); + joystick.leftBumper().onTrue(arm.deployArm(0.5).alongWith(pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF))); // Amp Toggle joystick.a().onTrue(robotCommands.setAmp()); diff --git a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java index 6a582f3..1c9040f 100644 --- a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java @@ -30,7 +30,7 @@ public void registerCommands() { NamedCommands.registerCommand("rollOut", intakeSubsystem.rollOut(-0.7)); NamedCommands.registerCommand("armUp", armSubsystem.moveUpAndStop(.5).until(() -> armSubsystem.getEnc() >= 0.75)); NamedCommands.registerCommand("armDown", armSubsystem.moveDownAndStop(.5).until( - () -> armSubsystem.getEnc() <= .5 && armSubsystem.getEnc() >= .44).alongWith(pivotSubsystem.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF))); + () -> armSubsystem.getEnc() <= .5 && armSubsystem.getEnc() >= .44).alongWith(pivotSubsystem.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF))); NamedCommands.registerCommand("shootBump", robotCommands.setTargetAndShoot(0.53, 100)); NamedCommands.registerCommand("shootLine", robotCommands.setTargetAndShoot(0.47, 100)); @@ -45,6 +45,6 @@ public void registerCommands() { NamedCommands.registerCommand("handoff", robotCommands.handoff().withTimeout(1.5)); NamedCommands.registerCommand("handoffMidLine", intakeSubsystem.rollIn(.275).withTimeout(0.5).andThen(robotCommands.handoff().withTimeout(1.5))); - NamedCommands.registerCommand("pivotHandoff", pivotSubsystem.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)); + NamedCommands.registerCommand("pivotHandoff", pivotSubsystem.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java index a919c0b..95f577c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/PivotSubsystem.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; @@ -23,7 +22,6 @@ public class PivotSubsystem extends SubsystemBase { public DigitalInput reverseLimitSwitch; private final StatusSignal pivotPositionStatusSignal; - private PivotConstants.PivotPosition setPosition; public static class PivotConstants { public static final int PIVOT_LIMIT_SWITCH_FORWARD = 7; @@ -59,16 +57,7 @@ public static class PivotConstants { public static final double ANGLE_THRESHOLD = 0.01; public enum PivotPosition { - CUSTOM(-1) { - @Override - public double getPosition() { - if (super.getPosition() == -1) { - throw new IllegalArgumentException("Custom pivot position was not set"); - } - - return super.getPosition(); - } - }, + BUMP_FIRE(0.53), HANDOFF(0.5); // placeholder @@ -81,12 +70,6 @@ public double getPosition() { public double getPosition() { return position; } - - public static PivotPosition CUSTOM(double position) { - PivotPosition pivotPosition = PivotPosition.CUSTOM; - pivotPosition.position = position; - return pivotPosition; - } } } @@ -100,8 +83,6 @@ public PivotSubsystem() { pivotMotor.setNeutralMode(NeutralModeValue.Brake); pivotPositionStatusSignal = pivotMotor.getPosition(); - setPosition = PivotConstants.PivotPosition.CUSTOM(pivotPositionStatusSignal.getValue()); - new Trigger(this::getForwardLimitSwitch).whileTrue(moveDown(0.05)); new Trigger(this::getReverseLimitSwitch).whileTrue(moveUp(0.05)); @@ -154,11 +135,9 @@ private boolean getReverseLimitSwitch() { return !reverseLimitSwitch.get(); } - private void setPosition(PivotConstants.PivotPosition pivotPosition) { - setPosition = pivotPosition; - + private void setPosition(double pivotPosition) { MotionMagicVoltage motionMagicControlRequest = new MotionMagicVoltage( - pivotPosition.getPosition(), true, 0, 0, false, false, false) + pivotPosition, true, 0, 0, false, false, false) .withLimitForwardMotion(getForwardLimitSwitch()) .withLimitReverseMotion(getReverseLimitSwitch()) .withSlot(0) @@ -183,26 +162,16 @@ private void stopPivotMotor() { pivotMotor.stopMotor(); } - private boolean isAtSetPoint() { - return Math.abs(setPosition.getPosition() - getPosition()) < PivotConstants.ANGLE_THRESHOLD; - } - - private Command waitUntilSetPointReached() { - return Commands.waitUntil(this::isAtSetPoint); - } - - public Command movePivotPositionTo(PivotConstants.PivotPosition pivotPosition) { - return runOnce(() -> setPosition(pivotPosition)) - .andThen(waitUntilSetPointReached()); + public Command adjustPivotPositionTo(PivotConstants.PivotPosition pivotPosition) { + return adjustPivotPositionTo(pivotPosition.getPosition()); } - public Command updatePivotPositionWith(Supplier positionSupplier) { - return runEnd(() -> setPosition(PivotConstants.PivotPosition.CUSTOM(positionSupplier.get())), this::stopPivotMotor) - .andThen(waitUntilSetPointReached()); + public Command adjustPivotPositionTo(Supplier positionSupplier) { + return runEnd(() -> setPosition(positionSupplier.get()), this::stopPivotMotor); } - public Command adjustPivotPositionTo(double angle) { - return movePivotPositionTo(PivotConstants.PivotPosition.CUSTOM(angle)); + public Command adjustPivotPositionTo(double position) { + return runOnce(() -> setPosition(position)); } public Command moveUp(double speed) { From a4479e1a8720bed57f820231d7ef483b14bef3ad Mon Sep 17 00:00:00 2001 From: boomermath Date: Tue, 24 Sep 2024 20:38:33 -0400 Subject: [PATCH 8/8] Test pivot bump fire --- src/main/java/frc/robot/RobotCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index bea8510..3a0f7b4 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -84,6 +84,6 @@ public Command stowAmp() { } public Command bumpFire(){ - return setTargetAndShoot(0.53, 100); + return pivot.adjustPivotPositionTo(0.53); } }