From 63a8e5465b1bed16e70873bac42780d6d351fc9d Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Tue, 6 Aug 2024 22:49:32 -0400 Subject: [PATCH] completed intake and related commands --- src/main/java/frc/robot/Constants.java | 13 ++--- src/main/java/frc/robot/RobotContainer.java | 36 +++++++----- .../commands/shooter/GrabNoteManual.java | 26 +++++++++ .../frc/robot/subsystems/intake/Intake.java | 55 +++++++++++++++++++ .../frc/robot/subsystems/intake/IntakeIO.java | 14 +++++ .../robot/subsystems/intake/IntakeIOReal.java | 21 +++++++ .../subsystems/shooter/FlyWheelsIOReal.java | 3 +- .../frc/robot/subsystems/shooter/Shooter.java | 9 ++- 8 files changed, 151 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/commands/shooter/GrabNoteManual.java create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e6a7bb6..e9191d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -77,25 +77,24 @@ public static final class PitchConfigs { public static final double PITCH_LOWEST_ROTATION_RAD = Math.toRadians(8); public static final double PITCH_HIGHER_LIMIT_RAD = Math.toRadians(92); - public static final double PITCH_KS = 0.12; - public static final double PITCH_KG = 0.2; - public static final double PITCH_KV = 7.2; + public static final double PITCH_KS = 0.15; + public static final double PITCH_KG = 0.22; + public static final double PITCH_KV = 8; public static final double PITCH_KA = 0.01; public static final MaplePIDController.MaplePIDConfig PITCH_PID = new MaplePIDController.MaplePIDConfig( 12, - Math.toRadians(25), + Math.toRadians(22), 0, Math.toRadians(2), - 0.05, + 0.03, false, 0 ); public static final TrapezoidProfile.Constraints PITCH_PROFILE_CONSTRAIN = new TrapezoidProfile.Constraints( - Math.toRadians(180), Math.toRadians(250) + Math.toRadians(160), Math.toRadians(320) ); - } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6d8b0d4..c3f599a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,18 +14,20 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.autos.Auto; import frc.robot.autos.AutoBuilder; import frc.robot.commands.drive.AutoAlignment; import frc.robot.commands.drive.JoystickDrive; +import frc.robot.commands.shooter.GrabNoteManual; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.drive.IO.GyroIOPigeon2; import frc.robot.subsystems.drive.IO.GyroIOSim; import frc.robot.subsystems.drive.IO.ModuleIOSim; import frc.robot.subsystems.drive.IO.ModuleIOTalonFX; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIOReal; import frc.robot.subsystems.shooter.PitchIOReal; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.FlyWheelsIOReal; @@ -58,6 +60,7 @@ public class RobotContainer { public final SwerveDrive drive; public final AprilTagVision aprilTagVision; public final Shooter shooter; + private final Intake intake; // Controller private final CommandXboxController driverController = new CommandXboxController(0), @@ -118,7 +121,9 @@ public RobotContainer() { drive ); - shooter = new Shooter(new FlyWheelsIOReal(), new PitchIOReal()); + this.shooter = new Shooter(new FlyWheelsIOReal(), new PitchIOReal()); + + this.intake = new Intake(new IntakeIOReal()); } case SIM -> { @@ -163,6 +168,8 @@ public RobotContainer() { // no shooters simulation (for now) shooter = new Shooter(shooterInputs -> {}, pitchInputs -> {}); + + this.intake = new Intake(inputs -> {}); } default -> { @@ -183,7 +190,9 @@ public RobotContainer() { drive ); - shooter = new Shooter(shooterInputs -> {}, pitchInputs -> {}); + this.shooter = new Shooter(shooterInputs -> {}, pitchInputs -> {}); + + this.intake = new Intake(inputs -> {}); } } SmartDashboard.putData("Select Test", testChooser = TestBuilder.buildTestsChooser(this)); @@ -233,18 +242,17 @@ private void configureButtonBindings() { ).ignoringDisable(true) ); - driverController.y().whileTrue(new AutoAlignment( - drive, - () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.35, Rotation2d.fromDegrees(-90))), - () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.7, Rotation2d.fromDegrees(-90))), - new Pose2d(0.1, 0.1, Rotation2d.fromDegrees(3)), - 0.5 - )); +// driverController.y().whileTrue(new AutoAlignment( +// drive, +// () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.35, Rotation2d.fromDegrees(-90))), +// () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.7, Rotation2d.fromDegrees(-90))), +// new Pose2d(0.1, 0.1, Rotation2d.fromDegrees(3)), +// 0.5 +// )); - driverController.a().whileTrue(Commands.run( - () -> shooter.runShooterState(Math.toRadians(50), 3000), - shooter - )); + driverController.y().whileTrue(Commands.run(() -> shooter.runShooterState(Math.toRadians(40), 4000), shooter)); + + driverController.a().whileTrue(new GrabNoteManual(shooter, intake)); } /** diff --git a/src/main/java/frc/robot/commands/shooter/GrabNoteManual.java b/src/main/java/frc/robot/commands/shooter/GrabNoteManual.java new file mode 100644 index 0000000..bf7a36f --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/GrabNoteManual.java @@ -0,0 +1,26 @@ +package frc.robot.commands.shooter; + + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; + +public class GrabNoteManual extends SequentialCommandGroup { + private final Shooter shooter; + private final Intake intake; + + public GrabNoteManual(Shooter shooter, Intake intake) { + this.shooter = shooter; + this.intake = intake; + + super.addRequirements(shooter, intake); + + super.addCommands( + Commands.run(shooter::runIdle) + .until(shooter::isPitchInPosition) + ); + + super.addCommands(intake.runIntakeUntilNoteDetected()); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..27f74fa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,55 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import frc.robot.subsystems.MapleSubsystem; +import org.littletonrobotics.junction.Logger; + +public class Intake extends MapleSubsystem { + private final IntakeIO io; + private final IntakeInputsAutoLogged inputs; + + private boolean beamBrakeAlwaysTrue = true; + + public Intake(IntakeIO io) { + super("Intake"); + this.io = io; + this.inputs = new IntakeInputsAutoLogged(); + } + + + @Override + public void onReset() { + + } + + @Override + public void periodic(double dt, boolean enabled) { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + beamBrakeAlwaysTrue &= inputs.beamBreakTriggered; + if (beamBrakeAlwaysTrue) + DriverStation.reportWarning("Intake beam breaker always blocked, check wiring!", false); + } + + public Command runIntakeUntilNoteDetected() { + return new FunctionalCommand( + () -> {}, + () -> io.runIntakeVoltage(12), + interrupted ->io.runIntakeVoltage(0), + () -> inputs.beamBreakTriggered, + this + ); + } + + public Command shootNoteUntilNoteGone() { + return new FunctionalCommand( + () -> {}, + () -> io.runIntakeVoltage(8), + interrupted ->io.runIntakeVoltage(0), + () -> !inputs.beamBreakTriggered, + this + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..966da9e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + class IntakeInputs { + boolean beamBreakTriggered = true; + } + + void updateInputs(IntakeInputs inputs); + + default void runIntakeVoltage(double volts) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java new file mode 100644 index 0000000..3cfe981 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotController; + +public class IntakeIOReal implements IntakeIO { + private final TalonSRX intakeMotor = new TalonSRX(9); + private final DigitalInput beamBreak = new DigitalInput(3); + @Override + public void updateInputs(IntakeInputs inputs) { + inputs.beamBreakTriggered = beamBreak.get(); + } + + @Override + public void runIntakeVoltage(double volts) { + intakeMotor.set(ControlMode.PercentOutput, + volts / RobotController.getBatteryVoltage()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/FlyWheelsIOReal.java b/src/main/java/frc/robot/subsystems/shooter/FlyWheelsIOReal.java index a11b313..f49fb8d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlyWheelsIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlyWheelsIOReal.java @@ -13,8 +13,7 @@ public class FlyWheelsIOReal implements FlyWheelsIO { shooter1SuppliedCurrentAmps, shooter2SuppliedCurrentAmps, shooterPositionRev, shooterVelocityRevPerSec; public FlyWheelsIOReal() { this.shooterFalcon1 = new TalonFX(12); - this.shooterFalcon2 = new TalonFX(13); - this.shooterFalcon2.setInverted(true); + this.shooterFalcon2 = new TalonFX(16); this.shooter1SuppliedCurrentAmps = shooterFalcon1.getSupplyCurrent(); this.shooter2SuppliedCurrentAmps = shooterFalcon2.getSupplyCurrent(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 01708f9..e55cec0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; @@ -64,9 +65,7 @@ public Shooter(FlyWheelsIO flyWheelsIO, PitchIO pitchIO) { this.pitchFeedBack = new MaplePIDController(PITCH_PID); this.pitchProfile = new TrapezoidProfile(PITCH_PROFILE_CONSTRAIN); - super.setDefaultCommand(Commands.run(() -> runShooterState( - PITCH_LOWEST_ROTATION_RAD, 0 - ), this)); + super.setDefaultCommand(Commands.run(this::runIdle, this)); } @Override @@ -99,6 +98,10 @@ public void periodic(double dt, boolean enabled) { DriverStation.reportWarning("Warning!!! Pitch not calibrated!", false); } + public void runIdle() { + runShooterState(PITCH_LOWEST_ROTATION_RAD, 0); + } + public void runShooterState(double pitchAngleSetpointRadians, double shooterSetpointRPM) { runPitchCloseLoop(pitchAngleSetpointRadians); runFlyWheelCloseLoop(shooterSetpointRPM);