Added Intake and Kicker subsystems

team422 · Aug 23, 2024 · f0b8868 · f0b8868
1 parent 678542c
commit f0b8868
Show file tree

Hide file tree

Showing 13 changed files with 338 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -52,6 +52,18 @@ public static final class DriveConstants {
     public static final double kTurnGearRatio = 150.0 / 7.0;
   }
 
+  public static final class IntakeConstants {
+    public static final double kIdleVoltage = 0.0;
+    public static final double kIntakeVoltage = 7.0;
+    public static final double kOuttakeVoltage = -7.0;
+  }
+
+  public static final class KickerConstants {
+    public static final double kIdleVoltage = 0.0;
+    public static final double kShootingVoltage = 7.0;
+    public static final double kEjectingVoltage = -7.0;
+  }
+
   public static final class Ports {
     public static final int kFrontLeftDrive = 0;
     public static final int kFrontLeftTurn = 1;
@@ -68,5 +80,8 @@ public static final class Ports {
     public static final int kBackRightDrive = 9;
     public static final int kBackRightTurn = 10;
     public static final int kBackRightCancoder = 11;
+
+    public static final int kIntakeNeo = 1;
+    public static final int kKickerNeo = 2;
   }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -18,13 +18,20 @@
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.Ports;
 import frc.robot.commands.DriveCommands;
 import frc.robot.oi.DriverControls;
 import frc.robot.oi.DriverControlsPS5;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.Intake.IntakeState;
+import frc.robot.subsystems.intake.IntakeIONeo;
+import frc.robot.subsystems.kicker.Kicker;
+import frc.robot.subsystems.kicker.Kicker.KickerState;
+import frc.robot.subsystems.kicker.KickerIONeo;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -36,6 +43,8 @@
 public class RobotContainer {
   // Subsystems
   private Drive m_drive;
+  private Intake m_intake;
+  private Kicker m_kicker;
 
   // Controller
   private DriverControls m_driverControls;
@@ -63,6 +72,10 @@ private void configureSubsystems() {
               new ModuleIOTalonFX(1),
               new ModuleIOTalonFX(2),
               new ModuleIOTalonFX(3));
+
+      m_intake = new Intake(new IntakeIONeo(Ports.kIntakeNeo));
+
+      m_kicker = new Kicker(new KickerIONeo(Ports.kKickerNeo));
     } else {
       m_drive =
           new Drive(
@@ -73,7 +86,7 @@ private void configureSubsystems() {
               new ModuleIOSim());
     }
 
-    RobotState.start(m_drive);
+    RobotState.start(m_drive, m_intake, m_kicker);
     m_robotState = RobotState.getInstance();
   }
 
@@ -107,6 +120,47 @@ private void configureButtonBindings() {
                             new Pose2d(m_drive.getPose().getTranslation(), new Rotation2d())),
                     m_drive)
                 .ignoringDisable(true));
