completed intake and related commands

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)
         );
-
     }
 
 

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));
     }
 
     /**

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());
+    }
+}

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
+        );
+    }
+}

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) {}
+}

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());
+    }
+}

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();

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);