Intake commands (#25)

Frc5572 · Jan 30, 2024 · 953cddf · 953cddf
1 parent 029736b
commit 953cddf
Show file tree

Hide file tree

Showing 6 changed files with 72 additions and 23 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -40,8 +40,8 @@ public static final class Motors {
          * Shooter Id's
          */
         public static final class Shooter {
-            public static final int shooterTopId = 0;
-            public static final int shooterBottomId = 0;
+            public static final int SHOOTER_TOP_ID = 0;
+            public static final int SHOOTER_BOTTOM_ID = 0;
         }
 
         /**
@@ -260,13 +260,20 @@ public static final class PID {
         public static final class SetPoints {
 
             public static final double HOME_HEIGHT = 0;
-            public static final double HOME_ANGLE = 0;
-            public static final double AMP_HEIGHT = 0;
-            public static final double AMP_ANGLE = 0;
-            public static final double TRAP_HEIGHT = 0;
-            public static final double TRAP_ANGLE = 0;
-            public static final double CLIMBING_HEIGHT = 0;
-            public static final double CLIMBING_ANGLE = 0;
+            public static final Rotation2d HOME_ANGLE = Rotation2d.fromDegrees(40);
+            public static final double AMP_HEIGHT = Units.inchesToMeters(34);
+            public static final Rotation2d AMP_ANGLE = Rotation2d.fromDegrees(-10);
+            public static final double TRAP_HEIGHT = Units.inchesToMeters(40);
+            public static final Rotation2d TRAP_ANGLE = Rotation2d.fromDegrees(30);
+
+            public static final double CLIMBING_HEIGHT = Units.inchesToMeters(0);
+            public static final Rotation2d CLIMBING_ANGLE = Rotation2d.fromDegrees(0);
+
+            public static final double MAX_EXTENSION = Units.inchesToMeters(48);
+            public static final Rotation2d MAX_ANGLE_UP_HOME = Rotation2d.fromDegrees(85);
+            public static final Rotation2d MAX_ANGLE_DOWN_HOME = Rotation2d.fromDegrees(-15);
+            public static final Rotation2d MAX_ANGLE_UP_EXTENDED = Rotation2d.fromDegrees(85);
+            public static final Rotation2d MAX_ANGLE_DOWN_EXTENDED = Rotation2d.fromDegrees(-15);
         }
 
 
@@ -284,10 +291,23 @@ public static final class Pneumatics {
      * Constants of Shooters
      */
     public static final class ShooterConstants {
-        public static final double kP = 0;
-        public static final double kI = 0;
-        public static final double kD = 0;
-        public static final double kS = 0;
-        public static final double kV = 0;
+        public static final double KP = 0;
+        public static final double KI = 0;
+        public static final double KD = 0;
+        public static final double KS = 0;
+        public static final double KV = 0;
+    }
+
+    /**
+     * Constants for intake
+     */
+    public static final class IntakeConstants {
+        public static final double INTAKE_MOTOR_FORWARD = 0;
+        public static final double INTAKE_MOTOR_BACKWARD = -0;
+        public static final double INTAKE_MOTOR_STOP = 0;
+        public static final double INDEX_MOTOR_FORWARD = 0;
+        public static final double INDEX_MOTOR_BACKWARD = -0;
+        public static final double INDEX_MOTOR_STOP = 0;
+
     }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -79,7 +79,6 @@ public RobotContainer(RobotRunType runtimeType) {
     private void configureButtonBindings() { /* Driver Buttons */
         /* Driver Buttons */
         driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
-        operator.a().whileTrue(intake.runIntakeMotor());
     }
 
     /**

diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java
@@ -0,0 +1,30 @@
+package frc.robot.commands;
+
+import java.util.function.BooleanSupplier;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
+import frc.robot.subsystems.intake.Intake;
+
+/**
+ * File to create commands using factories
+ */
+public class CommandFactory {
+
+    /**
+     * Checks to make sure all preconditions are met before running the intake
+     *
+     * @param intake Intake subsystem
+     * @param elevatorWrist Elevator and Wrist subsystem
+     * @return Returns a command
+     */
+    public static Command runIntake(Intake intake, ElevatorWrist elevatorWrist) {
+        BooleanSupplier sensor = () -> intake.getSensorStatus();
+        Command moveElevatorWrist =
+            elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
+                Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE);
+        Command runIntakeIndexer =
+            intake.runIntakeMotor(Constants.IntakeConstants.INDEX_MOTOR_FORWARD);
+        return moveElevatorWrist.andThen(runIntakeIndexer).unless(sensor);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -42,10 +42,10 @@ public boolean getSensorStatus() {
      *
      * @return {@link Command} to run the intake and indexer motors
      */
-    public Command runIntakeMotor() {
+    public Command runIntakeMotor(double speed) {
         return Commands.startEnd(() -> {
-            setIntakeMotor(0.5);
-            setIndexerMotor(0.5);
+            setIntakeMotor(speed);
+            setIndexerMotor(speed);
         }, () -> {
             setIntakeMotor(0);
             setIndexerMotor(0);

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -11,10 +11,10 @@
  */
 public class Shooter extends SubsystemBase {
     private ShooterIO io;
-    private PIDController pid = new PIDController(Constants.ShooterConstants.kP,
-        Constants.ShooterConstants.kI, Constants.ShooterConstants.kD);
+    private PIDController pid = new PIDController(Constants.ShooterConstants.KP,
+        Constants.ShooterConstants.KI, Constants.ShooterConstants.KD);
     private SimpleMotorFeedforward shooterFeed =
-        new SimpleMotorFeedforward(Constants.ShooterConstants.kS, Constants.ShooterConstants.kV);
+        new SimpleMotorFeedforward(Constants.ShooterConstants.KS, Constants.ShooterConstants.KV);
     private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
     public Shooter(ShooterIO io) {

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterVortex.java b/src/main/java/frc/robot/subsystems/shooter/ShooterVortex.java
@@ -10,9 +10,9 @@
  */
 public class ShooterVortex implements ShooterIO {
     public final CANSparkFlex shooterTopMotor =
-        new CANSparkFlex(Constants.Motors.Shooter.shooterTopId, MotorType.kBrushless);
+        new CANSparkFlex(Constants.Motors.Shooter.SHOOTER_TOP_ID, MotorType.kBrushless);
     public final CANSparkFlex shooterBottomMotor =
-        new CANSparkFlex(Constants.Motors.Shooter.shooterBottomId, MotorType.kBrushless);
+        new CANSparkFlex(Constants.Motors.Shooter.SHOOTER_BOTTOM_ID, MotorType.kBrushless);
     private RelativeEncoder topEncoder;
     private RelativeEncoder bottomEncoder;