diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2781eb9..58513e1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,7 +25,7 @@ public final class Constants { public static final double MAX_ROT_SPEED = 3.5; public static final double AUTO_MAX_ROT_SPEED = 1.5; - public static final double MIN_ROT_SPEED = 0.8; + public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; @@ -169,7 +169,7 @@ public final class Constants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final public static final double SHOOTER_SPEED = 0.50; // final - public static final double SHOOTER_IDLE = 0.15; // final + public static final double SHOOTER_IDLE = 0.20; // final public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 51b613f..bf50581 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -21,7 +21,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.commands.Autos.AutoAlign; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; @@ -145,7 +147,7 @@ public class RobotContainer { private SequentialCommandGroup ampShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.ampPosition()), - new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed + new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed ); // ! /* Autos */ @@ -367,18 +369,18 @@ public class RobotContainer { true))); // ! /* Auto Recording */ - new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - () -> autoplaybackName.get())) - .onFalse(new InstantCommand()); + // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + // () -> autoplaybackName.get())) + // .onFalse(new InstantCommand()); - new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - () -> autoplaybackName.get(), - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, false)) - .onFalse(new InstantCommand()); + // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false)) + // .onFalse(new InstantCommand()); // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, @@ -393,18 +395,24 @@ public class RobotContainer { // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) // .onFalse(new InstantCommand()); // ! /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .whileTrue(new InstantCommand(() -> - m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - new Translation2d(0, 0), - true), m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .whileTrue(new InstantCommand(() -> + // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true), m_robotSwerveDrive)); //? /* Operator Buttons */ @@ -427,14 +435,14 @@ public class RobotContainer { // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-59), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-55), m_robotIntake)); // Override Intake Position encoder: in new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-6.2), m_robotIntake)); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) .onFalse(turnOffShoot); @@ -462,12 +470,7 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) - .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); } @@ -508,7 +511,7 @@ public class RobotContainer { // Override Intake Position encoder: out new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-59), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); // Override Intake Position encoder: in new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) @@ -516,7 +519,7 @@ public class RobotContainer { new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) .onFalse(turnOffShoot.asProxy()); @@ -543,11 +546,11 @@ public class RobotContainer { */ public Command getAutonomousCommand() { //no auto - // return new neoJoystickPlayback(m_robotSwerveDrive, - // "2note.auto", - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false); - return playbackChooser.getCommand(); + return new neoJoystickPlayback(m_robotSwerveDrive, + () -> autoplaybackName.get(), + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false); + //return playbackChooser.getCommand(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index f543373..ec78e53 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,6 +25,8 @@ public class Climber extends SubsystemBase { public Climber(TalonFX climbMotor) { this.climbMotor = climbMotor; this.climbMotor.setInverted(true); + + climbMotor.setNeutralMode(NeutralModeValue.Brake); var slot0Configs = new Slot0Configs(); slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output @@ -53,6 +57,6 @@ public class Climber extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); + //SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index e8d48e2..cd9371c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -128,13 +128,13 @@ public class Intake extends SubsystemBase { // ! Talon Methods public void talonPIDIn() { - PositionVoltage request = new PositionVoltage(-59); + PositionVoltage request = new PositionVoltage(-53); talonPivot.setControl(request.withPosition(0)); } public void talonPIDOut() { PositionVoltage request = new PositionVoltage(0); - talonPivot.setControl(request.withPosition(-59)); + talonPivot.setControl(request.withPosition(-53)); } public void talonPIDPosition(double out2) { @@ -161,7 +161,7 @@ public class Intake extends SubsystemBase { return false; } - public void talonSetPivotEncoderPosition(int val) { + public void talonSetPivotEncoderPosition(double val) { talonPivot.setPosition(val); } @@ -188,7 +188,7 @@ public class Intake extends SubsystemBase { talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value } - public void ampShoot(double speed) { + public void ampOuttake(double speed) { talonSpinIntakeMotor(speed); } @@ -333,8 +333,8 @@ public class Intake extends SubsystemBase { resetArmPosition(); - //SmartDashboard.putNumber("Pivot Position", getArmPos()); + // SmartDashboard.putNumber("Pivot Position", getArmPos()); - smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + //smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 44ce9ad..f8c39e0 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -71,12 +71,12 @@ public class Limelight extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run - isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; - double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); + //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; + //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - if(newPose != cameraPose){ - cameraPose = newPose; - update(); - } + //if(newPose != cameraPose){ + // cameraPose = newPose; + //update(); + //} } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index ba8ed12..0e50a3b 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -103,12 +103,12 @@ public class Shooter extends SubsystemBase { // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. // Else if the robot is not near the speaker, then set the speed back to idle. - if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ - leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - spinMode = 2; - }else if(!limelight.isNearSpeaker() && spinMode == 2){ - idle(); - } + // if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ + // leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + // rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + // spinMode = 2; + // }else if(!limelight.isNearSpeaker() && spinMode == 2){ + // idle(); + // } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f979565..e4e6cc4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -34,6 +34,7 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double rotTarget = 0.0; @@ -81,7 +82,7 @@ public class SwerveDrive extends SubsystemBase { // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX(), gyro.getRotation2d());//.times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -279,6 +280,14 @@ public class SwerveDrive extends SubsystemBase { } } + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + }