Intake

build.gradle

@@ -69,6 +69,8 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     //testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    implementation files('vendordeps/DistanceSensor-java/DistanceSensor-java-0.4.0.jar')
 }
 
 test {

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

@@ -71,7 +71,8 @@ public static final class DriveConstants {
 
     /** Gear ratio between the motor and the wheel. */
     public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
-    // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration
+    // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2
+    // configuration
 
     // TODO: Tune this PID before running on a robot on the ground
     public static final double kPModuleTurningController = 0.3;
@@ -94,6 +95,22 @@ public static final class DriveConstants {
     public static final double kPHeadingCorrectionController = 5;
   }
 
+  public static final class IntakeConstants {
+    public static final int kIntakeMotorID = 25;
+    public static final int kArmMotorID = 39;
+    public static final int kArmEncoderChannel = 0;
+
+    public static final double kIntakeLoweredAngle = 0;
+    public static final double kIntakeRaisedAngle = 120;
+
+    public static final double kArmEncoderOffset = 0;
+
+    public static final double kIntakeSpeed = 3.0;
+
+    // TODO: Tune distance sensor threshold for detecting note
+    public static final double kDistanceSensorThreshold = 10;
+  }
+
   public static final class ShooterConstants {
     public static final int kTopShooterMotorPort = 35;
     public static final int kBottomShooterMotorPort = 20;

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

@@ -21,9 +21,12 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
 /*
@@ -36,7 +39,8 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
-
+  public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
+
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
 
@@ -52,7 +56,8 @@ public RobotContainer() {
             new PIDConstants(5, 0.0, 0.0), // Translation PID constants
             new PIDConstants(5, 0.0, 0.0), // Rotation PID constants
             DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
-            Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in meters. Distance from robot center to furthest module.
+            Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in meters. Distance
+                                                                               // from robot center to furthest module.
             new ReplanningConfig(true, true)),
         () -> false, m_robotDrive);
 
@@ -69,15 +74,15 @@ public RobotContainer() {
                     * (1 - m_driverController
                         .getLeftTriggerAxis()
                         * IOConstants.kSlowModeScalar),
-                    // * 0.8,
+                // * 0.8,
                 MathUtil.applyDeadband(
                     -m_driverController.getLeftX(),
                     IOConstants.kControllerDeadband)
                     * DriveConstants.kMaxSpeedMetersPerSecond
                     * (1 - m_driverController
                         .getLeftTriggerAxis()
                         * IOConstants.kSlowModeScalar),
-                    // * 0.8,
+                // * 0.8,
                 MathUtil.applyDeadband(
                     -m_driverController.getRightX(),
                     IOConstants.kControllerDeadband)
@@ -107,6 +112,10 @@ private void configureBindings() {
     new JoystickButton(m_operatorController, Button.kY.value)
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
+
+    new Trigger(() -> {
+      return m_driverController.getRightTriggerAxis() > 0.5;
+    }).whileTrue(new IntakeCommand(m_intakeSubsystem));
   }
 
   /**
@@ -126,9 +135,5 @@ public Command getAutonomousCommand() {
 
     // return AutoBuilder.followPath(autonPath);
     return null;
-
-    // PathPlannerAuto pathPlannerAuto = new PathPlannerAuto("New Auto");
-
-    // return pathPlannerAuto;
   }
 }

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

@@ -0,0 +1,43 @@
+// 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.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class IntakeCommand extends Command {
+  private IntakeSubsystem m_intakeSubsystem;
+
+  /** 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_intakeSubsystem.armExtend();
+    m_intakeSubsystem.intake();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_intakeSubsystem.armRetract();
+    m_intakeSubsystem.stopIntake();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -0,0 +1,112 @@
+// 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.subsystems;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.Rev2mDistanceSensor;
+import com.revrobotics.Rev2mDistanceSensor.Port;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
+
+public class IntakeSubsystem extends SubsystemBase {
+  private boolean deployed = false;
+  private boolean haveNote = false;
+
+  private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
+  private CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless);
+
+  private PIDController m_armPID = new PIDController(0.5, 0, 0);
+
+  private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
+
+  private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
+
+  private double m_intakeSpeed = 0;
+
+  /** Creates a new IntakeSubsystem */
+  public IntakeSubsystem() {
+    // m_armMotor.setInverted(IntakeConstants.kArmMotorInverted);
+    // m_intakeMotor.setInverted(IntakeConstants.kIntakeMotorInverted);
+
+    m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
+    m_armEncoder.setDistancePerRotation(2 * Math.PI);
+
+    m_intakeMotor.setIdleMode(IdleMode.kCoast);
+    m_armMotor.setIdleMode(IdleMode.kBrake);
+
+    m_armPID.setTolerance(0.05);
+
+    // TODO: See if this is needed
+    // m_distanceSensor.setAutomaticMode(true);
+  }
+
+  public void armExtend() {
+    m_armPID.setSetpoint(IntakeConstants.kIntakeLoweredAngle);
+
+    stopIntake();
+
+    deployed = true;
+  }
+
+  public void armRetract() {
+    m_armPID.setSetpoint(IntakeConstants.kIntakeRaisedAngle);
+
+    stopIntake();
+
+    deployed = false;
+  }
+
+  public void intake() {
+    if (deployed && !haveNote) {
+      m_intakeSpeed = IntakeConstants.kIntakeSpeed;
+    }
+  }
+
+  public void outake() {
+    if (!deployed && haveNote) {
+      m_intakeSpeed = -IntakeConstants.kIntakeSpeed;
+    }
+  }
+
+  public void stopIntake() {
+    m_intakeSpeed = 0;
+  }
+
+  /**
+   * Gets distance from Rev 2m sensor
+   * 
+   */
+  public double getDistanceSensor() {
+    return m_distanceSensor.getRange();
+  }
+
+  @Override
+  public void periodic() {
+    haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold;
+
+    // If we have a note and the arm is deployed, automatically bring it back in
+    if (haveNote && deployed) {
+      stopIntake();
+      armRetract();
+    }
+
+    m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition()));
+    m_intakeMotor.set(m_intakeSpeed);
+
+    SmartDashboard.putNumber("Arm Angle", m_armEncoder.getAbsolutePosition());
+    SmartDashboard.putBoolean("Arm Deployed?", deployed);
+    SmartDashboard.putBoolean("Have Note?", haveNote);
+  }
+
+  public boolean readyToShoot() {
+    return haveNote && !deployed && m_armPID.atSetpoint();
+  }
+}

vendordeps/DistanceSensor-java/.gitignore

@@ -0,0 +1 @@
+!*.jar