diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d05ab05..947a2bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static final class Config { } public static final class Drive { - public static final int PIGEON_PORT = 0; // placeholder + public static final int PIGEON_PORT = 0; // FIXME public static final String SWERVE_CANBUS = "rio"; // placeholder // max voltage delivered to drivebase @@ -118,7 +118,8 @@ public static final class Drive { public static final class Dims { // FIXME validate with hardware public static final double TRACKWIDTH_METERS = - .5207; // 20.5 inches (source: cad) converted to meters + .5207; // 20.5 inches (source: cad) converted to + // meters public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square public static final double BUMPER_WIDTH_METERS_X = .9779; @@ -126,34 +127,34 @@ public static final class Dims { } /* - module layout: - |────── - |->│# ##steer motor - │ │ ##cancoder - │ │##drive motor - module number - - steer is always left - from corner perspective - - robot visualization: - |──────────────────────| - │2 10 04 1│ - │ 25 24 │ - │11 S D 03│ - │ D S │ - │ │ - │ │ - │ S D │ - │ D S │ - │12 |────────| 02│ - │ 26 │ │ 27 │ - │3 13│ batt │01 4│ - |──────┴───┬┬───┴──────| - ││ - ││ - ▼▼ - software front + * module layout: + * |────── + * |->│# ##steer motor + * │ │ ##cancoder + * │ │##drive motor + * module number + * + * steer is always left + * from corner perspective + * + * robot visualization: + * |──────────────────────| + * │2 10 04 1│ + * │ 25 24 │ + * │11 S D 03│ + * │ D S │ + * │ │ + * │ │ + * │ S D │ + * │ D S │ + * │12 |────────| 02│ + * │ 26 │ │ 27 │ + * │3 13│ batt │01 4│ + * |──────┴───┬┬───┴──────| + * ││ + * ││ + * ▼▼ + * software front */ public static final class Modules { @@ -241,9 +242,9 @@ public static final class Ports { } public static final class Modes { - public static final IntakePowers INTAKE = new IntakePowers(.95, .75); - public static final IntakePowers HOLD = new IntakePowers(0, 0d); - public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5); + public static final IntakePowers INTAKE = new IntakePowers(.95); + public static final IntakePowers HOLD = new IntakePowers(0); + public static final IntakePowers REVERSE = new IntakePowers(-.5); } public static final boolean IS_BEAMBREAK = true; @@ -251,43 +252,39 @@ public static final class Modes { public static final class Shooter { public static final class Ports { - public static final int TOP_SHOOTER_MOTOR_PORT = 20; - public static final int BOTTOM_SHOOTER_MOTOR_PORT = 19; + public static final int TOP_SHOOTER_MOTOR_PORT = 0; + public static final int BOTTOM_SHOOTER_MOTOR_PORT = 5; public static final int ACCELERATOR_MOTOR_PORT = 17; public static final int BEAM_BREAK_SENSOR_PORT = 8; } public static final class Modes { public static final ShooterSubsystem.ShooterPowers INTAKE = - new ShooterSubsystem.ShooterPowers(76, 1, .15); + new ShooterSubsystem.ShooterPowers(0, 1, 0, 0.5); public static final ShooterSubsystem.ShooterPowers IDLE = - new ShooterSubsystem.ShooterPowers(0, 0, 0); + new ShooterSubsystem.ShooterPowers(0, 0, 0, 0); + public static final ShooterSubsystem.ShooterPowers RAMP_AMP = + new ShooterSubsystem.ShooterPowers(0, 0, 0, 0); public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = - new ShooterSubsystem.ShooterPowers(76, 1, 0); - public static final ShooterSubsystem.ShooterPowers RAMP_AMP_BACK = - new ShooterSubsystem.ShooterPowers(25, 0.4, 0); - public static final ShooterSubsystem.ShooterPowers RAMP_AMP_FRONT = - new ShooterSubsystem.ShooterPowers(10, 2.5, 0); + new ShooterSubsystem.ShooterPowers(76, 1, 0, 0); public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = - new ShooterSubsystem.ShooterPowers(76, 1, .5); - public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = - new ShooterSubsystem.ShooterPowers(0, 1, 0); - public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_BACK = - new ShooterSubsystem.ShooterPowers(25, 0.4, .5); - public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_FORWARD = - new ShooterSubsystem.ShooterPowers(8, 2.5, .5); - public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = - new ShooterSubsystem.ShooterPowers(40, 1, 0); - public static final ShooterSubsystem.ShooterPowers SHUTTLE = - new ShooterSubsystem.ShooterPowers(30, 1, 0); + new ShooterSubsystem.ShooterPowers(76, 1, .5, 0); + public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = + new ShooterSubsystem.ShooterPowers(0, 0, 0, -1); + public static final ShooterSubsystem.ShooterPowers RAMP_SHUTTLE = + new ShooterSubsystem.ShooterPowers(30, 1, 0, 0); public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = - new ShooterSubsystem.ShooterPowers(30, 1, 0.5); + new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0); public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = - new ShooterSubsystem.ShooterPowers(76, 1, 0.5); + new ShooterSubsystem.ShooterPowers(76, 1, 0.5, 0); public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = - new ShooterSubsystem.ShooterPowers(30, 1, 0); + new ShooterSubsystem.ShooterPowers(30, 1, 0, 0); public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = - new ShooterSubsystem.ShooterPowers(30, 1, 0.5); + new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0); + public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER = + new ShooterSubsystem.ShooterPowers(0, 0, 0.5, 0.5); + public static final ShooterSubsystem.ShooterPowers UNLOAD_SHOOTER = + new ShooterSubsystem.ShooterPowers(0, 0, -0.5, -0.5); } public static final Slot0Configs ROLLER_PID_CONFIG = @@ -346,8 +343,9 @@ public static final class Setpoints { public static final int EPSILON = 2; public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250); - public static final double PIVOT_GEAR_RATIO = - (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder values + /** degrees per rotation */ + public static final double PIVOT_GEAR_RATIO = 5.33333; // not used + // values public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275; public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null); @@ -367,30 +365,32 @@ public static record VisionSource(String name, Transform3d robotToCamera) {} public static final List VISION_SOURCES = List.of( - /*new VisionSource( - "frontCam", - new Transform3d( - new Translation3d( - -0.305, // front/back - -0.2286, // left/right - -0.2159 // up/down - ), - new Rotation3d( - 0, - Math.toRadians(30), // angle up/down - Math.toRadians(180)))), - new VisionSource( - "backCam", - new Transform3d( - new Translation3d( - -0.2796, // front/back - -0.2286, // left/right - -0.2159 // up/down - ), - new Rotation3d( - 0, - Math.toRadians(30), // angle up/down - Math.toRadians(180)))),*/ + /* + * new VisionSource( + * "frontCam", + * new Transform3d( + * new Translation3d( + * -0.305, // front/back + * -0.2286, // left/right + * -0.2159 // up/down + * ), + * new Rotation3d( + * 0, + * Math.toRadians(30), // angle up/down + * Math.toRadians(180)))), + * new VisionSource( + * "backCam", + * new Transform3d( + * new Translation3d( + * -0.2796, // front/back + * -0.2286, // left/right + * -0.2159 // up/down + * ), + * new Rotation3d( + * 0, + * Math.toRadians(30), // angle up/down + * Math.toRadians(180)))), + */ new VisionSource( "backUpCam", new Transform3d( @@ -565,8 +565,27 @@ public static final class AutoAlign { public static final double AMP_TARGET_ANGLE = 0; public static final PathConstraints CONSTRAINTS = new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720)); + } + + public static class Elevator { + public static final class Ports { + public static final int ARM_MOTOR_PORT = 18; + public static final int ELEVATOR_MOTOR_PORT = 28; } - public static class Elevator{ //Placeholder elevator constant (update as needed) - public static final double GEAR_RATIO = 0; + /** inches per rotation */ + public static final double ELEVATOR_GEAR_RATIO = 0.0906065; + /** FIXME */ + public static final double MAX_HEIGHT = 50; + /** FIXME */ + public static final double ELEVATOR_FEEDFORWARD = 0.01; + /** FIXME */ + public static final double AMP_HEIGHT = 20; + + public static final class Arm { + /** FIXME */ + public static final double MAX_ANGLE = 50; + /** inches per rotation */ + public static final double ARM_GEAR_RATIO = 0.0231481481; } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11fb263..951ecd9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,12 +33,14 @@ import frc.robot.autonomous.HeadingAngle; import frc.robot.autonomous.HeadingTargetLock; import frc.robot.commands.AccelNoteCommand; -import frc.robot.commands.AdvancedIntakeCommand; +import frc.robot.commands.AmpPreparationCommand; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; +import frc.robot.commands.DrivebaseTargetLockCommand; import frc.robot.commands.HaltDriveCommandsCommand; import frc.robot.commands.IntakeCommand; -import frc.robot.commands.MaintainShooterCommand; +import frc.robot.commands.LoadShooterCommand; +import frc.robot.commands.ManualElevatorCommand; import frc.robot.commands.OuttakeCommand; import frc.robot.commands.PivotAngleCommand; import frc.robot.commands.PivotManualCommand; @@ -47,21 +49,18 @@ import frc.robot.commands.RotateAngleDriveCommand; import frc.robot.commands.RotateVectorDriveCommand; import frc.robot.commands.RotateVelocityDriveCommand; -import frc.robot.commands.SetRampModeCommand; import frc.robot.commands.ShootCommand; -import frc.robot.commands.ShooterRampUpCommand; import frc.robot.commands.StopIntakeCommand; import frc.robot.commands.StopShooterCommand; -import frc.robot.commands.TargetLockCommand; import frc.robot.commands.VibrateHIDCommand; import frc.robot.subsystems.CANWatchdogSubsystem; import frc.robot.subsystems.DrivebaseSubsystem; +import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.NetworkWatchdogSubsystem; import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.RGBSubsystem; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; import frc.robot.subsystems.VisionSubsystem; import frc.util.ControllerUtil; import frc.util.Layer; @@ -92,6 +91,8 @@ public class RobotContainer { private final RGBSubsystem rgbSubsystem = new RGBSubsystem(); + private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + private final NetworkWatchdogSubsystem networkWatchdogSubsystem = new NetworkWatchdogSubsystem(Optional.of(rgbSubsystem)); @@ -104,7 +105,7 @@ public class RobotContainer { /** controller 0 */ private final CommandXboxController anthony = new CommandXboxController(0); /** controller 0 layer */ - // private final Layer anthonyLayer = new Layer(anthony.rightBumper()); + // private final Layer anthonyLayer = new Layer(anthony.rightBumper()); /** the sendable chooser to select which auto to run. */ private final SendableChooser autoSelector; @@ -126,12 +127,9 @@ public RobotContainer() { // reigster commands for pathplanner NamedCommands.registerCommand( - "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)); + "IntakeCommand", + new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)); NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem)); - NamedCommands.registerCommand( - "ShooterRampUpCommand", - new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)); - NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem)); NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55)); NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38)); NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40)); @@ -143,13 +141,13 @@ public RobotContainer() { NamedCommands.registerCommand( "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17)); NamedCommands.registerCommand( - "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem)); + "AngleDriveBase", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17)); NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem)); NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0)); NamedCommands.registerCommand( "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)); NamedCommands.registerCommand( - "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0)); + "AutoDrivebaseAngle", new DrivebaseTargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0)); NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem)); NamedCommands.registerCommand( "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope())); @@ -229,7 +227,7 @@ public RobotContainer() { translationXSupplier, translationYSupplier, // anthony.rightBumper(), - anthony.leftBumper())); + () -> false)); rgbSubsystem.setDefaultCommand( new RGBCommand( @@ -241,7 +239,7 @@ public RobotContainer() { visionSubsystem)); // pivotSubsystem.setDefaultCommand( - // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY())); + // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY())); // Configure the button bindings configureButtonBindings(); @@ -262,7 +260,7 @@ public RobotContainer() { if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) { driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity); driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString()); - driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError()); + driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getCurrentError()); driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError()); } @@ -340,11 +338,15 @@ private void configureButtonBindings() { // INTAKE anthony .leftBumper() - .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)); + .onTrue( + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)); jacob .leftBumper() - .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)); + .onTrue( + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)); // SHOOT anthony @@ -353,9 +355,12 @@ private void configureButtonBindings() { new AccelNoteCommand(shooterSubsystem) .andThen(new ShootCommand(shooterSubsystem)) .andThen( - new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem))); + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))); // SHOOT OVERRIDE + + // FIXME should be right trigger is move elevator up, left trigger is move elevator down jacob .rightTrigger() .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem))); @@ -365,38 +370,23 @@ private void configureButtonBindings() { jacob .y() .whileTrue( - new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier) + new DrivebaseTargetLockCommand( + drivebaseSubsystem, translationXSupplier, translationYSupplier) .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem))); - jacob - .a() - .onTrue( - new RotateAngleDriveCommand( - drivebaseSubsystem, - translationXSupplier, - translationYSupplier, - DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40) - .alongWith(new PivotAngleCommand(pivotSubsystem, 60)) - .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.SHUTTLE))); - - jacob - .povDown() - .onTrue( - new PivotAngleCommand(pivotSubsystem, 15) - .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER))); - - // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem, translationXSupplier, - // translationYSupplier, Setpoints.SPEAKER)); + DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY()); - // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY()); + new Trigger(() -> pivotManualRate.getAsDouble() > 0.07) + .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate)); - // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07) - // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate)); + DoubleSupplier elevatorDownSupplier = () -> modifyAxis(-jacob.getLeftTriggerAxis()); + DoubleSupplier elevatorUpSupplier = () -> modifyAxis(jacob.getLeftTriggerAxis()); - DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY()); + new Trigger(() -> elevatorDownSupplier.getAsDouble() < -0.07) + .onTrue(new ManualElevatorCommand(elevatorDownSupplier, elevatorSubsystem)); - new Trigger(() -> Math.abs(pivotManualRate.getAsDouble()) > 0.07) - .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate)); + new Trigger(() -> elevatorUpSupplier.getAsDouble() > 0.07) + .onTrue(new ManualElevatorCommand(elevatorUpSupplier, elevatorSubsystem)); // SOURCE anthony @@ -410,44 +400,27 @@ private void configureButtonBindings() { ? -Setpoints.SOURCE_DEGREES : Setpoints.SOURCE_DEGREES) .alongWith( - new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem))); + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))); - // SPEAKER FROM STAGE - anthony - .b() - .onTrue( - new RotateAngleDriveCommand( - drivebaseSubsystem, - translationXSupplier, - translationYSupplier, - DriverStation.getAlliance().get().equals(Alliance.Red) - ? -Setpoints.SPEAKER_DEGREES - : Setpoints.SPEAKER_DEGREES) - .alongWith(new PivotAngleCommand(pivotSubsystem, 25.1))); + // NOTE TO SHOOTER OR SERIALIZER + anthony.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)); - // AMP + jacob.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)); jacob - .b() + .a() .onTrue( new RotateAngleDriveCommand( drivebaseSubsystem, translationXSupplier, translationYSupplier, - DriverStation.getAlliance().get().equals(Alliance.Red) ? -90 : 90) - .alongWith(new PivotAngleCommand(pivotSubsystem, 50)) // FIXME idk - .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_AMP_BACK))); - - /* jacob - .a() - .onTrue( - new RotateAngleDriveCommand( - drivebaseSubsystem, - translationXSupplier, - translationYSupplier, - DriverStation.getAlliance().get().equals(Alliance.Red) ? 90 : -90) - .alongWith(new PivotAngleCommand(pivotSubsystem, 138)) // FIXME idk - .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_AMP_FRONT))); - */ + DriverStation.getAlliance().get().equals(Alliance.Red) + ? -50 + : 50) // FIXME confirm angles + .alongWith( + new AmpPreparationCommand( + pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem))); + // SPEAKER FROM SUBWOOFER anthony .a() @@ -463,8 +436,25 @@ private void configureButtonBindings() { drivebaseSubsystem, translationXSupplier, translationYSupplier, - DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) - .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1))); + DriverStation.getAlliance().get().equals(Alliance.Red) + ? -50 + : 50) // FIXME confirm angles + .alongWith( + new AmpPreparationCommand( + pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem))); + + // PIVOT SETPOINTS + anthony.povUp().onTrue(new PivotAngleCommand(pivotSubsystem, 30)); + + anthony.povLeft().onTrue(new PivotAngleCommand(pivotSubsystem, 60)); + + anthony.povRight().onTrue(new PivotAngleCommand(pivotSubsystem, 75)); + + anthony.povDown().onTrue(new PivotAngleCommand(pivotSubsystem, 55)); + + // TRAP CLIMB STUFF + // jacob. + // povCenter().onTrue(); DoubleSupplier rotation = exponential( @@ -483,7 +473,7 @@ private void configureButtonBindings() { translationXSupplier, translationYSupplier, rotationVelocity, - anthony.rightBumper())); + () -> false)); new Trigger( () -> @@ -496,7 +486,7 @@ private void configureButtonBindings() { translationYSupplier, anthony::getRightY, anthony::getRightX, - anthony.rightBumper())); + () -> false)); } /** diff --git a/src/main/java/frc/robot/commands/AccelNoteCommand.java b/src/main/java/frc/robot/commands/AccelNoteCommand.java index 265dc61..86b95c6 100644 --- a/src/main/java/frc/robot/commands/AccelNoteCommand.java +++ b/src/main/java/frc/robot/commands/AccelNoteCommand.java @@ -23,6 +23,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return shooterSubsystem.isReadyToShoot(); + return shooterSubsystem.isReadyToShootSpeaker(); } } diff --git a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java deleted file mode 100644 index 22b1606..0000000 --- a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; - -/** A complex auto command that drives forward, releases a hatch, and then drives backward. */ -public class AdvancedIntakeCommand extends SequentialCommandGroup { - - public AdvancedIntakeCommand( - IntakeSubsystem intakeSubsystem, - ShooterSubsystem shooterSubsystem, - PivotSubsystem pivotSubsystem) { - addCommands( - new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem), - new ParallelCommandGroup( - new OuttakeCommand(intakeSubsystem) - .withTimeout(3) - .andThen(new StopIntakeCommand(intakeSubsystem)), - new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER))); - } -} diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java new file mode 100644 index 0000000..6fda043 --- /dev/null +++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.Elevator; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class AmpPreparationCommand extends SequentialCommandGroup { + /** Creates a new AmpPreparationCommand. */ + public AmpPreparationCommand( + PivotSubsystem pivotSubsystem, + ElevatorSubsystem elevatorSubsystem, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem) { + if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) { + addCommands(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)); + } else { + addCommands( + new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem) + .andThen( + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)) + .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT))); + } + } +} diff --git a/src/main/java/frc/robot/commands/ShooterRampUpCommand.java b/src/main/java/frc/robot/commands/ArmAngleCommand.java similarity index 50% rename from src/main/java/frc/robot/commands/ShooterRampUpCommand.java rename to src/main/java/frc/robot/commands/ArmAngleCommand.java index 618a720..66c18ed 100644 --- a/src/main/java/frc/robot/commands/ShooterRampUpCommand.java +++ b/src/main/java/frc/robot/commands/ArmAngleCommand.java @@ -4,31 +4,40 @@ package frc.robot.commands; +// import frc.robot.subsystems.ElevatorSubsystem; + import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; +import frc.robot.subsystems.ElevatorSubsystem; + +/** An example command that uses an example subsystem. */ +public class ArmAngleCommand extends Command { + + ElevatorSubsystem elevatorSubsystem; -public class ShooterRampUpCommand extends Command { - private ShooterSubsystem shooterSubsystem; - private ShooterMode mode; + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + double angle; - /** Creates a new ShooterRampUpCommand. */ - public ShooterRampUpCommand(ShooterSubsystem shooterSubsystem, ShooterMode mode) { + public ArmAngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) { + this.elevatorSubsystem = elevatorSubsystem; + this.angle = angle; // Use addRequirements() here to declare subsystem dependencies. - this.shooterSubsystem = shooterSubsystem; - this.mode = mode; - addRequirements(shooterSubsystem); + addRequirements(elevatorSubsystem); } // Called when the command is initially scheduled. @Override - public void initialize() { - shooterSubsystem.setShooterMode(mode); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + + elevatorSubsystem.setTargetAngle(angle); + } // Called once the command ends or is interrupted. @Override @@ -37,6 +46,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return elevatorSubsystem.atTargetAngle(); } } diff --git a/src/main/java/frc/robot/commands/AutoAlignCommand.java b/src/main/java/frc/robot/commands/AutoAlignCommand.java deleted file mode 100644 index 5f08948..0000000 --- a/src/main/java/frc/robot/commands/AutoAlignCommand.java +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DrivebaseSubsystem; - -public class AutoAlignCommand extends Command { - DrivebaseSubsystem drivebaseSubsystem; - /** Creates a new AutoAlignCommand. */ - public AutoAlignCommand( - DrivebaseSubsystem drivebaseSubsystem, AutoBuilder autoBuilder, Pose2d targetPose) { - this.drivebaseSubsystem = drivebaseSubsystem; - addRequirements(drivebaseSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - drivebaseSubsystem.setDefenseMode(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/TargetLockCommand.java b/src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java similarity index 96% rename from src/main/java/frc/robot/commands/TargetLockCommand.java rename to src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java index 16b5e47..e3796d8 100644 --- a/src/main/java/frc/robot/commands/TargetLockCommand.java +++ b/src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java @@ -17,7 +17,7 @@ * This command takes a drive angle and a target x,y coordinate, and snaps the robot to face the * target. This is useful to lock the robot to fixed target with a button. */ -public class TargetLockCommand extends Command { +public class DrivebaseTargetLockCommand extends Command { private final DrivebaseSubsystem drivebaseSubsystem; private final DoubleSupplier translationXSupplier; @@ -28,7 +28,7 @@ public class TargetLockCommand extends Command { private double targetAngle; /** Creates a new TargetLockCommand. */ - public TargetLockCommand( + public DrivebaseTargetLockCommand( DrivebaseSubsystem drivebaseSubsystem, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier) { diff --git a/src/main/java/frc/robot/commands/SetRampModeCommand.java b/src/main/java/frc/robot/commands/ElevatorHeightCommand.java similarity index 58% rename from src/main/java/frc/robot/commands/SetRampModeCommand.java rename to src/main/java/frc/robot/commands/ElevatorHeightCommand.java index 4a89c2e..b61bb8b 100644 --- a/src/main/java/frc/robot/commands/SetRampModeCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorHeightCommand.java @@ -4,24 +4,28 @@ package frc.robot.commands; +// import frc.robot.subsystems.ElevatorSubsystem; + import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; +import frc.robot.subsystems.ElevatorSubsystem; -public class SetRampModeCommand extends Command { - private ShooterSubsystem shooterSubsystem; - /** Creates a new SetRampModeCommand. */ - public SetRampModeCommand(ShooterSubsystem shooterSubsystem) { - // Use addRequirements() here to declare subsystem dependencies. - this.shooterSubsystem = shooterSubsystem; +/** An example command that uses an example subsystem. */ +public class ElevatorHeightCommand extends Command { - addRequirements(shooterSubsystem); + ElevatorSubsystem elevatorSubsystem; + double height; + + public ElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height) { + this.elevatorSubsystem = elevatorSubsystem; + this.height = height; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(elevatorSubsystem); } // Called when the command is initially scheduled. @Override public void initialize() { - shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER); + elevatorSubsystem.setTargetHeight(height); } // Called every time the scheduler runs while the command is scheduled. @@ -35,6 +39,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return elevatorSubsystem.atTargetHeight(); } } diff --git a/src/main/java/frc/robot/commands/HashMapCommand.java b/src/main/java/frc/robot/commands/HashMapCommand.java deleted file mode 100644 index fa0b241..0000000 --- a/src/main/java/frc/robot/commands/HashMapCommand.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import java.util.Map; -import java.util.function.Supplier; -import java.util.stream.Collectors; - -public class HashMapCommand extends Command { - private final Map commandMap; - private final Supplier keySupplier; - private Command selectedCommand; - private boolean runsWhenDisabled; - - /** - * Creates a new ConditionalCommand. - * - * @param onTrue the command to run if the condition is true - * @param onFalse the command to run if the condition is false - * @param condition the condition to determine which command to run - */ - public HashMapCommand(Map commandMap, Supplier keySupplier) { - this.commandMap = commandMap; - this.keySupplier = keySupplier; - - CommandScheduler.getInstance() - .registerComposedCommands(commandMap.values().toArray(new Command[0])); - - m_requirements.addAll( - commandMap.values().stream() - .flatMap(command -> command.getRequirements().stream()) - .collect(Collectors.toList())); - - runsWhenDisabled = commandMap.values().stream().allMatch(Command::runsWhenDisabled); - } - - @Override - public void initialize() { - var key = keySupplier.get(); - selectedCommand = commandMap.get(key); - if (selectedCommand == null) { - throw new IllegalArgumentException("No command for key " + key); - } - selectedCommand.initialize(); - } - - @Override - public void execute() { - selectedCommand.execute(); - } - - @Override - public void end(boolean interrupted) { - selectedCommand.end(interrupted); - } - - @Override - public boolean isFinished() { - return selectedCommand.isFinished(); - } - - @Override - public boolean runsWhenDisabled() { - return runsWhenDisabled; - } -} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 1453543..3176e01 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -5,8 +5,9 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.IntakeSubsystem.Modes; +import frc.robot.subsystems.IntakeSubsystem.IntakeMode; import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShooterMode; @@ -16,16 +17,19 @@ public class IntakeCommand extends Command { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; PivotSubsystem pivotSubsystem; + ElevatorSubsystem elevatorSubsystem; /** Creates a new IntakeCommand. */ public IntakeCommand( IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, - PivotSubsystem pivotSubsystem) { + PivotSubsystem pivotSubsystem, + ElevatorSubsystem elevatorSubsystem) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; this.pivotSubsystem = pivotSubsystem; - + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem); // Use addRequirements() here to declare subsystem dependencies. // addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem); } @@ -33,26 +37,28 @@ public IntakeCommand( // Called when the command is initially scheduled. @Override public void initialize() { - pivotSubsystem.setTargetDegrees(20); - intakeSubsystem.setIntakeMode(Modes.INTAKE); - shooterSubsystem.setShooterMode(ShooterMode.INTAKE); + elevatorSubsystem.setTargetHeight(0); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (elevatorSubsystem.atTargetHeight()) { + intakeSubsystem.setIntakeMode(IntakeMode.INTAKE); + shooterSubsystem.setShooterMode(ShooterMode.INTAKE); + } + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - intakeSubsystem.setIntakeMode(IntakeSubsystem.Modes.HOLD); - shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER); - shooterSubsystem.haltAccelerator(); + intakeSubsystem.setIntakeMode(IntakeSubsystem.IntakeMode.HOLD); + shooterSubsystem.setShooterMode(ShooterMode.IDLE); } // Returns true when the command should end. @Override public boolean isFinished() { - return shooterSubsystem.isBeamBreakSensorTriggered(); + return shooterSubsystem.isSerializerBeamBreakSensorTriggered(); } } diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java new file mode 100644 index 0000000..23b73d8 --- /dev/null +++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* EDIT THIS COMMAND the elevator needs time to realign with the pivot!!!! */ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.ShooterSubsystem.ShooterMode; + +public class LoadShooterCommand extends Command { + /** Creates a new LoadShooterCommand. */ + ShooterSubsystem shooterSubsystem; + + ElevatorSubsystem elevatorSubsystem; + PivotSubsystem pivotSubsystem; + + /** Use PivotAndElevatorTransferCommand before running */ + public LoadShooterCommand( + ShooterSubsystem shooterSubsystem, + PivotSubsystem pivotSubsystem, + ElevatorSubsystem elevatorSubsystem) { + this.shooterSubsystem = shooterSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + this.pivotSubsystem = pivotSubsystem; + addRequirements(elevatorSubsystem, shooterSubsystem, pivotSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevatorSubsystem.setTargetHeight(0); + pivotSubsystem.setTargetDegrees(20); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) { + shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return shooterSubsystem.isShooterBeamBreakSensorTriggered(); + } +} diff --git a/src/main/java/frc/robot/commands/MaintainShooterCommand.java b/src/main/java/frc/robot/commands/MaintainShooterCommand.java deleted file mode 100644 index c042b44..0000000 --- a/src/main/java/frc/robot/commands/MaintainShooterCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; - -public class MaintainShooterCommand extends Command { - ShooterSubsystem shooterSubsystem; - - public MaintainShooterCommand(ShooterSubsystem shooterSubsystem) { - this.shooterSubsystem = shooterSubsystem; - } - - @Override - public void initialize() { - shooterSubsystem.setShooterMode(ShooterMode.MAINTAIN_VELOCITY); - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ManualElevatorCommand.java b/src/main/java/frc/robot/commands/ManualElevatorCommand.java new file mode 100644 index 0000000..8f61e17 --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualElevatorCommand.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ElevatorSubsystem; +import java.util.function.DoubleSupplier; + +public class ManualElevatorCommand extends Command { + /** Creates a new ManualElevatorCommand. */ + DoubleSupplier directionSupplier; + + ElevatorSubsystem elevatorSubsystem; + + public ManualElevatorCommand( + DoubleSupplier directionSupplier, ElevatorSubsystem elevatorSubsystem) { + this.directionSupplier = directionSupplier; + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevatorSubsystem.setTargetHeight( + elevatorSubsystem.getElevatorPosition() + directionSupplier.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/OuttakeCommand.java b/src/main/java/frc/robot/commands/OuttakeCommand.java index f0386e7..0883a30 100644 --- a/src/main/java/frc/robot/commands/OuttakeCommand.java +++ b/src/main/java/frc/robot/commands/OuttakeCommand.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.IntakeSubsystem.Modes; +import frc.robot.subsystems.IntakeSubsystem.IntakeMode; public class OuttakeCommand extends Command { @@ -23,7 +23,7 @@ public OuttakeCommand(IntakeSubsystem intakeSubsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - intakeSubsystem.setIntakeMode(Modes.REVERSE); + intakeSubsystem.setIntakeMode(IntakeMode.REVERSE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/PivotAngleCommand.java b/src/main/java/frc/robot/commands/PivotAngleCommand.java index ba0a31e..60f47d9 100644 --- a/src/main/java/frc/robot/commands/PivotAngleCommand.java +++ b/src/main/java/frc/robot/commands/PivotAngleCommand.java @@ -36,6 +36,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return pivotSubsystem.isAtTargetDegrees(); + return pivotSubsystem.atTargetDegrees(); } } diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java index e9335d7..7af4188 100644 --- a/src/main/java/frc/robot/commands/RGBCommand.java +++ b/src/main/java/frc/robot/commands/RGBCommand.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.RGBSubsystem; import frc.robot.subsystems.RGBSubsystem.RGBMessage; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.ShooterSubsystem.ShooterMode; import frc.robot.subsystems.VisionSubsystem; import java.util.Optional; @@ -58,8 +58,8 @@ public void initialize() {} public void execute() { // two note = yellow - if (shooterSubsystem.isBeamBreakSensorTriggered() - && intakeSubsystem.isBeamBreakSensorTriggered() + if (shooterSubsystem.isShooterBeamBreakSensorTriggered() + && shooterSubsystem.isSerializerBeamBreakSensorTriggered() && twoNoteMsg.isEmpty()) { twoNoteMsg = Optional.of( @@ -67,15 +67,15 @@ public void execute() { Constants.Lights.Colors.YELLOW, RGBSubsystem.PatternTypes.PULSE, RGBSubsystem.MessagePriority.C_TWO_NOTE_WARNING)); - } else if (!(shooterSubsystem.isBeamBreakSensorTriggered() - && intakeSubsystem.isBeamBreakSensorTriggered())) { + } else if (!(shooterSubsystem.isShooterBeamBreakSensorTriggered() + && shooterSubsystem.isSerializerBeamBreakSensorTriggered())) { twoNoteMsg.ifPresent(RGBMessage::expire); twoNoteMsg = Optional.empty(); } // serializer = blue - if ((intakeSubsystem.isBeamBreakSensorTriggered() - || shooterSubsystem.isBeamBreakSensorTriggered()) + if ((shooterSubsystem.isShooterBeamBreakSensorTriggered() + || shooterSubsystem.isSerializerBeamBreakSensorTriggered()) && noteInRobotMsg.isEmpty()) { /*|| shooterSubsystem.isBeamBreakSensorTriggered()*/ noteInRobotMsg = @@ -84,15 +84,15 @@ public void execute() { Constants.Lights.Colors.WHITE, RGBSubsystem.PatternTypes.STROBE, RGBSubsystem.MessagePriority.F_NOTE_IN_ROBOT)); - } else if (!(intakeSubsystem.isBeamBreakSensorTriggered() - || shooterSubsystem.isBeamBreakSensorTriggered())) { + } else if (!(shooterSubsystem.isShooterBeamBreakSensorTriggered() + || shooterSubsystem.isSerializerBeamBreakSensorTriggered())) { noteInRobotMsg.ifPresent(RGBMessage::expire); noteInRobotMsg = Optional.empty(); } // ready to shoot = red if (isReadyToShootInSun() - && pivotSubsystem.isAtTargetDegrees() + && pivotSubsystem.atTargetDegrees() && Math.abs(drivebaseSubsystem.getAngularError()) < 2 && readyToShootMsg.isEmpty() && visionSubsystem.getCanSeeSpeakerTags()) { @@ -102,25 +102,24 @@ public void execute() { Constants.Lights.Colors.RED, RGBSubsystem.PatternTypes.STROBE, RGBSubsystem.MessagePriority.D_READY_TO_SHOOT)); - } else if (shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SPEAKER) || //if it shot the note, it is not ready to shoot - shooterSubsystem.getMode().equals(ShooterMode.SHOOT_AMP_BACK)|| //instead of relying on beam break sensor input - shooterSubsystem.getMode().equals(ShooterMode.SHOOT_AMP_FORWARD)|| - shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE)|| - shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR)|| - pivotSubsystem.isAtTargetDegrees()){ - readyToShootMsg.ifPresent(RGBMessage::expire); - readyToShootMsg = Optional.empty(); + } else if (shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SPEAKER) + || // if it shot the note, it is not ready to shoot instead of relying on beam break sensor + // input + shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE) + || shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR) + || pivotSubsystem.atTargetDegrees()) { + readyToShootMsg.ifPresent(RGBMessage::expire); + readyToShootMsg = Optional.empty(); } } - private boolean isReadyToShootInSun(){ - if(shooterSubsystem.isReadyToShoot()){ - sensorCounter +=1; + private boolean isReadyToShootInSun() { + if (shooterSubsystem.isReadyToShootSpeaker()) { + sensorCounter += 1; } - if (sensorCounter>=38){//waits 0.75 seconds to varify the note did not leave robot + if (sensorCounter >= 38) { // waits 0.75 seconds to varify the note did not leave robot return true; - } - else{ + } else { sensorCounter = 0; return false; } diff --git a/src/main/java/frc/robot/commands/ShootAmpCommand.java b/src/main/java/frc/robot/commands/ShootAmpCommand.java deleted file mode 100644 index aa7136e..0000000 --- a/src/main/java/frc/robot/commands/ShootAmpCommand.java +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.ShooterMode; - -public class ShootAmpCommand extends Command { - private ShooterSubsystem shooterSubsystem; - - /** Creates a new ShootAmp. */ - public ShootAmpCommand(ShooterSubsystem shooterSubsystem) { - // Use addRequirements() here to declare subsystem dependencies. - this.shooterSubsystem = shooterSubsystem; - addRequirements(shooterSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - if (shooterSubsystem.getMode().equals(ShooterMode.RAMP_AMP_BACK)) { - shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP_BACK); - } else { - shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP_FORWARD); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return !shooterSubsystem.isBeamBreakSensorTriggered(); - } -} diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index 6ceb2fa..238d9e8 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -37,6 +37,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return !shooterSubsystem.isBeamBreakSensorTriggered(); + return !shooterSubsystem.isShooterBeamBreakSensorTriggered(); } } diff --git a/src/main/java/frc/robot/commands/SpeakerLockCommand.java b/src/main/java/frc/robot/commands/SpeakerLockCommand.java index c0e54e5..63155db 100644 --- a/src/main/java/frc/robot/commands/SpeakerLockCommand.java +++ b/src/main/java/frc/robot/commands/SpeakerLockCommand.java @@ -7,7 +7,7 @@ public class SpeakerLockCommand extends ParallelCommandGroup { public SpeakerLockCommand(DrivebaseSubsystem drivebaseSubsystem, PivotSubsystem pivotSubsystem) { addCommands( - new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0), + new DrivebaseTargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0), new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)); } } diff --git a/src/main/java/frc/robot/commands/StopIntakeCommand.java b/src/main/java/frc/robot/commands/StopIntakeCommand.java index 696f4c3..1f5a4fa 100644 --- a/src/main/java/frc/robot/commands/StopIntakeCommand.java +++ b/src/main/java/frc/robot/commands/StopIntakeCommand.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.IntakeSubsystem.Modes; +import frc.robot.subsystems.IntakeSubsystem.IntakeMode; public class StopIntakeCommand extends Command { @@ -23,7 +23,7 @@ public StopIntakeCommand(IntakeSubsystem intakeSubsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - intakeSubsystem.setIntakeMode(Modes.HOLD); + intakeSubsystem.setIntakeMode(IntakeMode.HOLD); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java new file mode 100644 index 0000000..92a11ed --- /dev/null +++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.ShooterSubsystem.ShooterMode; + +public class UnloadShooterCommand extends Command { + /** Creates a new UnloadShooterCommand. */ + ShooterSubsystem shooterSubsystem; + + PivotSubsystem pivotSubsystem; + ElevatorSubsystem elevatorSubsystem; + boolean pass; + + public UnloadShooterCommand( + ShooterSubsystem shooterSubsystem, + PivotSubsystem pivotSubsystem, + ElevatorSubsystem elevatorSubsystem) { + this.shooterSubsystem = shooterSubsystem; + this.pivotSubsystem = pivotSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem, shooterSubsystem, pivotSubsystem); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + pivotSubsystem.setTargetDegrees(20); + elevatorSubsystem.setTargetHeight(0); + pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) { + shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD); + } + if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) { + pass = true; + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooterSubsystem.setShooterMode(ShooterMode.IDLE); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return !shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == true; + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 786c04d..11be8cb 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -1,8 +1,9 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; +// import com.ctre.phoenix6.signals.NeutralModeValue; +// import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -13,55 +14,106 @@ public class ElevatorSubsystem extends SubsystemBase { - private TalonFX motor; - private PIDController controller; + private TalonFX armMotor; + private TalonFX elevatorMotor; + private PIDController elevatorController; + private PIDController armController; private double targetHeight; - private double motorPower; - private double currentHeight = 0; + private double targetAngle; + private double elevatorMotorPower; + private double armMotorPower; + private double currentHeight; + private double currentAngle; + private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); /** Creates a new ElevatorSubsystem. */ public ElevatorSubsystem() { - motor = new TalonFX(15); + elevatorMotor = new TalonFX(Elevator.Ports.ELEVATOR_MOTOR_PORT); + armMotor = new TalonFX(Elevator.Ports.ARM_MOTOR_PORT); - motor.clearStickyFaults(); + elevatorMotor.clearStickyFaults(); + armMotor.clearStickyFaults(); + elevatorMotor.setPosition(0); + armMotor.setPosition(0); - controller = new PIDController(0.4, 0, 0.0125); + // FIXME placeholder values + elevatorController = new PIDController(0.4, 0, 0.0125); + armController = new PIDController(0.2, 0, 0); - ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower); ElevatorTab.addNumber("Target Height", () -> this.targetHeight); - // ElevatorTab.addNumber("PID output", () -> this.controller); + ElevatorTab.addNumber("Target Angle", () -> this.targetAngle); ElevatorTab.addNumber("Current Height", () -> this.currentHeight); - ElevatorTab.add(controller); + ElevatorTab.add(elevatorController); + ElevatorTab.add(armController); targetHeight = 0; currentHeight = 0; + targetAngle = 0; + currentAngle = 0; + } + + public double getArmPosition() { + return armInchesToRotations(armMotor.getPosition().getValueAsDouble()); + } + + public double getElevatorPosition() { + return inchesToRotations(elevatorMotor.getPosition().getValueAsDouble()); } public static double inchesToRotations(double height) { - return height / Elevator.GEAR_RATIO; + return height / Elevator.ELEVATOR_GEAR_RATIO; } public static double rotationsToInches(double rotations) { - return rotations * Elevator.GEAR_RATIO; + return rotations * Elevator.ELEVATOR_GEAR_RATIO; + } + + public static double armInchesToRotations(double degrees) { + return degrees / Elevator.Arm.ARM_GEAR_RATIO; + } + + public static double rotationsToArmInches(double rotations) { + return rotations * Elevator.Arm.ARM_GEAR_RATIO; } public void setTargetHeight(double targetHeight) { - this.targetHeight = targetHeight; + this.targetHeight = MathUtil.clamp(targetHeight, Elevator.MAX_HEIGHT, 0); } - @Override - public void periodic() { - // This method will be called once per scheduler run - motorPower = controller.calculate(rotationsToInches(0), targetHeight); //Placeholder for rotations + public void setTargetAngle(double targetAngle) { + this.targetAngle = MathUtil.clamp(targetAngle, Elevator.Arm.MAX_ANGLE, 0); + } + public boolean atTargetAngle() { + // replace with epsilon equals + if (targetAngle - 0.5 <= getArmPosition() && getArmPosition() <= targetAngle + 0.5) { + return true; + } + return false; } - public boolean nearTargetHeight() { - if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) { + public boolean atTargetHeight() { + if (targetHeight - 0.5 <= getElevatorPosition() + && getElevatorPosition() <= targetHeight + 0.5) { return true; } return false; } + + public double calculateArmFeedforward() { + return 0; // FIXME + // using a constant force spring + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + elevatorMotorPower = elevatorController.calculate(getElevatorPosition(), targetHeight); + armMotorPower = armController.calculate(getArmPosition(), targetAngle); + elevatorMotor.setVoltage( + MathUtil.clamp(elevatorMotorPower + Elevator.ELEVATOR_FEEDFORWARD, -10, 10)); + armMotor.setVoltage(MathUtil.clamp(armMotorPower + calculateArmFeedforward(), -10, 10)); + } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 37d9314..059c142 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -16,30 +15,24 @@ public class IntakeSubsystem extends SubsystemBase { private final TalonFX intakeMotor; - private final TalonFX serializerMotor; private final ShuffleboardTab tab = Shuffleboard.getTab("Intake"); - private final DigitalInput noteSensor; - private Modes intakeMode; - private Modes pastMode; - private double timeSincePenaltyHazard; - private boolean pastPenalty; + private IntakeMode intakeMode; - public enum Modes { + public enum IntakeMode { INTAKE(Intake.Modes.INTAKE), HOLD(Intake.Modes.HOLD), REVERSE(Intake.Modes.REVERSE); - public final IntakePowers modePowers; + public final IntakePowers modePowers; // rename to modePower - private Modes(IntakePowers modePowers) { + private IntakeMode(IntakePowers modePowers) { this.modePowers = modePowers; } } - public record IntakePowers(double intakeSpeed, double serializerSpeed) { - public IntakePowers(double intakeSpeed, double serializerSpeed) { + public record IntakePowers(double intakeSpeed) { + public IntakePowers(double intakeSpeed) { this.intakeSpeed = intakeSpeed; - this.serializerSpeed = serializerSpeed; } } @@ -47,45 +40,29 @@ public IntakePowers(double intakeSpeed, double serializerSpeed) { public IntakeSubsystem() { intakeMotor = new TalonFX(Intake.Ports.INTAKE_MOTOR_PORT); - serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT); - noteSensor = new DigitalInput(Intake.Ports.INTAKE_SENSOR_PORT); intakeMotor.clearStickyFaults(); - serializerMotor.clearStickyFaults(); intakeMotor.setNeutralMode(NeutralModeValue.Brake); - serializerMotor.setNeutralMode(NeutralModeValue.Brake); intakeMotor.setInverted(true); - serializerMotor.setInverted(true); - intakeMode = Modes.HOLD; - - timeSincePenaltyHazard = 7; + intakeMode = IntakeMode.HOLD; if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) { tab.addDouble("intake voltage", () -> intakeMotor.getMotorVoltage().getValueAsDouble()); - tab.addDouble( - "Serializer motor voltage", () -> serializerMotor.getMotorVoltage().getValueAsDouble()); tab.addString("Current Mode", () -> intakeMode.toString()); - tab.addBoolean("Intake Sensor", this::isBeamBreakSensorTriggered); } } - public void setIntakeMode(Modes intakeMode) { + public void setIntakeMode(IntakeMode intakeMode) { this.intakeMode = intakeMode; } - public boolean isBeamBreakSensorTriggered() { - // if is triggered return true - return !noteSensor.get(); - } - - private Modes getIntakeMode() { + private IntakeMode getIntakeMode() { return intakeMode; } @Override public void periodic() { intakeMotor.set(intakeMode.modePowers.intakeSpeed); - serializerMotor.set(intakeMode.modePowers.serializerSpeed); } } diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java index 36b280a..5733376 100644 --- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java @@ -36,9 +36,8 @@ public class PivotSubsystem extends SubsystemBase { private double pidVoltageOutput; private double calculatedTargetDegrees; - private boolean inRange; private Pose2d pose; - private double distance; + private double distanceFromSpeaker; private GenericEntry debugTarget; private double pastDebugTarget = 0; @@ -57,6 +56,7 @@ public PivotSubsystem() { pivotMotor.setInverted(true); pivotMotor.clearStickyFaults(); pivotMotor.set(0); + pivotMotor.setPosition(0); pivotMotor.setNeutralMode(NeutralModeValue.Brake); @@ -69,14 +69,14 @@ public PivotSubsystem() { pivotTab.addNumber( "Current Motor Position", () -> pivotMotor.getPosition().getValueAsDouble()); pivotTab.addNumber("Current Pivot Angle", this::getCurrentAngle); - pivotTab.addBoolean("Is at target", this::isAtTargetDegrees); + pivotTab.addBoolean("Is at target", this::atTargetDegrees); pivotTab.addNumber("Motor Error", this::getCurrentError); pivotTab.addNumber("PID Error", pidController::getPositionError); pivotTab.addNumber("Target Degrees", this::getTargetDegrees); pivotTab.addNumber("Applied Voltage", () -> pivotMotor.getMotorVoltage().getValueAsDouble()); pivotTab.addDouble("PID Voltage Output", () -> pidVoltageOutput); pivotTab.addDouble("Calculated Target Angle", () -> calculatedTargetDegrees); - pivotTab.addDouble("Distance", () -> distance); + pivotTab.addDouble("Distance", () -> distanceFromSpeaker); pivotTab.add(pidController); debugTarget = pivotTab @@ -103,21 +103,17 @@ public double getTargetDegrees() { return targetDegrees; } - public boolean isAtTargetDegrees() { + public boolean atTargetDegrees() { return Util.epsilonEquals(getCurrentAngle(), targetDegrees, Pivot.EPSILON); } - public boolean isReadyToShoot() { - return isAtTargetDegrees() && inRange; - } - public void setTargetDegrees(double degrees) { this.targetDegrees = MathUtil.clamp(degrees, Setpoints.MINIMUM_ANGLE, Setpoints.MAXIMUM_ANGLE); listenToDebug = false; } private static double rotationsToDegrees(double rotations) { - return (rotations * 360); + return rotations * 360; } public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) { @@ -135,27 +131,24 @@ public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) { speakerX = Pivot.BLUE_SPEAKER_POSE.getX(); speakerY = Pivot.BLUE_SPEAKER_POSE.getY(); } - distance = + distanceFromSpeaker = (Math.sqrt(Math.pow((x - speakerX), 2) + Math.pow((y - speakerY), 2))) - Pivot.CENTER_OF_ROBOT_TO_BUMPER; targetDegrees = - -0.006073 * Math.pow(distance, 6) - + 0.167509 * Math.pow(distance, 5) - - 1.803043 * Math.pow(distance, 4) - + 9.309355 * Math.pow(distance, 3) - - 21.517994 * Math.pow(distance, 2) - + 6.8762 * distance + -0.006073 * Math.pow(distanceFromSpeaker, 6) + + 0.167509 * Math.pow(distanceFromSpeaker, 5) + - 1.803043 * Math.pow(distanceFromSpeaker, 4) + + 9.309355 * Math.pow(distanceFromSpeaker, 3) + - 21.517994 * Math.pow(distanceFromSpeaker, 2) + + 6.8762 * distanceFromSpeaker + 64.78029 - 2.7; } - public double getAngularError() { - return targetDegrees - getCurrentAngle(); - } - @Override public void periodic() { + /* debugging stuff */ double currentTarget = debugTarget.getDouble(23.5); if (currentTarget != pastDebugTarget) { @@ -170,6 +163,6 @@ public void periodic() { pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10); - pivotMotor.setVoltage(pidVoltageOutput); + pivotMotor.set(getFeedForward() + pidVoltageOutput); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b250a62..d70bec0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -15,15 +15,18 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.Config; +import frc.robot.Constants.Intake; import frc.robot.Constants.Shooter; import java.util.Map; public class ShooterSubsystem extends SubsystemBase { - private final TalonFX rollerMotorBottom; - private final TalonFX rollerMotorTop; - private final TalonFX acceleratorMotor; + private final TalonFX pivotShooterTopMotor; // pivotShooterTop + private final TalonFX pivotShooterBottomMotor; // pivotShooterBottom + private final TalonFX pivotAcceleratorMotor; // pivotAccelerator + private final TalonFX serializerMotor; // ampShooter - private DigitalInput noteSensor; + private DigitalInput shooterSensor; + private DigitalInput serializerSensor; private ShooterMode shooterMode; @@ -45,17 +48,16 @@ public enum ShooterMode { INTAKE(Shooter.Modes.INTAKE), IDLE(Shooter.Modes.IDLE), RAMP_SPEAKER(Shooter.Modes.RAMP_SPEAKER), - RAMP_AMP_BACK(Shooter.Modes.RAMP_AMP_BACK), - RAMP_AMP_FRONT(Shooter.Modes.RAMP_AMP_FRONT), SHOOT_SPEAKER(Shooter.Modes.SHOOT_SPEAKER), - SHOOT_AMP_BACK(Shooter.Modes.SHOOT_AMP_BACK), - SHOOT_AMP_FORWARD(Shooter.Modes.SHOOT_AMP_FORWARD), - MAINTAIN_VELOCITY(Shooter.Modes.MAINTAIN_VELOCITY), - SHUTTLE(Shooter.Modes.SHUTTLE), + SHOOT_AMP(Shooter.Modes.SHOOT_AMP), + SHUTTLE(Shooter.Modes.RAMP_SHUTTLE), SHOOT_SHUTTLE(Shooter.Modes.SHOOT_SHUTTLE), ACCEL_SECURE(Shooter.Modes.ACCEL_SECURE), VARIABLE_VELOCITY(Shooter.Modes.VARIABLE_VELOCITY), - SHOOT_VAR(Shooter.Modes.SHOOT_VAR); + SHOOT_VAR(Shooter.Modes.SHOOT_VAR), + LOAD_SHOOTER(Shooter.Modes.LOAD_SHOOTER), + SHOOTER_UNLOAD(Shooter.Modes.UNLOAD_SHOOTER), + RAMP_AMP(Shooter.Modes.RAMP_AMP); public final ShooterPowers shooterPowers; @@ -64,58 +66,65 @@ private ShooterMode(ShooterPowers shooterPowers) { } } - public record ShooterPowers(double roller, double topToBottomRatio, double accelerator) { - public ShooterPowers(double roller, double topToBottomRatio, double accelerator) { + public record ShooterPowers( + double roller, double topToBottomRatio, double accelerator, double serializerSpeed) { + public ShooterPowers( + double roller, double topToBottomRatio, double accelerator, double serializerSpeed) { this.roller = roller; this.topToBottomRatio = topToBottomRatio; this.accelerator = accelerator; + this.serializerSpeed = serializerSpeed; } } public ShooterSubsystem() { - rollerMotorTop = new TalonFX(Shooter.Ports.TOP_SHOOTER_MOTOR_PORT); - rollerMotorBottom = new TalonFX(Shooter.Ports.BOTTOM_SHOOTER_MOTOR_PORT); - acceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT); + pivotShooterBottomMotor = new TalonFX(Shooter.Ports.TOP_SHOOTER_MOTOR_PORT); + pivotShooterTopMotor = new TalonFX(Shooter.Ports.BOTTOM_SHOOTER_MOTOR_PORT); + pivotAcceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT); + serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT); - noteSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT); + shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT); + serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT); - // rollerMotorTop.getConfigurator().apply(new TalonFXConfiguration()); - // rollerMotorBottom.getConfigurator().apply(new TalonFXConfiguration()); - // acceleratorMotor.getConfigurator().apply(new TalonFXConfiguration()); + pivotShooterBottomMotor.clearStickyFaults(); + pivotAcceleratorMotor.clearStickyFaults(); + pivotShooterTopMotor.clearStickyFaults(); + serializerMotor.clearStickyFaults(); - rollerMotorTop.clearStickyFaults(); - acceleratorMotor.clearStickyFaults(); - rollerMotorBottom.clearStickyFaults(); + pivotShooterTopMotor.setControl(new Follower(pivotShooterBottomMotor.getDeviceID(), false)); - rollerMotorBottom.setControl(new Follower(rollerMotorTop.getDeviceID(), false)); + pivotAcceleratorMotor.setInverted(true); + pivotShooterTopMotor.setInverted(true); + pivotShooterBottomMotor.setInverted(true); + serializerMotor.setInverted(true); - acceleratorMotor.setInverted(true); - rollerMotorBottom.setInverted(true); - rollerMotorTop.setInverted(true); - - acceleratorMotor.setNeutralMode(NeutralModeValue.Brake); - rollerMotorTop.setNeutralMode(NeutralModeValue.Coast); - rollerMotorBottom.setNeutralMode(NeutralModeValue.Coast); + serializerMotor.setNeutralMode(NeutralModeValue.Brake); + pivotAcceleratorMotor.setNeutralMode(NeutralModeValue.Brake); + pivotShooterBottomMotor.setNeutralMode(NeutralModeValue.Coast); + pivotShooterTopMotor.setNeutralMode(NeutralModeValue.Coast); shooterMode = ShooterMode.IDLE; if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) { - shooterTab.addBoolean("Sensor Input", this::isBeamBreakSensorTriggered); + shooterTab.addBoolean("Sensor Input", this::isShooterBeamBreakSensorTriggered); shooterTab .addDouble( - "Top Roller Velocity (RPS)", () -> rollerMotorTop.getVelocity().getValueAsDouble()) + "Top Roller Velocity (RPS)", + () -> pivotShooterBottomMotor.getVelocity().getValueAsDouble()) .withWidget(BuiltInWidgets.kGraph) .withSize(2, 1); + shooterTab.addDouble( + "Serializer motor voltage", () -> serializerMotor.getMotorVoltage().getValueAsDouble()); shooterTab .addDouble( "Bottom Roller Velocity (RPS)", - () -> rollerMotorBottom.getVelocity().getValueAsDouble()) + () -> pivotShooterTopMotor.getVelocity().getValueAsDouble()) .withWidget(BuiltInWidgets.kGraph) .withSize(2, 1); shooterTab.addDouble( - "Top roller amps", () -> rollerMotorTop.getSupplyCurrent().getValueAsDouble()); + "Top roller amps", () -> pivotShooterBottomMotor.getSupplyCurrent().getValueAsDouble()); shooterTab.addDouble( - "Bottom roller amps", () -> rollerMotorBottom.getSupplyCurrent().getValueAsDouble()); + "Bottom roller amps", () -> pivotShooterTopMotor.getSupplyCurrent().getValueAsDouble()); shooterTab.addString("mode", () -> shooterMode.toString()); ampRollerRatioEntry = @@ -146,20 +155,23 @@ public ShooterMode getMode() { } public boolean isShooterUpToSpeed() { - return rollerMotorBottom.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD - && rollerMotorTop.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD; + return pivotShooterTopMotor.getVelocity().getValueAsDouble() + >= Shooter.SHOOTER_VELOCITY_THRESHOLD + && pivotShooterBottomMotor.getVelocity().getValueAsDouble() + >= Shooter.SHOOTER_VELOCITY_THRESHOLD; } - public boolean isBeamBreakSensorTriggered() { - return !noteSensor.get(); + public boolean isShooterBeamBreakSensorTriggered() { + return shooterSensor.get(); } - public boolean isReadyToShoot() { - return isBeamBreakSensorTriggered() && isShooterUpToSpeed(); + public boolean isSerializerBeamBreakSensorTriggered() { + // if is triggered return true + return serializerSensor.get(); } - public void haltAccelerator() { - acceleratorMotor.set(0); + public boolean isReadyToShootSpeaker() { + return isShooterBeamBreakSensorTriggered() && isShooterUpToSpeed(); } public void setShooterMode(ShooterMode shooterMode) { @@ -173,12 +185,8 @@ public void setVariableVelocity(double velocity) { public void advanceToShootMode() { switch (shooterMode) { - case RAMP_AMP_FRONT: - shooterMode = ShooterMode.SHOOT_AMP_FORWARD; - break; - case RAMP_AMP_BACK: - shooterMode = ShooterMode.SHOOT_AMP_BACK; - break; + case RAMP_AMP: + shooterMode = ShooterMode.SHOOT_AMP; case SHUTTLE: shooterMode = ShooterMode.SHOOT_SHUTTLE; break; @@ -197,22 +205,22 @@ public void periodic() { // In debug use all of the debug values for our mode d_topToBottomRatio = ampRollerRatioEntry.getDouble(1); d_ShooterSpeed = shooterSpeedEntry.getDouble(0); - rollerMotorBottom.setControl(velocityVoltageRequest.withVelocity(d_ShooterSpeed)); - rollerMotorTop.setControl( + pivotShooterTopMotor.setControl(velocityVoltageRequest.withVelocity(d_ShooterSpeed)); + pivotShooterBottomMotor.setControl( velocityVoltageRequest.withVelocity(d_ShooterSpeed * d_topToBottomRatio)); } else if (shooterMode == ShooterMode.VARIABLE_VELOCITY || shooterMode == ShooterMode.SHOOT_VAR) { - rollerMotorBottom.setControl(velocityVoltageRequest.withVelocity(variableVelocity)); - rollerMotorTop.setControl(velocityVoltageRequest.withVelocity(variableVelocity)); + pivotShooterTopMotor.setControl(velocityVoltageRequest.withVelocity(variableVelocity)); + pivotShooterBottomMotor.setControl(velocityVoltageRequest.withVelocity(variableVelocity)); } else { // Otherwise just use our current mode for the values - rollerMotorBottom.setControl( + pivotShooterTopMotor.setControl( velocityVoltageRequest.withVelocity(shooterMode.shooterPowers.roller())); - rollerMotorTop.setControl( + pivotShooterBottomMotor.setControl( velocityVoltageRequest.withVelocity( shooterMode.shooterPowers.roller() * shooterMode.shooterPowers.topToBottomRatio())); } - - acceleratorMotor.set(shooterMode.shooterPowers.accelerator()); + pivotAcceleratorMotor.set(shooterMode.shooterPowers.accelerator()); + serializerMotor.set(shooterMode.shooterPowers.serializerSpeed()); } } diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java deleted file mode 100644 index e69de29..0000000