created a PID for the pivot (im geekin)

src/main/java/frc4388/robot/Constants.java

@@ -164,6 +164,10 @@ public final class Constants {
         public static final double INTAKE_SPEED = 0.2; //TODO:
         public static final double HANDOFF_SPEED = 0.2; //TODO:
         public static final double PIVOT_SPEED = 0.2; //TODO:
+
+        public static final class ArmPID {
+            public static final Gains INTAKE_GAINS = new Gains(0, 0, 0, 0, 0, 1.0);
+        }
     }
 
     public static final class OIConstants {

src/main/java/frc4388/robot/RobotContainer.java

@@ -123,6 +123,7 @@ public class RobotContainer {
          new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
              .onTrue(new InstantCommand(() ->  new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake))
              .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter));
+             
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor()))
             .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors()));

src/main/java/frc4388/robot/subsystems/Intake.java

@@ -6,21 +6,43 @@ package frc4388.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkLimitSwitch;
+import com.revrobotics.SparkPIDController;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.wpilibj.CAN;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
 import frc4388.robot.Constants.IntakeConstants;
+import frc4388.robot.commands.PID;
+import frc4388.utility.Gains;
 
 public class Intake extends SubsystemBase {
   
   private CANSparkMax intakeMotor;
   private CANSparkMax pivot;
+  private SparkPIDController m_spedController;
+  private static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
+  private SparkLimitSwitch forwardLimit;
+  private SparkLimitSwitch reverseLimit;
 
   /** Creates a new Intake. */
   public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
     this.intakeMotor = intakeMotor;
     this.pivot = pivot;
+
+    pivot.restoreFactoryDefaults();
+
+    forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed);
+    reverseLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed);
+    forwardLimit.enableLimitSwitch(true);
+    reverseLimit.enableLimitSwitch(true);
+    
+
+    //Arm PID
+    m_spedController = intakeMotor.getPIDController();
+    m_spedController.setP(armGains.kP);
+    m_spedController.setI(armGains.kI);
+    m_spedController.setD(armGains.kD);
   }
 
   //hanoff