From 0526a58ae707255cdda4a40b845e8c7e0a90fc22 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Wed, 17 Apr 2024 21:37:33 -0700 Subject: [PATCH] practice day changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 84 +++++++++++++------ .../roller/IntakeRollerIntakeCommand.java | 11 ++- .../intake/IntakePivotSubsystem.java | 39 ++++++++- .../shooter/ShooterPivotSubsystem.java | 2 +- 5 files changed, 105 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f9838b16..47d357ae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -98,7 +98,7 @@ public static class IntakeConstants { public static final int BACK_MOTOR_ID = 18; public static final int PIVOT_MOTOR_ID = 16; - public static final double PIVOT_P = 1; + public static final double PIVOT_P = 2.5; public static final double PIVOT_I = 0; public static final double PIVOT_D = 0; public static final double PIVOT_CONVERSION_FACTOR = 0.2142; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 37ebd15f..a57aead5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -463,7 +463,9 @@ private void configureBindings() { /* ElEVATOR TEST */ dPadUp.onTrue(new ElevatorToTrapCommand(elevatorSubsystem)); + dPadUp.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false))); dPadDown.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem)); + dPadDown.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(true))); // elevatorSubsystem.setManual(); @@ -510,31 +512,40 @@ private void configureBindings() { // if the elevator is down, run the amp sequence rightBumper.onTrue( new ConditionalCommand( - // if elevator is up - new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator - () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow the pivot - - // if elevator is down - new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem) - .until(() -> mechController.getLeftTriggerAxis() > .05 - || mechController.getRightTriggerAxis() > .05), - - // check if the elevator is currently targeting one of the upper positions to choose what to do - () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP - || elevatorSubsystem.getTargetState() == ElevatorState.TRAP)); + // if elevator is up + new ElevatorToZeroCommand(elevatorSubsystem).alongWith( + Commands.runOnce(() -> { + intakePivotSubsystem.enablePowerLimit(true); + intakePivotSubsystem.setPosition(0); + }, intakePivotSubsystem) + ), // stow the pivot + + // if elevator is down + new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem) + .until(() -> mechController.getLeftTriggerAxis() > .05 + || mechController.getRightTriggerAxis() > .05), + + // check if the elevator is currently targeting one of the upper positions to choose what to do + () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP + || elevatorSubsystem.getTargetState() == ElevatorState.TRAP + ) + ); // leftBumper toggles the trap position for the elevator leftBumper.onTrue( new ConditionalCommand( new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator - () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow intake + () -> { + intakePivotSubsystem.enablePowerLimit(true); + intakePivotSubsystem.setPosition(0); + }, intakePivotSubsystem)), // stow intake new ConditionalCommand( new ElevatorToTrapCommand(elevatorSubsystem).andThen( - new IntakePivotSetPositionCommand(intakePivotSubsystem, .45).withTimeout(.1) + new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false)), + new IntakePivotSetPositionCommand(intakePivotSubsystem, .45) ), new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(// extend pivot - new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor - .until(() -> intakeRollerSubsystem.getFrontSensorReached()), + new ConditionalWaitCommand(intakeRollerSubsystem::getFrontSensorReached), new IntakePivotSetPositionCommand(intakePivotSubsystem, 0) ), intakeRollerSubsystem::getAmpSensor @@ -544,18 +555,36 @@ private void configureBindings() { ); + // roll behaviors + leftBumper.onTrue( + new ConditionalCommand( + null, + new ConditionalCommand( + null, + new WaitCommand(.05).andThen( + new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot + new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor + .until(intakeRollerSubsystem::getFrontSensorReached) + ), + intakeRollerSubsystem::getAmpSensor + ), // raise the elevator + () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos + || elevatorSubsystem.getTargetState() == ElevatorState.TRAP) + ); + + // aButton runs the intake sequence aButton.onTrue( - new InstantCommand(() -> {intakePivotSubsystem.setPosition(1);}, intakePivotSubsystem).alongWith(// then extend the intake + Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith( new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen( new ConditionalCommand( new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen( - new InstantCommand(() -> {intakePivotSubsystem.setPosition(0);}, intakePivotSubsystem) + new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem) ), new InstantCommand( - () -> {intakePivotSubsystem.setPosition(0);}, intakePivotSubsystem + () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem ), () -> aButton.getAsBoolean() ) @@ -564,7 +593,7 @@ private void configureBindings() { ); // bButton stops the rollers - bButton.onTrue(new InstantCommand(() -> {}, intakeRollerSubsystem)); + bButton.onTrue(Commands.idle(intakeRollerSubsystem)); // xButton toggles the intake being stowed xButton.onTrue(new InstantCommand(() -> { @@ -592,14 +621,18 @@ private void configureBindings() { new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen( new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem), new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition) - ).unless(intakeRollerSubsystem::getRockwellSensorValue) + ).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen( + new IntakePivotSetPositionCommand(intakePivotSubsystem, 0) + ) ); shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> { if (yButton.getAsBoolean()) { lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2); // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning - shooterFlywheelSubsystem.setShooterMotorSpeed(); + if (intakePivotSubsystem.atPosition()) { + shooterFlywheelSubsystem.setShooterMotorSpeed(); + } shooterPivotSubsystem.setAutoAimBoolean(true); if (shooterFlywheelSubsystem.atSpeed()) { mechController.setRumble(RumbleType.kBothRumble, .4); @@ -658,8 +691,11 @@ private void configureBindings() { new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition) ).unless(intakeRollerSubsystem::getRockwellSensorValue)); - dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .65, mechController).onlyWhile(dPadRight)); - + dPadRight.whileTrue( + new ConditionalWaitCommand(() -> intakePivotSubsystem.atPosition()) + .andThen(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, + shooterPivotSubsystem, .7, mechController)) + ); // intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> { // intakePosition = MathUtil.clamp(intakePosition, 0, 1); // intakePivotSubsystem.setPosition(intakePosition); diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java index 999a8d11..40c76046 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java @@ -9,7 +9,8 @@ public class IntakeRollerIntakeCommand extends Command { private final IntakeRollerSubsystem intakeSubsystem; private final LightBarSubsystem lightBarSubsystem; - private double speed = .7; + private double frontSpeed = .7; + private double integrationSpeed = .4; /** * Runs the rollers on the intake until a note is detected. @@ -23,11 +24,12 @@ public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBar addRequirements(intakeSubsystem); } - public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed) { + public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed, double integrationSpeed) { this.intakeSubsystem = intakeSubsystem; this.lightBarSubsystem = lightBarSubsystem; addRequirements(intakeSubsystem); - this.speed = speed; + this.frontSpeed = speed; + this.integrationSpeed = integrationSpeed; } @Override @@ -37,7 +39,8 @@ public void initialize() { @Override public void execute() { - intakeSubsystem.setRollSpeeds(speed, speed); + intakeSubsystem.setRollSpeeds(frontSpeed, integrationSpeed); + System.out.println(intakeSubsystem.getRockwellSensorValue()); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java index 48e3ffbd..df30d85e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java @@ -3,13 +3,14 @@ import static frc.robot.Constants.IntakeConstants; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; /** The subsystem for the pivot on the intake. */ @@ -18,7 +19,9 @@ public class IntakePivotSubsystem extends SubsystemBase { private final TalonFX pivotMotor; private PositionVoltage request = new PositionVoltage(0).withSlot(0); + private VoltageConfigs voltageConfig = new VoltageConfigs(); private double setPos = 0; + private static final double FORWARD_VOLTAGE = 1.75; private NetworkTableInstance ntInstance; private NetworkTable motorsNTTable; @@ -32,6 +35,7 @@ public class IntakePivotSubsystem extends SubsystemBase { public IntakePivotSubsystem() { pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); pivotMotor.setNeutralMode(NeutralModeValue.Brake); + // intakeencoder = new Encoder(1, 2); // extendedlimitswitch = new DigitalInput(extendedlimitswitchID); // retractedlimitswitch = new DigitalInput(retractedlimitswitchID); @@ -42,8 +46,16 @@ public IntakePivotSubsystem() { slot0Configs.kI = IntakeConstants.PIVOT_I; slot0Configs.kD = IntakeConstants.PIVOT_D; + voltageConfig.PeakForwardVoltage = FORWARD_VOLTAGE; + voltageConfig.PeakReverseVoltage = -16; + + pivotMotor.getConfigurator().apply(slot0Configs); pivotMotor.setPosition(0); + pivotMotor.getConfigurator().apply(voltageConfig); + + + request.withLimitForwardMotion(false); ntInstance = NetworkTableInstance.getDefault(); motorsNTTable = ntInstance.getTable("Motors"); @@ -77,6 +89,11 @@ public double getEncoderPosition() { return pivotMotor.getPosition().getValueAsDouble() * IntakeConstants.PIVOT_CONVERSION_FACTOR; } + /** Returns whether or not the intake is at (or within tolerance of) its setpoint. */ + public boolean atPosition() { + return Math.abs(getEncoderPosition() - setPos) < .05; + } + /** * Sets the pivot motor to a speed. * @@ -86,10 +103,26 @@ public void movePivot(double speed) { pivotMotor.set(speed); } + /** + * Enables/disables the pivot motor's forward voltage limit for deploying the intake. + * When enabled, the linkage will not slam down when deploying. + * + * @param enabled True enables the limit, false disables it. + */ + public void enablePowerLimit(boolean enabled) { + if (enabled) { + voltageConfig.PeakForwardVoltage = FORWARD_VOLTAGE; + } else { + voltageConfig.PeakForwardVoltage = 16; + } + + pivotMotor.getConfigurator().apply(voltageConfig); + } + @Override public void periodic() { pivotMotor.setControl(request.withPosition(setPos / IntakeConstants.PIVOT_CONVERSION_FACTOR)); - + intake16CurrentEntry.setDouble(pivotMotor.getSupplyCurrent().getValueAsDouble()); intake16VoltageEntry.setDouble(pivotMotor.getMotorVoltage().getValueAsDouble()); intake16TemperatureEntry.setDouble(pivotMotor.getDeviceTemp().getValueAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 0eef54b4..611be74b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -109,7 +109,7 @@ public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier re 5, 6, ShooterConstants.MAX_SHOOTER_DISTANCE}; - double[] angles = {Units.degreesToRadians(60), + double[] angles = {Units.degreesToRadians(59), Units.degreesToRadians(52), Units.degreesToRadians(36), Units.degreesToRadians(31), //4