Only Intake Subsystem and commands

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 java {

src/main/java/frc/robot/Constants.java

@@ -100,18 +100,15 @@ public static final class DriveConstants {
 
     // Intake PID and Encoder Constants
     public static class IntakeConstants {
+        //TODO: figure out constants
+        //public static final boolean kPivotMotorInverted = true;
         public static final double kIntakeLoweredAngle = 9.0;
-        public static final double kIntakeRaisedAngle = 9.0;
+        public static final double kIntakeRaisedAngle = 60.0;
         public static final int kIntakeMotorID = 0;
+        public static final double kIntakeMotorSpeed = -3.0;
         public static final int kArmMotorID = 0;
-        public static final double kIntakeP = 0;
-        public static final double kIntakeI = 0;
-        public static final double kIntakeD = 0;
-        public static final double kArmP = 0;
-        public static final double kArmI = 0;
-        public static final double kArmD = 0;
         public static final int kArmEncoderCh = 0;
-        public static double kIntakeSpeed;
+        public static double kIntakeSpeed = 3.0;
     }
 
     // Shooter subsystem speed constants

src/main/java/frc/robot/RobotContainer.java

@@ -13,11 +13,10 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.Constants.IntakeConstants;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
-import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -26,84 +25,34 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-
-  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
-  public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
   public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
 
-  private boolean IntakeDropped = false;
-  private boolean lastAButton = false;
-
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
 
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
-    m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5
-        ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem)
-        : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem));
-
-    m_robotDrive.setDefaultCommand(
-        new RunCommand(
-            () -> m_robotDrive.drive(
-                MathUtil.applyDeadband(
-                    -m_driverController.getLeftY(),
-                    IOConstants.kControllerDeadband)
-                    * DriveConstants.kMaxSpeedMetersPerSecond
-                    * (1 - m_driverController
-                        .getLeftTriggerAxis()
-                        * IOConstants.kSlowModeScalar)
-                    * 0.8,
-                MathUtil.applyDeadband(
-                    -m_driverController.getLeftX(),
-                    IOConstants.kControllerDeadband)
-                    * DriveConstants.kMaxSpeedMetersPerSecond
-                    * (1 - m_driverController
-                        .getLeftTriggerAxis()
-                        * IOConstants.kSlowModeScalar)
-                    * 0.8,
-                MathUtil.applyDeadband(
-                    m_driverController.getRightX(),
-                    IOConstants.kControllerDeadband)
-                    * DriveConstants.kMaxAngularSpeedRadiansPerSecond
-                    * (1 - m_driverController
-                        .getLeftTriggerAxis()
-                        * IOConstants.kSlowModeScalar)
-                    / 2,
-                !m_driverController.getRightBumper()),
-            m_robotDrive));
-
   }
 
   public void periodic() {
-    if (m_driverController.getAButton()) {
-      lastAButton = true;
-      if (!lastAButton)
-        IntakeDropped = !IntakeDropped;
-    } else {
-      lastAButton = false;
-    }
   }
 
   /**
    * Use this method to define your button->command mappings.
    */
   private void configureBindings() {
-    // TODO: Move shoot commands to operator controller
-    new JoystickButton(m_driverController, Button.kY.value)
-        .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle),
-            m_intakeSubsystem))
-        .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
     new JoystickButton(m_driverController, Button.kX.value)
-        .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle),
-            m_intakeSubsystem))
-        .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
-    new JoystickButton(m_driverController, Button.kB.value)
-        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
-        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
-    new JoystickButton(m_driverController, Button.kA.value)
-        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
-        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
+        .whileTrue( new IntakeCommand(m_intakeSubsystem) );
+    // TODO: Move shoot commands to operator controller
+    // new JoystickButton(m_driverController, Button.kY.value)
+    //     .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle),
+    //         m_intakeSubsystem))
+    //     .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
+    // new JoystickButton(m_driverController, Button.kX.value)
+    //     .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle),
+    //         m_intakeSubsystem))
+    //     .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
+
   }
 
   /**

src/main/java/frc/robot/commands/IntakeCommand.java

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class IntakeCommand extends Command {
+  private IntakeSubsystem m_intakeSubsystem;
+  private Timer m_timer = new Timer();
+
+
+  /** Creates a new intakeCommand. */
+  public IntakeCommand(IntakeSubsystem subsystem) {
+    m_intakeSubsystem = subsystem;
+    addRequirements(m_intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_timer.reset();
+    m_timer.start();
+    m_intakeSubsystem.turnOn();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_intakeSubsystem.getDistanceSensor() < 5){
+      m_intakeSubsystem.turnOff();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_intakeSubsystem.turnOff();
+  }
+
+  @Override
+  public boolean isFinished(){
+    return m_timer.hasElapsed(5);
+  }
+}