diff --git a/src/main/java/com/team2052/swervemodule/SwerveModule.java b/src/main/java/com/team2052/swervemodule/SwerveModule.java index 937e4ff..43cce3c 100644 --- a/src/main/java/com/team2052/swervemodule/SwerveModule.java +++ b/src/main/java/com/team2052/swervemodule/SwerveModule.java @@ -210,6 +210,10 @@ public SwerveModulePosition getPosition() { ); } + public TalonFX getTalonFX(){ + return driveMotor; + } + public static double getMaxVelocityMetersPerSecond() { /* * The formula for calculating the theoretical maximum velocity is: diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5de8968..cdee204 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,7 +20,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; - +import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.ShamperSubsystem; public final class Constants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9050706..6af5dfa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -27,7 +27,7 @@ public void robotInit() { Logger.start(); //Start AdvantageKit Logger m_robotContainer = new RobotContainer(); - // m_robotContainer.robotStatusCommunicator.onRobotInitiation(); + m_robotContainer.robotStatusCommunicator.onRobotInitiation(); } /** @@ -46,13 +46,13 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // m_robotContainer.robotStatusCommunicator.onRobotPeriodic(); + m_robotContainer.robotStatusCommunicator.onRobotPeriodic(); } /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { - //m_robotContainer.robotStatusCommunicator.onRobotDisable(); + // m_robotContainer.robotStatusCommunicator.onRobotDisable(); } @Override @@ -85,8 +85,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - - // m_robotContainer.robotStatusCommunicator.onRobotTeleop(); + + m_robotContainer.robotStatusCommunicator.onRobotTeleop(); } /** 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 aaa8960..a071cae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.robot.commands.indexer.IndexerIndexCommand; import frc.robot.commands.intake.IntakeThenBackupCommand; import frc.robot.commands.intake.OuttakeCommand; +import frc.robot.commands.music.PlayFOTBCommand; import frc.robot.commands.shamper.ShamperAmpCommand; import frc.robot.commands.shamper.ShamperDefaultCommand; import frc.robot.commands.shamper.ShamperLobOrShootCommand; @@ -46,10 +47,12 @@ import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LedSubsystem; +import frc.robot.subsystems.MusicPlayerSubsystem; import frc.robot.subsystems.ShamperSubsystem; import frc.robot.subsystems.TrapArmSubsystem; import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed; import frc.robot.util.AimingCalculator; +import frc.robot.util.RobotStatusCommunicator; import frc.robot.util.io.Dashboard; import frc.robot.util.io.pixy.Pixy2CCC; @@ -73,7 +76,7 @@ public class RobotContainer { private final AprilTagSubsystem aprilTag; private final LedSubsystem ledSubsystem; private ForwardPixySubsystem pixy; - // private final MusicPlayerSubsystem musicPlayer; + private final MusicPlayerSubsystem musicPlayer; // private final VisionSubsystem vision; private final AdvantageScopeSubsystem advantageScope; private final TrapArmSubsystem trapArm; @@ -85,7 +88,7 @@ public class RobotContainer { private final Joystick controlPanel; public static boolean musicOn; - // public RobotStatusCommunicator robotStatusCommunicator; + public RobotStatusCommunicator robotStatusCommunicator; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { drivetrain = new DrivetrainSubsystem(); @@ -95,14 +98,14 @@ public RobotContainer() { climber = new ClimberSubsystem(); aprilTag = AprilTagSubsystem.getInstance(); ledSubsystem = LedSubsystem.getInstance(); - // musicPlayer = new MusicPlayerSubsystem(); + musicPlayer = new MusicPlayerSubsystem(); // vision = new VisionSubsystem(); trapArm = new TrapArmSubsystem(); advantageScope = new AdvantageScopeSubsystem(intake, shamper, climber, drivetrain, indexer); pixy = new ForwardPixySubsystem(); - // robotStatusCommunicator = new RobotStatusCommunicator(musicPlayer); + robotStatusCommunicator = new RobotStatusCommunicator(musicPlayer); musicOn = true; ledSubsystem.enableLEDs(); @@ -293,6 +296,9 @@ private void configureButtonBindings() { */ // JoystickButton toggleMusicPlayerButton = new JoystickButton(controlPanel, 2); // toggleMusicPlayerButton.onTrue(toggleMusic()); + + JoystickButton musictest = new JoystickButton(translationJoystick, 10); + musictest.onTrue(new PlayFOTBCommand(musicPlayer)); } // public Command toggleMusic() { diff --git a/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java b/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java index 62de3f3..cfe8930 100644 --- a/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java @@ -13,8 +13,9 @@ public PlayFOTBCommand(MusicPlayerSubsystem player) { @Override public void initialize() { + System.out.println("RUNING MUSIC STUFF"); player.stopMusic(); - player.loadMusic("FlightOfTheBumblebees.chrp"); + player.loadMusic("music/FlightOfTheBumblebees.chrp"); player.playLoadedTrack(); } diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index fc0cdd5..565c80b 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -6,6 +6,7 @@ import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.hardware.TalonFX; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; import com.team2052.swervemodule.SwerveModule; @@ -275,6 +276,11 @@ public static double getMaxAngularVelocityRadiansPerSecond() { //return 6 * Math.PI; } + public TalonFX[] getDriveMotors() { + TalonFX[] driveMotorArray = {frontLeftModule.getTalonFX(), frontRightModule.getTalonFX(), backRightModule.getTalonFX(), backLeftModule.getTalonFX()}; + return driveMotorArray; + } + public void debug() { frontLeftModule.debug(); backLeftModule.debug(); diff --git a/src/main/java/frc/robot/util/RobotStatusCommunicator.java b/src/main/java/frc/robot/util/RobotStatusCommunicator.java index e3cdf17..626b3e7 100644 --- a/src/main/java/frc/robot/util/RobotStatusCommunicator.java +++ b/src/main/java/frc/robot/util/RobotStatusCommunicator.java @@ -30,7 +30,7 @@ public void onRobotInitiation() { } public void onRobotPeriodic() { - if (DriverStation.getMatchTime() >= 180 && !hasRunTwentySeconds) { + if (DriverStation.getMatchTime() >= 130 && !hasRunTwentySeconds) { onTwentySecondsLeft(); hasRunTwentySeconds = true; }