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..1a76881 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 { @@ -244,6 +245,8 @@ 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 SHOOT_SPEAKER = new IntakePowers(0, 0.7); + public static final IntakePowers SHOOT_AMP = new IntakePowers(0, -1); } public static final boolean IS_BEAMBREAK = true; @@ -251,43 +254,43 @@ 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(76, 1, 0, 0); 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); + new ShooterSubsystem.ShooterPowers(76, 1, .5, 0); 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); + new ShooterSubsystem.ShooterPowers(0, 1, 0, 0); + public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = + new ShooterSubsystem.ShooterPowers(0, 0, 0, -1); public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = - new ShooterSubsystem.ShooterPowers(40, 1, 0); + new ShooterSubsystem.ShooterPowers(40, 1, 0, 0); public static final ShooterSubsystem.ShooterPowers SHUTTLE = - new ShooterSubsystem.ShooterPowers(30, 1, 0); + 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 SHOOTER_UNLOAD = + new ShooterSubsystem.ShooterPowers(0, 0, -0.5, -0.5); } public static final Slot0Configs ROLLER_PID_CONFIG = @@ -346,8 +349,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 +371,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 +571,25 @@ 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; + + 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..a96ef14 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,6 +37,7 @@ import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.HaltDriveCommandsCommand; +import frc.robot.commands.HeightCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.MaintainShooterCommand; import frc.robot.commands.OuttakeCommand; @@ -53,9 +54,11 @@ import frc.robot.commands.StopIntakeCommand; import frc.robot.commands.StopShooterCommand; import frc.robot.commands.TargetLockCommand; +import frc.robot.commands.TransferNoteCommand; 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; @@ -92,6 +95,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 +109,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; @@ -142,6 +147,8 @@ public RobotContainer() { "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90)); NamedCommands.registerCommand( "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17)); + NamedCommands.registerCommand( + "AngleDriveBase", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17)); NamedCommands.registerCommand( "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem)); NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem)); @@ -241,7 +248,7 @@ public RobotContainer() { visionSubsystem)); // pivotSubsystem.setDefaultCommand( - // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY())); + // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY())); // Configure the button bindings configureButtonBindings(); @@ -368,16 +375,17 @@ private void configureButtonBindings() { new TargetLockCommand(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 + // .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() @@ -385,17 +393,18 @@ private void configureButtonBindings() { new PivotAngleCommand(pivotSubsystem, 15) .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER))); - // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem, translationXSupplier, + // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem, + // translationXSupplier, // translationYSupplier, Setpoints.SPEAKER)); // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY()); // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07) - // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate)); + // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate)); DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY()); - new Trigger(() -> Math.abs(pivotManualRate.getAsDouble()) > 0.07) + new Trigger(() -> pivotManualRate.getAsDouble() > 0.07) .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate)); // SOURCE @@ -412,42 +421,16 @@ private void configureButtonBindings() { .alongWith( new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem))); - // SPEAKER FROM STAGE + // NOTE TO SHOOTER OR SERIALIZER 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))); + new TransferNoteCommand( + shooterSubsystem, intakeSubsystem, pivotSubsystem, elevatorSubsystem)); + + jacob.b().onTrue(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)); + jacob.a().onTrue(new ShootCommand(shooterSubsystem)); - // AMP - jacob - .b() - .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))); - */ // SPEAKER FROM SUBWOOFER anthony .a() @@ -464,7 +447,7 @@ private void configureButtonBindings() { translationXSupplier, translationYSupplier, DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) - .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1))); + .alongWith(new HeightCommand(elevatorSubsystem, 20))); DoubleSupplier rotation = exponential( diff --git a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java index 22b1606..b0bdeab 100644 --- a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java +++ b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java @@ -17,9 +17,7 @@ public AdvancedIntakeCommand( addCommands( new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem), new ParallelCommandGroup( - new OuttakeCommand(intakeSubsystem) - .withTimeout(3) - .andThen(new StopIntakeCommand(intakeSubsystem)), + (new StopIntakeCommand(intakeSubsystem)), new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER))); } } diff --git a/src/main/java/frc/robot/commands/AngleCommand.java b/src/main/java/frc/robot/commands/AngleCommand.java new file mode 100644 index 0000000..66984cf --- /dev/null +++ b/src/main/java/frc/robot/commands/AngleCommand.java @@ -0,0 +1,51 @@ +// 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 frc.robot.subsystems.ElevatorSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ElevatorSubsystem; + +/** An example command that uses an example subsystem. */ +public class AngleCommand extends Command { + + ElevatorSubsystem elevatorSubsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + double angle; + + public AngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) { + this.elevatorSubsystem = elevatorSubsystem; + this.angle = angle; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(elevatorSubsystem); + } + + // 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.setTargetAngle(angle); + } + + // 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 elevatorSubsystem.nearTargetAngle(); + } +} diff --git a/src/main/java/frc/robot/commands/HeightCommand.java b/src/main/java/frc/robot/commands/HeightCommand.java new file mode 100644 index 0000000..434ffe1 --- /dev/null +++ b/src/main/java/frc/robot/commands/HeightCommand.java @@ -0,0 +1,51 @@ +// 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 frc.robot.subsystems.ElevatorSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ElevatorSubsystem; + +/** An example command that uses an example subsystem. */ +public class HeightCommand extends Command { + + ElevatorSubsystem elevatorSubsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + double height; + + public HeightCommand(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() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + elevatorSubsystem.setTargetHeight(height); + } + + // 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 elevatorSubsystem.nearTargetHeight(); + } +} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 1453543..ee4d0d0 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.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; import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShooterMode; @@ -25,7 +25,7 @@ public IntakeCommand( this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; this.pivotSubsystem = pivotSubsystem; - + addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem); // Use addRequirements() here to declare subsystem dependencies. // addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem); } @@ -34,7 +34,7 @@ public IntakeCommand( @Override public void initialize() { pivotSubsystem.setTargetDegrees(20); - intakeSubsystem.setIntakeMode(Modes.INTAKE); + intakeSubsystem.setIntakeMode(IntakeMode.INTAKE); shooterSubsystem.setShooterMode(ShooterMode.INTAKE); } @@ -45,14 +45,14 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - intakeSubsystem.setIntakeMode(IntakeSubsystem.Modes.HOLD); - shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER); + intakeSubsystem.setIntakeMode(IntakeSubsystem.IntakeMode.HOLD); + shooterSubsystem.setShooterMode(ShooterMode.IDLE); shooterSubsystem.haltAccelerator(); } // 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..3a8a8a4 --- /dev/null +++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java @@ -0,0 +1,53 @@ +// 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 LoadShooterCommand extends Command { + /** Creates a new LoadShooterCommand. */ + ShooterSubsystem shooterSubsystem; + + ElevatorSubsystem elevatorSubsystem; + PivotSubsystem pivotSubsystem; + + 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() { + shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER); + elevatorSubsystem.setTargetHeight(0); + pivotSubsystem.setTargetDegrees(20); + } + + // 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) { + 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/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/PivotManualCommand.java b/src/main/java/frc/robot/commands/PivotManualCommand.java index 67b3690..0bc78d3 100644 --- a/src/main/java/frc/robot/commands/PivotManualCommand.java +++ b/src/main/java/frc/robot/commands/PivotManualCommand.java @@ -19,7 +19,7 @@ public PivotManualCommand(PivotSubsystem pivotSubsystem, DoubleSupplier joystick @Override public void execute() { - pivotSubsystem.setTargetDegrees(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble()); + pivotSubsystem.setPower(joystickRate.getAsDouble()); } @Override diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java index e9335d7..85ff0e5 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,8 +84,8 @@ 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(); } @@ -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.isAtTargetDegrees()) { + readyToShootMsg.ifPresent(RGBMessage::expire); + readyToShootMsg = Optional.empty(); } } - private boolean isReadyToShootInSun(){ - if(shooterSubsystem.isReadyToShoot()){ - sensorCounter +=1; + private boolean isReadyToShootInSun() { + if (shooterSubsystem.isReadyToShoot()) { + 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 index aa7136e..a178b05 100644 --- a/src/main/java/frc/robot/commands/ShootAmpCommand.java +++ b/src/main/java/frc/robot/commands/ShootAmpCommand.java @@ -21,11 +21,7 @@ public ShootAmpCommand(ShooterSubsystem 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); - } + shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP); } // Called every time the scheduler runs while the command is scheduled. @@ -39,6 +35,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/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..dab0c0b 100644 --- a/src/main/java/frc/robot/commands/SpeakerLockCommand.java +++ b/src/main/java/frc/robot/commands/SpeakerLockCommand.java @@ -7,7 +7,8 @@ public class SpeakerLockCommand extends ParallelCommandGroup { public SpeakerLockCommand(DrivebaseSubsystem drivebaseSubsystem, PivotSubsystem pivotSubsystem) { addCommands( - new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0), - new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)); + new TargetLockCommand(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/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java new file mode 100644 index 0000000..a4ec6e1 --- /dev/null +++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class TransferNoteCommand extends SequentialCommandGroup { + + public TransferNoteCommand( + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + PivotSubsystem pivotSubsystem, + ElevatorSubsystem elevatorSubsystem) { + if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) { + addCommands(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)); + } else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()) { + addCommands( + new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem) + .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem))); + } + } +} 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..e8c18ec --- /dev/null +++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java @@ -0,0 +1,60 @@ +// 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() { + pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered(); + shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD); + pivotSubsystem.setTargetDegrees(20); + elevatorSubsystem.setTargetHeight(0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + 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..23f8f2e 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,100 @@ public class ElevatorSubsystem extends SubsystemBase { - private TalonFX motor; + private TalonFX armMotor; + private TalonFX elevatorMotor; private PIDController controller; 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); - ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower); ElevatorTab.addNumber("Target Height", () -> this.targetHeight); + ElevatorTab.addNumber("Target Angle", () -> this.targetAngle); // ElevatorTab.addNumber("PID output", () -> this.controller); ElevatorTab.addNumber("Current Height", () -> this.currentHeight); ElevatorTab.add(controller); 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 nearTargetAngle() { + 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) { + if (targetHeight - 0.5 <= getElevatorPosition() + && getElevatorPosition() <= targetHeight + 0.5) { return true; } return false; } + + public double calculateFeedforward() { + return 0; // FIXME + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + elevatorMotorPower = controller.calculate(getElevatorPosition(), targetHeight); + armMotorPower = controller.calculate(getArmPosition(), targetAngle); + elevatorMotor.setVoltage( + MathUtil.clamp(elevatorMotorPower + Elevator.ELEVATOR_FEEDFORWARD, -10, 10)); + armMotor.setVoltage(MathUtil.clamp(armMotorPower + calculateFeedforward(), -10, 10)); + } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 37d9314..214ed71 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,22 +15,22 @@ 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 IntakeMode intakeMode; + private IntakeMode pastMode; private double timeSincePenaltyHazard; private boolean pastPenalty; - public enum Modes { + public enum IntakeMode { INTAKE(Intake.Modes.INTAKE), HOLD(Intake.Modes.HOLD), - REVERSE(Intake.Modes.REVERSE); + REVERSE(Intake.Modes.REVERSE), + SHOOT_SPEAKER(Intake.Modes.SHOOT_SPEAKER), + SHOOT_AMP(Intake.Modes.SHOOT_AMP); public final IntakePowers modePowers; - private Modes(IntakePowers modePowers) { + private IntakeMode(IntakePowers modePowers) { this.modePowers = modePowers; } } @@ -47,45 +46,31 @@ 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; + intakeMode = IntakeMode.HOLD; timeSincePenaltyHazard = 7; 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..f727946 100644 --- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java @@ -40,6 +40,7 @@ public class PivotSubsystem extends SubsystemBase { private Pose2d pose; private double distance; private GenericEntry debugTarget; + private double power; private double pastDebugTarget = 0; private boolean listenToDebug = true; @@ -57,6 +58,7 @@ public PivotSubsystem() { pivotMotor.setInverted(true); pivotMotor.clearStickyFaults(); pivotMotor.set(0); + pivotMotor.setPosition(0); pivotMotor.setNeutralMode(NeutralModeValue.Brake); @@ -76,6 +78,7 @@ public PivotSubsystem() { pivotTab.addNumber("Applied Voltage", () -> pivotMotor.getMotorVoltage().getValueAsDouble()); pivotTab.addDouble("PID Voltage Output", () -> pidVoltageOutput); pivotTab.addDouble("Calculated Target Angle", () -> calculatedTargetDegrees); + pivotTab.addDouble("Power", () -> power); pivotTab.addDouble("Distance", () -> distance); pivotTab.add(pidController); debugTarget = @@ -96,7 +99,7 @@ public double getCurrentError() { } public double getCurrentAngle() { - return rotationsToDegrees(pivotCANcoder.getAbsolutePosition().getValueAsDouble()); + return rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()); } public double getTargetDegrees() { @@ -117,7 +120,7 @@ public void setTargetDegrees(double degrees) { } private static double rotationsToDegrees(double rotations) { - return (rotations * 360); + return rotations * Pivot.PIVOT_GEAR_RATIO; } public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) { @@ -153,6 +156,10 @@ public double getAngularError() { return targetDegrees - getCurrentAngle(); } + public void setPower(double power) { + this.power = power; + } + @Override public void periodic() { @@ -170,6 +177,6 @@ public void periodic() { pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10); - pivotMotor.setVoltage(pidVoltageOutput); + pivotMotor.set(getFeedForward() + power); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b250a62..66ab7e9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -15,6 +15,7 @@ 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; @@ -22,8 +23,10 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX rollerMotorBottom; private final TalonFX rollerMotorTop; private final TalonFX acceleratorMotor; + private final TalonFX serializerMotor; - private DigitalInput noteSensor; + private DigitalInput shooterSensor; + private DigitalInput serializerSensor; private ShooterMode shooterMode; @@ -45,17 +48,17 @@ 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), + SHOOT_AMP(Shooter.Modes.SHOOT_AMP), MAINTAIN_VELOCITY(Shooter.Modes.MAINTAIN_VELOCITY), SHUTTLE(Shooter.Modes.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.SHOOTER_UNLOAD), + RAMP_AMP(Shooter.Modes.RAMP_AMP); public final ShooterPowers shooterPowers; @@ -64,11 +67,14 @@ 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; } } @@ -76,8 +82,10 @@ 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); + 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()); @@ -86,13 +94,16 @@ public ShooterSubsystem() { rollerMotorTop.clearStickyFaults(); acceleratorMotor.clearStickyFaults(); rollerMotorBottom.clearStickyFaults(); + serializerMotor.clearStickyFaults(); rollerMotorBottom.setControl(new Follower(rollerMotorTop.getDeviceID(), false)); acceleratorMotor.setInverted(true); rollerMotorBottom.setInverted(true); rollerMotorTop.setInverted(true); + serializerMotor.setInverted(true); + serializerMotor.setNeutralMode(NeutralModeValue.Brake); acceleratorMotor.setNeutralMode(NeutralModeValue.Brake); rollerMotorTop.setNeutralMode(NeutralModeValue.Coast); rollerMotorBottom.setNeutralMode(NeutralModeValue.Coast); @@ -100,12 +111,14 @@ public ShooterSubsystem() { 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()) .withWidget(BuiltInWidgets.kGraph) .withSize(2, 1); + shooterTab.addDouble( + "Serializer motor voltage", () -> serializerMotor.getMotorVoltage().getValueAsDouble()); shooterTab .addDouble( "Bottom Roller Velocity (RPS)", @@ -150,12 +163,17 @@ public boolean isShooterUpToSpeed() { && rollerMotorTop.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD; } - public boolean isBeamBreakSensorTriggered() { - return !noteSensor.get(); + public boolean isShooterBeamBreakSensorTriggered() { + return true; + } + + public boolean isSerializerBeamBreakSensorTriggered() { + // if is triggered return true + return true; } public boolean isReadyToShoot() { - return isBeamBreakSensorTriggered() && isShooterUpToSpeed(); + return isShooterBeamBreakSensorTriggered() && isShooterUpToSpeed(); } public void haltAccelerator() { @@ -173,12 +191,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; @@ -213,6 +227,7 @@ public void periodic() { shooterMode.shooterPowers.roller() * shooterMode.shooterPowers.topToBottomRatio())); } - acceleratorMotor.set(shooterMode.shooterPowers.accelerator()); + acceleratorMotor.set(0); + serializerMotor.set(1); } } 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