made PID, commented out all of the auto stuff cuz it broke the code (im tweakin)

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

@@ -10,6 +10,7 @@ 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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
 import frc4388.robot.Constants.IntakeConstants;
@@ -31,15 +32,16 @@ public class Intake extends SubsystemBase {
     this.pivot = pivot;
 
     pivot.restoreFactoryDefaults();
+    //pivot.setInverted(true);
 
-    forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed);
-    reverseLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed);
+    forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
+    reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
     forwardLimit.enableLimitSwitch(true);
     reverseLimit.enableLimitSwitch(true);
     
 
     //Arm PID
-    m_spedController = intakeMotor.getPIDController();
+    m_spedController = pivot.getPIDController();
     m_spedController.setP(armGains.kP);
     m_spedController.setI(armGains.kI);
     m_spedController.setD(armGains.kD);
@@ -52,6 +54,7 @@ public class Intake extends SubsystemBase {
 
   //Rotate robot in for handoff
   public void rotateArmIn() {
+    //pivot.set(IntakeConstants.PIVOT_SPEED);
     pivot.set(IntakeConstants.PIVOT_SPEED);
   }
 
@@ -61,6 +64,15 @@ public class Intake extends SubsystemBase {
 
   }
 
+  public void pidIn() {
+    m_spedController.setReference(8000, CANSparkMax.ControlType.kVelocity);
+    //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
+  }
+
+  public void pidOut() {
+
+  }
+
   public void handoff() {
     intakeMotor.set(-IntakeConstants.INTAKE_SPEED);
   }
@@ -68,12 +80,36 @@ public class Intake extends SubsystemBase {
   public void stopIntakeMotors() {
     intakeMotor.set(0);
   }
+
+  public void stopArmMotor() {
+    pivot.set(0);
+  }
+
   public RelativeEncoder getEncoder() {
     return pivot.getEncoder();
   }
   public void setVoltage(double voltage) {
    pivot.setVoltage(voltage);
   }
+
+  public double getVelocity() {
+    return pivot.getEncoder().getVelocity();
+  }
+
+  public void resetPostion() {
+    pivot.getEncoder().setPosition(0);
+  }
+
+  public void resetPosition1() {
+    if(forwardLimit.isPressed() == true) {
+      resetPostion();
+    }
+  }
+
+  public double getPos() {
+    return pivot.getEncoder().getPosition();
+  }
+
   public void rotateArm() {
 
   }
@@ -81,5 +117,8 @@ public class Intake extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Vel Output", getVelocity());
+    SmartDashboard.putNumber("Position", getPos());
+    resetPosition1();
   }
 }