Add intake subsystem

Co-Authored-By: Caleb-J-Wallace <[email protected]> Co-Authored-By: Keegan Jamison Welch <[email protected]> Co-Authored-By: Adam Audycki <[email protected]>
Greater-Rochester-Robotics · Jan 19, 2024 · d784c38 · d784c38
1 parent 4001c0b
commit d784c38
Show file tree

Hide file tree

Showing 2 changed files with 105 additions and 0 deletions.
diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java
@@ -70,6 +70,26 @@ public static final class RobotMap {
         public static final int BACK_RIGHT_TURN = 7;
         public static final int FRONT_RIGHT_MOVE = 8;
         public static final int FRONT_RIGHT_TURN = 9;
+
+        public static final int INTAKE_DEPLOY_MOTOR = 20;
+        public static final int INTAKE_ROLLER_MOTOR = 21;
+
+        public static final int SHOOTER_PIVOT_MOTOR = 30;
+        public static final int SHOOTER_FEED_MOTOR = 31;
+        public static final int SHOOTER_LEFT_SHOOT_MOTOR = 32;
+        public static final int SHOOTER_RIGHT_SHOOT_MOTOR = 33;
+    }
+
+    public static final class IntakeConstants {
+
+        public static final PIDConfig INTAKE_DEPLOY_STRONG_PID = new PIDConfig(0.0, 0.0, 0.0);
+        public static final PIDConfig INTAKE_DEPLOY_WEAK_PID = new PIDConfig(0.0, 0.0, 0.0);
+
+        public static final double INTAKE_DEPLOY_POSITION = 0.0;
+        public static final double INTAKE_DEPLOY_POSITION_TOLERANCE = 0.0;
+
+        public static final double INTAKE_DEPLOY_ROLLER_SPEED = 0.0;
+        public static final double INTAKE_SPIT_ROLLER_SPEED = -0.0;
     }
 
     /**

diff --git a/src/main/java/org/team340/robot/subsystems/Intake.java b/src/main/java/org/team340/robot/subsystems/Intake.java
@@ -0,0 +1,85 @@
+package org.team340.robot.subsystems;
+
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkPIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team340.lib.GRRSubsystem;
+import org.team340.lib.util.Math2;
+import org.team340.lib.util.Mutable;
+import org.team340.robot.Constants.IntakeConstants;
+import org.team340.robot.Constants.RobotMap;
+
+/**
+ * The Intake subsystem.
+ */
+public class Intake extends GRRSubsystem {
+
+    private final CANSparkMax deployMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_DEPLOY_MOTOR, MotorType.kBrushless);
+    private final CANSparkMax rollerMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_MOTOR, MotorType.kBrushless);
+
+    private final SparkPIDController deployPID = deployMotor.getPIDController();
+
+    public Intake() {
+        super("Intake");
+        deployPID.setP(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.p(), 0);
+        deployPID.setI(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.i(), 0);
+        deployPID.setD(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.d(), 0);
+
+        deployPID.setP(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.p(), 1);
+        deployPID.setI(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.i(), 1);
+        deployPID.setD(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.d(), 1);
+    }
+
+    public Command deploy() {
+        Mutable<Boolean> weakPID = new Mutable<>(false);
+
+        return commandBuilder("intake.deploy()")
+            .onInitialize(() -> rollerMotor.set(IntakeConstants.INTAKE_DEPLOY_ROLLER_SPEED))
+            .onExecute(() -> {
+                if (
+                    Math2.epsilonEquals(
+                        IntakeConstants.INTAKE_DEPLOY_POSITION,
+                        deployMotor.getEncoder().getPosition(),
+                        IntakeConstants.INTAKE_DEPLOY_POSITION_TOLERANCE
+                    )
+                ) weakPID.set(true);
+                deployPID.setReference(IntakeConstants.INTAKE_DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
+            })
+            .onEnd(() -> {
+                rollerMotor.stopMotor();
+                deployMotor.stopMotor();
+            });
+    }
+
+    public Command retract() {
+        return commandBuilder("intake.retract()")
+            .onInitialize(() -> rollerMotor.stopMotor())
+            .onExecute(() -> {
+                deployPID.setReference(0, ControlType.kPosition, 0);
+            });
+    }
+
+    public Command spit() {
+        Mutable<Boolean> weakPID = new Mutable<>(false);
+
+        return commandBuilder("intake.spit()")
+            .onInitialize(() -> rollerMotor.set(IntakeConstants.INTAKE_SPIT_ROLLER_SPEED))
+            .onExecute(() -> {
+                if (
+                    Math2.epsilonEquals(
+                        IntakeConstants.INTAKE_DEPLOY_POSITION,
+                        deployMotor.getEncoder().getPosition(),
+                        IntakeConstants.INTAKE_DEPLOY_POSITION_TOLERANCE
+                    )
+                ) weakPID.set(true);
+
+                deployPID.setReference(IntakeConstants.INTAKE_DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
+            })
+            .onEnd(() -> {
+                rollerMotor.stopMotor();
+                deployMotor.stopMotor();
+            });
+    }
+}