diff --git a/networktables.json.bck b/networktables.json.bck index 9add20b..fe51488 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -1,466 +1 @@ -[ - { - "name": "/Preferences/auto/D", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/auto/P", - "type": "double", - "value": 0.4, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/auto/rotationP", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/auto/rotationD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/intake/positionkP", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/intake/positionkI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/intake/positionkD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/RPMkP", - "type": "double", - "value": 9e-05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/RPMkI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/RPMkD", - "type": "double", - "value": 0.001, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/RPMkF", - "type": "double", - "value": 0.0001975, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/PositionkP", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/PositionkI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/PositionkD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/kP", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/power", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/intake/intakePower", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/intake/intakePosition", - "type": "double", - "value": 100.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/intake/outtakePower", - "type": "double", - "value": -0.75, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/umbrella/Power", - "type": "double", - "value": 0.25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/Power", - "type": "double", - "value": 0.79, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/RPM", - "type": "double", - "value": 4000.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/Position", - "type": "double", - "value": 56.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/IndexPower", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/ShootingIndexPower", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/OuttakePower", - "type": "double", - "value": -0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/shootingPosition", - "type": "double", - "value": 65.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/OutdexPower", - "type": "double", - "value": -0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/preSpinDistanceM", - "type": "double", - "value": 5.842, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/UpPower", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/DownPower", - "type": "double", - "value": -0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/UpPosition", - "type": "double", - "value": 5.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/DownPosition", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/tuning_rpm", - "type": "double", - "value": 2500.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/tuning_Position", - "type": "double", - "value": 65.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/wingShotAngle", - "type": "double", - "value": 34.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/podiumShotAngle", - "type": "double", - "value": 52.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/subShotAngle", - "type": "double", - "value": 93.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/wingRPM", - "type": "double", - "value": 4000.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/podiumRPM", - "type": "double", - "value": 3200.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/subRPM", - "type": "double", - "value": 3200.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/highPower", - "type": "double", - "value": 78.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Direction Slew Rate", - "type": "double", - "value": 1.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Magnitude Slew Rate", - "type": "double", - "value": 3.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Rotational Slew Rate", - "type": "double", - "value": 6.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/turnTest/kP", - "type": "double", - "value": 0.014, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/turnTest/kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/turnTest/tolerance", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/autoCenterRingAngle", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/autoCenterRingRPM", - "type": "double", - "value": 3200.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/autoPodiumRingAngle", - "type": "double", - "value": 85.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/autoPodiumRingRPM", - "type": "double", - "value": 3200.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/autoAmpRingAngle", - "type": "double", - "value": 70.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/autoAmpRingRPM", - "type": "double", - "value": 3200.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/ampRingShotOnlyAngle", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/shooter/ampRingShotOnlyRPM", - "type": "double", - "value": 3200.0, - "properties": { - "persistent": true - } - } -] +[] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f12bded..54c9a42 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.RumbleSubsystem; import monologue.Monologue; import monologue.Annotations.Log; import monologue.Logged; @@ -161,10 +162,11 @@ public void teleopInit() { // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove - // this line or comment it out. + // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.m_rumble.teleopRumble(5); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4c1968d..9d9a306 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,6 +38,7 @@ import frc.robot.commands.Shooter.PrepareShooter; import frc.robot.subsystems.Climber; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RumbleSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.UmbrellaSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; @@ -81,10 +82,16 @@ public class RobotContainer implements Logged { private final PhotonCameraWrapper m_photonCameraWrapper = new PhotonCameraWrapper(); protected final DriveSubsystem m_robotDrive = new DriveSubsystem(m_photonCameraWrapper); + + //controllers + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); + XboxController m_manipController = new XboxController(OIConstants.kManipControllerPort); private final IntakeSubsystem m_robotIntake = new IntakeSubsystem(); //private final UmbrellaSubsystem m_umbrella = new UmbrellaSubsystem(); private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + public final RumbleSubsystem m_rumble = new RumbleSubsystem(m_driverController, m_manipController, 0, 0); + private final Climber m_Climber = new Climber(); @@ -160,7 +167,7 @@ public class RobotContainer implements Logged { private final Command resetGyro = new ResetGyro(m_robotDrive); private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition); private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition); - private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition); + private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, m_rumble, intakePower, intakePosition, indexPower, intakeShooterPosition); private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower); private final Command stowIntake = new StowIntake(m_robotIntake); private final Command parkCommand = new ParkCommand(m_robotDrive); @@ -168,9 +175,9 @@ public class RobotContainer implements Logged { private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition); private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower); private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower); - private final Command shootSubwoofer = new ShootPiece(m_shooter, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootPodium = new ShootPiece(m_shooter, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootWing = new ShootPiece(m_shooter, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootSubwoofer = new ShootPiece(m_shooter, m_rumble, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootPodium = new ShootPiece(m_shooter, m_rumble, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootWing = new ShootPiece(m_shooter, m_rumble, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM); private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM); private final Command ejectPiece = new EjectPiece(m_shooter); @@ -182,7 +189,7 @@ public class RobotContainer implements Logged { private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15); - private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shooterTune = new ShootPiece(m_shooter, m_rumble, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command driveToTarget = new DriveToTarget(m_robotDrive, m_photonCameraWrapper, @@ -197,7 +204,7 @@ public class RobotContainer implements Logged { private final Command autoAmpRingShot = new AutonShoot(m_shooter, autoAmpRingShotAngle, autoAmpRingShotRPM, shooterIndexPower).withTimeout(2); private final Command ampShotStart = new AutonShoot(m_shooter, ampRingShotOnlyAngle, ampRingShotOnlyRPM, shooterIndexPower).withTimeout(2); private final Command ringToss = new RingToss(m_robotIntake, m_shooter, intakePower, intakePosition, shooterIndexPower, intakeShooterPosition, () -> 1000.0); - private final Command autoIntake = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75); + private final Command autoIntake = new IntakePiece(m_robotIntake, m_shooter, m_rumble, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75); //Autonomous Wait Command @@ -210,9 +217,6 @@ public class RobotContainer implements Logged { private final double pathSpeed = 2; - // The driver's controller - XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); - XboxController m_manipController = new XboxController(OIConstants.kManipControllerPort); private static class Operator { @@ -355,6 +359,7 @@ private void configureDefaultCommands(){ m_shooter.setDefaultCommand(stowShooter); } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index edd1de6..e016473 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -48,7 +48,7 @@ public synchronized void setSpeakerPose(Pose2d pose, int id){ } public Pose2d getSpeakerPose2d(){ - return m_robotPose2d; + return m_speakerPose2d; } public int getSpeakerID(){ diff --git a/src/main/java/frc/robot/commands/Shooter/PrepareShooter.java b/src/main/java/frc/robot/commands/Shooter/PrepareShooter.java index 781a7cb..3668901 100644 --- a/src/main/java/frc/robot/commands/Shooter/PrepareShooter.java +++ b/src/main/java/frc/robot/commands/Shooter/PrepareShooter.java @@ -10,6 +10,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.utils.InterCalculator; import frc.robot.RobotState; +import frc.robot.subsystems.RumbleSubsystem; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java index 06f6df5..9bff7d9 100644 --- a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java +++ b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.shuffleboard.SuppliedValueWidget; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.RumbleSubsystem; import frc.robot.subsystems.ShooterSubsystem; import java.util.function.Supplier; @@ -13,13 +14,15 @@ public class ShootPiece extends Command { private final ShooterSubsystem m_shooter; + private final RumbleSubsystem m_rumble; private final Supplier m_position; private final Supplier m_rpm; private final Supplier m_indexPower; private final Supplier m_ready; /** Creates a new ShootPiece. */ - public ShootPiece(ShooterSubsystem shooter, Supplier position, Supplier rpm, Supplier indexpower, Supplier ready) { + public ShootPiece(ShooterSubsystem shooter, RumbleSubsystem rumble, Supplier position, Supplier rpm, Supplier indexpower, Supplier ready) { m_shooter = shooter; + m_rumble = rumble; m_position = position; m_rpm = rpm; m_indexPower = indexpower; @@ -41,6 +44,9 @@ public void execute() { if (m_ready.get()){ m_shooter.runIndex(m_indexPower.get()); } + if(m_shooter.armAtSetpoint() && m_shooter.atRPM()) { + m_rumble.shooterAtRPMAndAngle(3); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/intake/IntakePiece.java b/src/main/java/frc/robot/commands/intake/IntakePiece.java index ef58990..7b57833 100644 --- a/src/main/java/frc/robot/commands/intake/IntakePiece.java +++ b/src/main/java/frc/robot/commands/intake/IntakePiece.java @@ -9,12 +9,14 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.RumbleSubsystem; import java.util.function.Supplier; public class IntakePiece extends Command { private final IntakeSubsystem m_intake; private final ShooterSubsystem m_shooter; + private final RumbleSubsystem m_rumble; private final Supplier m_intakePower; private final Supplier m_intakePosition; private final Supplier m_indexPower; @@ -26,10 +28,11 @@ public class IntakePiece extends Command { /** Creates a new IntakePiece. */ - public IntakePiece(IntakeSubsystem intake, ShooterSubsystem shooter, Supplier intakePower, Supplier intakePosition, Supplier indexPower, Supplier indexPosition ) { + public IntakePiece(IntakeSubsystem intake, ShooterSubsystem shooter, RumbleSubsystem rumble, Supplier intakePower, Supplier intakePosition, Supplier indexPower, Supplier indexPosition ) { m_intake = intake; m_shooter = shooter; + m_rumble = rumble; m_intakePower = intakePower; m_intakePosition = intakePosition; m_indexPower = indexPower; @@ -57,6 +60,9 @@ public void execute() { public void end(boolean interrupted) { m_shooter.stopAll(); m_intake.stop(); + if(isFinished()) { + m_rumble.noteLoadedRumble(3); + } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/RumbleSubsystem.java b/src/main/java/frc/robot/subsystems/RumbleSubsystem.java new file mode 100644 index 0000000..556901f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RumbleSubsystem.java @@ -0,0 +1,81 @@ +// 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.subsystems; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; + +public class RumbleSubsystem extends SubsystemBase { + /** Creates a new RumbleSubsystem. */ + XboxController m_driveController; + XboxController m_manipController; + double m_rumbleTime; + double m_driverRumblePower; + double m_manipRumblePower; + Timer rumbleTimer = new Timer(); + double noteLoops; + boolean isContinuous; + + public RumbleSubsystem(XboxController driverController, XboxController manipController, double driverRumblePower, double manipRumblePower) { + m_driveController = driverController; + m_manipController = manipController; + m_driverRumblePower = driverRumblePower; + m_manipRumblePower = manipRumblePower; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + if(rumbleTimer.hasElapsed(m_rumbleTime)){ + m_driveController.setRumble(RumbleType.kBothRumble, 0); + m_manipController.setRumble(RumbleType.kBothRumble, 0); + } + + if(noteLoops <= 2 && !isContinuous) { + noteLoadedRumbleLoop(0.25); + noteLoops++; + } else { + m_driveController.setRumble(RumbleType.kBothRumble, 0); + m_manipController.setRumble(RumbleType.kBothRumble, 0); + } + } + + public void teleopRumble(double rumbleTime) { + rumbleTimer.reset(); + rumbleTimer.start(); + m_rumbleTime = rumbleTime; + isContinuous = true; + m_driveController.setRumble(RumbleType.kBothRumble, 1); + m_manipController.setRumble(RumbleType.kBothRumble, 0); + } + + public void noteLoadedRumble(double rumbleTime) { + rumbleTimer.reset(); + rumbleTimer.start(); + m_rumbleTime = rumbleTime; + isContinuous = false; + m_driveController.setRumble(RumbleType.kBothRumble, 1); + m_manipController.setRumble(RumbleType.kBothRumble, 1); + } + + public void noteLoadedRumbleLoop(double rumbleTime) { + rumbleTimer.reset(); + rumbleTimer.start(); + m_rumbleTime = rumbleTime; + isContinuous = true; + m_driveController.setRumble(RumbleType.kBothRumble, 1); + m_manipController.setRumble(RumbleType.kBothRumble, 1); + } + + public void shooterAtRPMAndAngle(double rumbleTime) { + rumbleTimer.reset(); + rumbleTimer.start(); + m_rumbleTime = rumbleTime; + isContinuous = true; + m_driveController.setRumble(RumbleType.kBothRumble, 0.5); + m_manipController.setRumble(RumbleType.kBothRumble, 0.5); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d181f00..25f2e68 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -395,7 +395,12 @@ public void stopAll(){ @Log.File @Log.NT public boolean getIndexerBeamBreak(){ - return !m_indexerBeamBreak.get(); + if(Robot.isReal()){ + return !m_indexerBeamBreak.get(); + } else { + return true; + } + } @Override