+
+    m_driverControls
+        .runIntake()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_intake.updateState(IntakeState.kIntaking);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_intake.updateState(IntakeState.kIdle);
+                }));
+
+    m_driverControls
+        .runKicker()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_kicker.updateState(KickerState.kShooting);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_kicker.updateState(KickerState.kIdle);
+                }));
+
+    m_driverControls
+        .ejectGamePiece()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_kicker.updateState(KickerState.kEjecting);
+                  m_intake.updateState(IntakeState.kOuttaking);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_kicker.updateState(KickerState.kIdle);
+                  m_intake.updateState(IntakeState.kIdle);
+                }));
   }
 
   /**

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -1,20 +1,26 @@
 package frc.robot;
 
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.kicker.Kicker;
 
 @SuppressWarnings("unused")
 public class RobotState {
   private static RobotState m_instance;
 
   // Subsystems
   private Drive m_drive;
+  private Intake m_intake;
+  private Kicker m_kicker;
 
-  private RobotState(Drive drive) {
+  private RobotState(Drive drive, Intake intake, Kicker kicker) {
     m_drive = drive;
+    m_intake = intake;
+    m_kicker = kicker;
   }
 
-  public static void start(Drive drive) {
-    m_instance = new RobotState(drive);
+  public static void start(Drive drive, Intake intake, Kicker kicker) {
+    m_instance = new RobotState(drive, intake, kicker);
   }
 
   public static RobotState getInstance() {

diff --git a/src/main/java/frc/robot/oi/DriverControls.java b/src/main/java/frc/robot/oi/DriverControls.java
@@ -10,4 +10,10 @@ public interface DriverControls {
   public double getTurn();
 
   public Trigger resetOdometry();
+
+  public Trigger runIntake();
+
+  public Trigger runKicker();
+
+  public Trigger ejectGamePiece();
 }
diff --git a/src/main/java/frc/robot/oi/DriverControlsPS5.java b/src/main/java/frc/robot/oi/DriverControlsPS5.java
@@ -29,4 +29,19 @@ public double getTurn() {
   public Trigger resetOdometry() {
     return m_controller.povUp();
   }
+
+  @Override
+  public Trigger runIntake() {
+    return m_controller.R2();
+  }
+
+  @Override
+  public Trigger runKicker() {
+    return m_controller.R1();
+  }
+
+  @Override
+  public Trigger ejectGamePiece() {
+    return m_controller.cross();
+  }
 }
diff --git a/src/main/java/frc/robot/oi/DriverControlsXbox.java b/src/main/java/frc/robot/oi/DriverControlsXbox.java
@@ -29,4 +29,19 @@ public double getTurn() {
   public Trigger resetOdometry() {
     return m_controller.povUp();
   }
+
+  @Override
+  public Trigger runIntake() {
+    return m_controller.rightTrigger();
+  }
+
+  @Override
+  public Trigger runKicker() {
+    return m_controller.rightBumper();
+  }
+
+  @Override
+  public Trigger ejectGamePiece() {
+    return m_controller.b();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -0,0 +1,50 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.util.SubsystemProfiles;
+import java.util.HashMap;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+  private IntakeIO m_io;
+  public final IntakeInputsAutoLogged m_inputs;
+  private SubsystemProfiles m_profiles;
+
+  public enum IntakeState {
+    kIdle,
+    kIntaking,
+    kOuttaking
+  }
+
+  public Intake(IntakeIO io) {
+    m_io = io;
+    m_inputs = new IntakeInputsAutoLogged();
+
+    m_profiles = new SubsystemProfiles(IntakeState.class, new HashMap<>(), IntakeState.kIdle);
+  }
+
+  @Override
+  public void periodic() {
+    m_io.updateInputs(m_inputs);
+
+    Logger.processInputs("Intake", m_inputs);
+    Logger.recordOutput("Intake/CurrentState", (IntakeState) m_profiles.getCurrentProfile());
+  }
+
+  public void updateState(IntakeState state) {
+    switch (state) {
+      case kIdle:
+        m_io.setVoltage(IntakeConstants.kIdleVoltage);
+        break;
+      case kIntaking:
+        m_io.setVoltage(IntakeConstants.kIntakeVoltage);
+        break;
+      case kOuttaking:
+        m_io.setVoltage(IntakeConstants.kOuttakeVoltage);
+        break;
+    }
+
+    m_profiles.setCurrentProfile(state);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeInputs {
+    public double angularVelocityRPM;
+    public double voltage;
+    public double current;
+  }
+
+  public void updateInputs(IntakeInputs inputs);
+
+  public void setVoltage(double voltage);
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIONeo.java b/src/main/java/frc/robot/subsystems/intake/IntakeIONeo.java
@@ -0,0 +1,27 @@
+package frc.robot.subsystems.intake;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+public class IntakeIONeo implements IntakeIO {
+  private CANSparkMax m_motor;
+  private RelativeEncoder m_encoder;
+
+  public IntakeIONeo(int port) {
+    m_motor = new CANSparkMax(port, MotorType.kBrushless);
+    m_encoder = m_motor.getEncoder();
+  }
+
+  @Override
+  public void updateInputs(IntakeInputs inputs) {
+    inputs.angularVelocityRPM = m_encoder.getVelocity();
+    inputs.voltage = m_motor.getBusVoltage() * m_motor.getAppliedOutput();
+    inputs.current = m_motor.getOutputCurrent();
+  }
+
+  @Override
+  public void setVoltage(double voltage) {
+    m_motor.setVoltage(voltage);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/kicker/Kicker.java b/src/main/java/frc/robot/subsystems/kicker/Kicker.java
@@ -0,0 +1,50 @@
+package frc.robot.subsystems.kicker;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.KickerConstants;
+import frc.robot.util.SubsystemProfiles;
+import java.util.HashMap;
+import org.littletonrobotics.junction.Logger;
+
+public class Kicker extends SubsystemBase {
+  private KickerIO m_io;
+  public final KickerInputsAutoLogged m_inputs;
+  private SubsystemProfiles m_profiles;
+
+  public enum KickerState {
+    kIdle,
+    kShooting,
+    kEjecting
+  }
+
+  public Kicker(KickerIO io) {
+    m_io = io;
+    m_inputs = new KickerInputsAutoLogged();
+
+    m_profiles = new SubsystemProfiles(KickerState.class, new HashMap<>(), KickerState.kIdle);
+  }
+
+  @Override
+  public void periodic() {
+    m_io.updateInputs(m_inputs);
+
+    Logger.processInputs("Kicker", m_inputs);
+    Logger.recordOutput("Kicker/State", (KickerState) m_profiles.getCurrentProfile());
+  }
+
+  public void updateState(KickerState state) {
+    switch (state) {
+      case kIdle:
+        m_io.setVoltage(KickerConstants.kIdleVoltage);
+        break;
+      case kShooting:
+        m_io.setVoltage(KickerConstants.kShootingVoltage);
+        break;
+      case kEjecting:
+        m_io.setVoltage(KickerConstants.kEjectingVoltage);
+        break;
+    }
+
+    m_profiles.setCurrentProfile(state);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems.kicker;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface KickerIO {
+  @AutoLog
+  public static class KickerInputs {
+    public double angularVelocityRPM;
+    public double voltage;
+    public double current;
+  }
+
+  public void updateInputs(KickerInputs inputs);
+
+  public void setVoltage(double voltage);
+}
diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIONeo.java b/src/main/java/frc/robot/subsystems/kicker/KickerIONeo.java
@@ -0,0 +1,27 @@
+package frc.robot.subsystems.kicker;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+public class KickerIONeo implements KickerIO {
+  private CANSparkMax m_motor;
+  private RelativeEncoder m_encoder;
+
+  public KickerIONeo(int port) {
+    m_motor = new CANSparkMax(port, MotorType.kBrushless);
+    m_encoder = m_motor.getEncoder();
+  }
+
+  @Override
+  public void updateInputs(KickerInputs inputs) {
+    inputs.angularVelocityRPM = m_encoder.getVelocity();
+    inputs.voltage = m_motor.getAppliedOutput() * m_motor.getBusVoltage();
+    inputs.current = m_motor.getOutputCurrent();
+  }
+
+  @Override
+  public void setVoltage(double voltage) {
+    m_motor.setVoltage(voltage);
+  }
+}