From 5ceca118c72dbe935fb7f1e2beb76b694f492146 Mon Sep 17 00:00:00 2001
From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com>
Date: Wed, 7 Feb 2024 18:53:57 -0700
Subject: [PATCH] created a PID for the pivot (im geekin)

---
 src/main/java/frc4388/robot/Constants.java    |  4 ++++
 .../java/frc4388/robot/RobotContainer.java    |  1 +
 .../java/frc4388/robot/subsystems/Intake.java | 22 +++++++++++++++++++
 3 files changed, 27 insertions(+)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3dc5352..e6153f2 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/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 {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b485a53..fd86c37 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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()));
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 11ece07..27b83bb 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/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