From aef38aef8f5d07a004b97db7a79a8241f9e3050d Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sun, 21 Apr 2024 16:53:25 -0400
Subject: [PATCH] A very small amount of work on IntakePivot and IntakeRoller

---
 .../constants/IntakeRollerConstants.java      |  2 +
 .../frc/robot/subsystems/IntakePivot.java     | 43 ++++++++++++++++++-
 .../frc/robot/subsystems/IntakeRoller.java    | 41 +++---------------
 3 files changed, 50 insertions(+), 36 deletions(-)

diff --git a/src/main/java/frc/robot/constants/IntakeRollerConstants.java b/src/main/java/frc/robot/constants/IntakeRollerConstants.java
index 6f1cca8..61b097f 100644
--- a/src/main/java/frc/robot/constants/IntakeRollerConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java
@@ -2,4 +2,6 @@ package frc.robot.constants;
 
 public class IntakeRollerConstants {
     public static final int kMotorCANID = 12;
+
+    public static final int kMotorCurrentLimit = 60;
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java
index a5fb0a2..7fd30aa 100644
--- a/src/main/java/frc/robot/subsystems/IntakePivot.java
+++ b/src/main/java/frc/robot/subsystems/IntakePivot.java
@@ -1,7 +1,48 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IntakeRollerConstants;
+import frc.robot.constants.IntakePivotConstants;
 
 public class IntakePivot extends SubsystemBase {
-    
+    private CANSparkMax intakePivot;
+
+    private RelativeEncoder intakeEncoder;
+
+    private PIDController intakeAnglePID;
+
+    private ArmFeedforward intakeFeedForward;
+
+    private double armOffset;
+
+    public IntakePivot() {
+        intakePivot = new CANSparkMax(IntakePivotConstants.kIntakePivotID, MotorType.kBrushless);
+
+        intakePivot.setIdleMode(IdleMode.kBrake);
+        intakePivot.setSmartCurrentLimit(IntakePivotConstants.kPivotCurrentLimit);
+        intakePivot.setInverted(true);
+        intakePivot.burnFlash();
+
+        intakeEncoder = intakePivot.getEncoder();
+        intakeEncoder.setPosition(Units.radiansToRotations(IntakePivotConstants.kStartingAngle));
+
+        intakeAnglePID = new PIDController(
+            IntakePivotConstants.kP
+        );
+
+        intakeFeedForward = new ArmFeedforward(
+            IntakeRollerConstants.kSFeedForward, 
+            IntakeRollerConstants.kGFeedForward, 
+            IntakeRollerConstants.kVFeedForward
+        );
+
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeRoller.java b/src/main/java/frc/robot/subsystems/IntakeRoller.java
index b49358f..3c9224a 100644
--- a/src/main/java/frc/robot/subsystems/IntakeRoller.java
+++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java
@@ -15,49 +15,20 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakeRollerConstants;
 
-public class Intake extends SubsystemBase{
-    private PIDController intakeAnglePID;
-
+public class IntakeRoller extends SubsystemBase{
+    
     private CANSparkMax intakeRoller;
-    private CANSparkMax intakePivot;
 
-    private RelativeEncoder intakeEncoder;
-
-    private ArmFeedforward intakeFeedForward;
-
-    private double armOffset;
-
-    public Intake(){
+    public IntakeRoller(){
         armOffset = 0;
 
-        intakeRoller = new CANSparkMax(IntakeRollerConstants.kIntakeRollerID, MotorType.kBrushless);
+        intakeRoller = new CANSparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
         
-        intakeRoller.setSmartCurrentLimit(60);
+        intakeRoller.setSmartCurrentLimit(IntakeRollerConstants.kMotorCurrentLimit);
         intakeRoller.setIdleMode(IdleMode.kBrake);
         intakeRoller.burnFlash();
 
-        intakePivot = new CANSparkMax(IntakeRollerConstants.kIntakePivotID, MotorType.kBrushless);
-
-        intakePivot.setIdleMode(IdleMode.kBrake);
-        intakePivot.setSmartCurrentLimit(IntakeRollerConstants.kPivotCurrentLimit);
-        intakePivot.setInverted(true);
-        intakePivot.burnFlash();
-
-        intakeFeedForward = new ArmFeedforward(
-            IntakeRollerConstants.kSFeedForward, 
-            IntakeRollerConstants.kGFeedForward, 
-            IntakeRollerConstants.kVFeedForward
-        );
-
-        intakeEncoder = intakePivot.getEncoder();
-        intakeEncoder.setPosition(Units.radiansToRotations( IntakeRollerConstants.kStartingAngle));
-       // intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
-
-        intakeAnglePID = new PIDController(
-            IntakeRollerConstants.kPIntake, 
-            IntakeRollerConstants.kIIntake, 
-            IntakeRollerConstants.kDIntake
-        );
+        
 
         armOffset = getIntakeAngle()-IntakeRollerConstants.kStartingAngle;
     }