diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c42c67b..bc7516d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,28 +97,21 @@ public class RobotContainer { operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse)); }*/ - private void elevatorOnlyBindings(){ - elevator.setDefaultCommand(elevator.maintainPosition()); - manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition()); - - driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position)); - } - private void configureButtonBindings() { //Default commands climberPivot.setDefaultCommand( - climberPivot.runPivot(0) + climberPivot.runPivot(() -> 0) ); climberRollers.setDefaultCommand( - climberRollers.runRoller(0) + climberRollers.runRoller(() -> 0) ); drivetrain.setDefaultCommand( drivetrain.drive( - () -> Math.pow(driver.getLeftY(), 3), - () -> Math.pow(driver.getLeftX(), 3), - driver::getRightX, //Math.pow(driver.getRightX(), 3) + () -> Math.signum(driver.getLeftY()) * Math.pow(driver.getLeftY(), 3), + () -> Math.signum(driver.getLeftX()) * Math.pow(driver.getLeftX(), 3), + driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3) () -> true ) ); @@ -159,12 +152,7 @@ public class RobotContainer { driver.y().whileTrue(drivetrain.zeroHeading()); - driver.povDown().whileTrue(climberPivot.runPivot(-0.5)); - driver.povUp().whileTrue(climberPivot.runPivot(0.5)); - - driver.povLeft().whileTrue(climberRollers.runRoller(0.5)); - driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); - driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false)); + driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false)); /* driver.rightBumper().whileTrue( @@ -213,6 +201,8 @@ public class RobotContainer { ) ); + operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5))); + operator.a().onTrue( safeMoveManipulator(ElevatorConstants.kL1Position, 0.0) ); @@ -220,18 +210,18 @@ public class RobotContainer { operator.x().onTrue( safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) .alongWith(manipulator.runManipulator(() -> 0.5, false)) - .until(driver.rightTrigger()) + .until(() -> driver.a().getAsBoolean()) ); operator.b().onTrue( safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) .alongWith(manipulator.runManipulator(() -> 0.5, false)) - .until(driver.rightTrigger()) + .until(() -> driver.a().getAsBoolean()) ); operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) .alongWith(manipulator.runManipulator(() -> 0.5, false)) - .until(driver.rightTrigger()) + .until(() -> driver.a().getAsBoolean()) ); } diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index fdb54aa..14f902d 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -2,6 +2,7 @@ package frc.robot.constants; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.ctre.phoenix6.configs.AudioConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -57,6 +58,7 @@ public class ModuleConstants { public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs(); public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs(); public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs(); + public static final AudioConfigs kAudioConfig = new AudioConfigs(); public static final Slot0Configs kDriveSlot0Config = new Slot0Configs(); static { @@ -70,6 +72,8 @@ public class ModuleConstants { kDriveMotorConfig.Inverted = kDriveInversionState; kDriveMotorConfig.NeutralMode = kDriveIdleMode; + kAudioConfig.AllowMusicDurDisable = true; + kDriveSlot0Config.kP = kDriveP; kDriveSlot0Config.kI = kDriveI; kDriveSlot0Config.kD = kDriveD; diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java index 4fcc77f..08f756b 100644 --- a/src/main/java/frc/robot/subsystems/ClimberPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; @@ -30,9 +32,9 @@ public class ClimberPivot extends SubsystemBase { neoEncoder = pivotMotor.getEncoder(); } - public Command runPivot(double speed) { + public Command runPivot(DoubleSupplier speed) { return run(() -> { - pivotMotor.set(speed); + pivotMotor.set(speed.getAsDouble()); }); } diff --git a/src/main/java/frc/robot/subsystems/ClimberRollers.java b/src/main/java/frc/robot/subsystems/ClimberRollers.java index 3a24b99..a557b33 100644 --- a/src/main/java/frc/robot/subsystems/ClimberRollers.java +++ b/src/main/java/frc/robot/subsystems/ClimberRollers.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -32,9 +34,9 @@ public class ClimberRollers extends SubsystemBase { * @param speed The speed in which the roller runs * @return Runs the rollers at a set speed */ - public Command runRoller(double speed) { + public Command runRoller(DoubleSupplier speed) { return run(() -> { - rollerMotor.set(speed); + rollerMotor.set(speed.getAsDouble()); }); } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f3eb779..28c60d0 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -24,7 +24,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -47,8 +46,6 @@ public class Drivetrain extends SubsystemBase { // Odometry class for tracking robot pose private SwerveDrivePoseEstimator m_estimator; - private Vision vision; - public Orchestra m_orchestra = new Orchestra(); private Timer musicTimer = new Timer(); diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 3e1d7a7..01c0e16 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -56,6 +56,7 @@ public class MAXSwerveModule { m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig); m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig); m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig); + m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig); m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config); m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,