Intake subsystem

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

@@ -145,7 +145,7 @@ public static final class ShooterConstants {
     public static final int kWinchMotorCanId = 54;
 
     public static final double kFrontShootPower = 1.0;
-    public static final double kBackShootPower = 1.0;
+    public static final double kBackShootPower = -1.0;
     public static final double kFrontIntakePower = -0.35;
     public static final double kBackIntakePower = -0.30;
     public static final double kIndexIntakePower = 0.1;
@@ -170,4 +170,11 @@ public static final class LimelightConstants {
     public static final double kCamHeight = 0; // Height of the limelight from the ground
     public static final double kCamAngle = 0; // Pitch angle of direction the limelight is pointed in
   }
+
+  public static final class IntakeConstants {
+    public static final int kFlippyCanId = 1;
+    public static final int kGroundIntakeCanId = 2;
+    public static final int kIntakeMaxCurrent = 10;
+    public static final double kFlipperGearRatio = 45;
+  }
 }

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

@@ -26,11 +26,17 @@
 import frc.robot.commands.auto.ShootAutoCommand;
 import frc.robot.commands.auto.TurnAndShootAutoCommand;
 import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.commands.AirIntakeCommand;
 import frc.robot.commands.ClimbCommand;
+import frc.robot.commands.ExpelRingCommand;
+import frc.robot.commands.FeedShooterCommand;
+import frc.robot.commands.GroundIntakeCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.ResetWinchCommand;
+import frc.robot.commands.RetractIntakeCommand;
 import frc.robot.commands.ShootCommand;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -53,6 +59,7 @@ public class RobotContainer {
   private boolean fastMode = false;
   private boolean fasterMode = false;
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
 
   // Auto
   private final ShuffleboardTab m_tab;
@@ -74,6 +81,7 @@ public RobotContainer() {
             this::getPOV));
     m_climberSubsystem.setDefaultCommand(new ClimbCommand(m_climberSubsystem, this::getAuxRightY, this::getAuxLeftY));
     m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed));
+    m_intakeSubsystem.setDefaultCommand(new RetractIntakeCommand(m_intakeSubsystem));
 
     // Auto chooser setup
     m_tab = Shuffleboard.getTab("Auto");
@@ -92,7 +100,7 @@ public RobotContainer() {
       new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive back only",
       new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto
-    m_chosenAuto.addOption("Shoot only", 
+    m_chosenAuto.addOption("Shoot only",
       new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0)).withTimeout(4));
 
     m_tab.add(m_chosenAuto);
@@ -120,6 +128,14 @@ private void configureButtonBindings() {
     // Driver stuff
     new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) // Reset gyro
       .whileTrue(new ZeroHeadingCommand(m_robotDrive));
+    new JoystickButton(m_driverController, XboxController.Button.kA.value)
+      .whileTrue(new GroundIntakeCommand(m_intakeSubsystem));
+    new JoystickButton(m_driverController, XboxController.Button.kB.value)
+      .whileTrue(new ExpelRingCommand(m_intakeSubsystem));
+    new JoystickButton(m_driverController, XboxController.Button.kX.value)
+      .whileTrue(new FeedShooterCommand(m_intakeSubsystem));
+    new JoystickButton(m_driverController, XboxController.Button.kY.value)
+      .whileTrue(new AirIntakeCommand(m_intakeSubsystem));
       // Left bumper = Toggle fastmode
       // Left trigger = Toggle fastermode
       // POV = Nudge
@@ -159,9 +175,7 @@ boolean getFasterMode() {
     else fasterMode = false;
     return fasterMode;
   }
-  double getRightX() {
-    System.out.println("DELAY: " + m_delay.getDouble(0));
-    return m_driverController.getRightX();}
+  double getRightX() {return m_driverController.getRightX();}
   double getLeftX() {return -m_driverController.getLeftX();}
   double getLeftY() {return -m_driverController.getLeftY();}
   double getPOV() {return m_driverController.getPOV();}

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

@@ -0,0 +1,35 @@
+// 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 AirIntakeCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public AirIntakeCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.intake(1);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,35 @@
+// 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 ExpelRingCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public ExpelRingCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.expelRing();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,35 @@
+// 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 FeedShooterCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public FeedShooterCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.feedShooter();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,35 @@
+// 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 GroundIntakeCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public GroundIntakeCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.intakeFromGround();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,35 @@
+// 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 RetractIntakeCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public RetractIntakeCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.retractIntake();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,91 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.SparkRelativeEncoder.Type;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
+
+public class IntakeSubsystem extends SubsystemBase {
+
+  public final CANSparkMax m_flippyMotor;
+  public final CANSparkMax m_intakeMotor;
+  public final RelativeEncoder m_flippyEncoder;
+
+  public final ShuffleboardTab m_tab;
+
+  public IntakeSubsystem() {
+
+    //Intake mechanism motors
+    m_flippyMotor = new CANSparkMax(IntakeConstants.kFlippyCanId, MotorType.kBrushless);
+    m_intakeMotor = new CANSparkMax(IntakeConstants.kGroundIntakeCanId, MotorType.kBrushed);
+    m_flippyEncoder = m_flippyMotor.getEncoder(Type.kHallSensor, 42);
+
+    m_flippyMotor.setIdleMode(IdleMode.kCoast);
+    m_intakeMotor.setIdleMode(IdleMode.kBrake);
+
+    m_tab = Shuffleboard.getTab("Intake");
+    m_tab.addNumber("Intake Current", this::getIntakeCurrent);
+    m_tab.addNumber("Flipper Position", this::getFlipperEncoder);
+    m_tab.addNumber("Flipper Current", this::getFlipperAmps);
+
+    resetFlipperEncoder();
+  }
+
+  //Intake
+  public void intake(double power) {m_intakeMotor.set(-power);}
+  public void stopIntake() {m_intakeMotor.stopMotor();}
+  public double getIntakeCurrent() {return m_intakeMotor.getOutputCurrent();}
+  public boolean intakeCurrentSpike() {return getIntakeCurrent() >= IntakeConstants.kIntakeMaxCurrent;}
+  public void intakeCurrentSensitive(double power) {
+    if (!intakeCurrentSpike()) intake(power);
+    else stopIntake();
+  }
+
+  //Flipper
+  public void flip(double power) {m_flippyMotor.set(-power);}
+  public void stopFlipper() {m_flippyMotor.stopMotor();}
+  public void flipToPosition(double pos) {
+    // Moves to the goal encoder angle
+    flip(MathUtil.clamp(pos - m_flippyEncoder.getPosition(), -1, 1));
+  }
+  public double getFlipperEncoder() {return -m_flippyEncoder.getPosition() / IntakeConstants.kFlipperGearRatio;}
+  public void resetFlipperEncoder() {m_flippyEncoder.setPosition(0);}
+  public double getFlipperAmps(){return m_flippyMotor.getOutputCurrent();}
+
+  //Combo
+  public void intakeFromGround() {
+    if (getFlipperEncoder() < .3) {
+      flip(.3);
+      stopIntake();
+    }
+    else {
+      intake(1);
+      stopFlipper();
+    }
+  }
+  public void retractIntake() {
+    if (getFlipperEncoder() > .1) flip(-.3);
+    else stopFlipper();
+  }
+  public void expelRing() {
+    intake(-1);
+  }
+  public void feedShooter() {
+    retractIntake();
+    if (getFlipperEncoder() < .1) expelRing();
+    else stopFlipper();
+  }
+  public void stopEverything() {
+    stopFlipper();
+    stopIntake();
+  }
+
+
+}