From 1faf05f85b88a1e4eb3f7860f20c902d3f7beda9 Mon Sep 17 00:00:00 2001
From: Ronan-B <rdavidbouza@gmail.com>
Date: Thu, 23 Jan 2025 09:24:51 -0800
Subject: [PATCH 1/9] Created subsystem (WIP)

---
 .../java/frc/robot/subsystems/EndEffector.java  | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/EndEffector.java

diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
new file mode 100644
index 0000000..96d8e8d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class EndEffector extends SubsystemBase {
+  /** Creates a new EndEffector. */
+  public EndEffector() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From 2d1118122589f21ea8b0a902c560991fabf2f1ab Mon Sep 17 00:00:00 2001
From: Ronan-B <rdavidbouza@gmail.com>
Date: Thu, 23 Jan 2025 10:08:52 -0800
Subject: [PATCH 2/9] WIP

---
 src/main/java/frc/robot/constants/CAN.java          |  2 ++
 src/main/java/frc/robot/constants/ENDEFFECTOR.java  |  8 ++++++++
 src/main/java/frc/robot/subsystems/EndEffector.java | 11 ++++++++++-
 3 files changed, 20 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/constants/ENDEFFECTOR.java

diff --git a/src/main/java/frc/robot/constants/CAN.java b/src/main/java/frc/robot/constants/CAN.java
index dfdc0f7..ce1d22f 100644
--- a/src/main/java/frc/robot/constants/CAN.java
+++ b/src/main/java/frc/robot/constants/CAN.java
@@ -22,4 +22,6 @@ public class CAN {
 
   public static final int coralOuttakeMotor = 30;
   public static final int algaeMotor = 31;
+  public static final int EndEffector =35;
+
 }
diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
new file mode 100644
index 0000000..64db4de
--- /dev/null
+++ b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
@@ -0,0 +1,8 @@
+package frc.robot.constants;
+
+public class ENDEFFECTOR {
+  public static final double kP = 0.0;
+  public static final double kI = 0.0;
+  public static final double kD = 0.0;
+  public static final double endEffectorGearRatio = 1.0;
+}
diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
index 96d8e8d..ed58531 100644
--- a/src/main/java/frc/robot/subsystems/EndEffector.java
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -4,11 +4,20 @@
 
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.CAN;
 
 public class EndEffector extends SubsystemBase {
+  private final TalonFX m_endEffectorMotor = new TalonFX(CAN.EndEffector);
+  
   /** Creates a new EndEffector. */
-  public EndEffector() {}
+  public EndEffector() {
+TalonFXConfiguration m_endEffectorMotorconfig = new TalonFXConfiguration();
+
+  }
 
   @Override
   public void periodic() {

From 823a1bd9a8482e3edd01c3d7d197ac98687ac089 Mon Sep 17 00:00:00 2001
From: Ronan-B <rdavidbouza@gmail.com>
Date: Sat, 25 Jan 2025 12:30:54 -0800
Subject: [PATCH 3/9] Subsystem WIP

---
 src/main/java/frc/robot/constants/CAN.java      |  3 +--
 .../java/frc/robot/subsystems/EndEffector.java  | 17 ++++++++++++++---
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/constants/CAN.java b/src/main/java/frc/robot/constants/CAN.java
index ce1d22f..b6609c6 100644
--- a/src/main/java/frc/robot/constants/CAN.java
+++ b/src/main/java/frc/robot/constants/CAN.java
@@ -22,6 +22,5 @@ public class CAN {
 
   public static final int coralOuttakeMotor = 30;
   public static final int algaeMotor = 31;
-  public static final int EndEffector =35;
-
+  public static final int EndEffector = 35;
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
index ed58531..03a70a7 100644
--- a/src/main/java/frc/robot/subsystems/EndEffector.java
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -6,17 +6,28 @@
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
-
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.CAN;
+import frc.robot.constants.ENDEFFECTOR;
+import frc.robot.utils.CtreUtils;
 
 public class EndEffector extends SubsystemBase {
   private final TalonFX m_endEffectorMotor = new TalonFX(CAN.EndEffector);
-  
+
   /** Creates a new EndEffector. */
   public EndEffector() {
-TalonFXConfiguration m_endEffectorMotorconfig = new TalonFXConfiguration();
+    TalonFXConfiguration m_endEffectorMotorconfig = new TalonFXConfiguration();
+    m_endEffectorMotorconfig.Slot0.kP = ENDEFFECTOR.kP;
+    m_endEffectorMotorconfig.Slot0.kI = ENDEFFECTOR.kI;
+    m_endEffectorMotorconfig.Slot0.kD = ENDEFFECTOR.kD;
+    m_endEffectorMotorconfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    m_endEffectorMotorconfig.Feedback.SensorToMechanismRatio = ENDEFFECTOR.endEffectorGearRatio;
+    CtreUtils.configureTalonFx(m_endEffectorMotor, m_endEffectorMotorconfig);
+  }
 
+  public void setPercentOutput(double output) {
+    m_endEffectorMotor.set(output);
   }
 
   @Override

From 080e1919276bcb1c0320d58d433e0477c59f5150 Mon Sep 17 00:00:00 2001
From: Ronan-B <rdavidbouza@gmail.com>
Date: Sat, 25 Jan 2025 15:30:25 -0800
Subject: [PATCH 4/9] Added Comand

---
 .../robot/commands/RunEndEffectorIntake.java  | 45 ++++++++++
 ...enix6-25.1.0.json => Phoenix6-25.2.1.json} | 84 +++++++++++++------
 2 files changed, 102 insertions(+), 27 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/RunEndEffectorIntake.java
 rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.1.json} (85%)

diff --git a/src/main/java/frc/robot/commands/RunEndEffectorIntake.java b/src/main/java/frc/robot/commands/RunEndEffectorIntake.java
new file mode 100644
index 0000000..078eb97
--- /dev/null
+++ b/src/main/java/frc/robot/commands/RunEndEffectorIntake.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeIntake;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class RunAlgaeIntake extends Command {
+
+  private final AlgaeIntake m_algaeIntake;
+  private final double m_speed;
+
+  /** Creates a new SetAlgaeIntakeSpeed. */
+  public RunAlgaeIntake(AlgaeIntake algae, double speed) {
+    m_algaeIntake = algae;
+    m_speed = speed;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_algaeIntake);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_algaeIntake.setPercentOutput(m_speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_algaeIntake.setPercentOutput(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.1.json
similarity index 85%
rename from vendordeps/Phoenix6-25.1.0.json
rename to vendordeps/Phoenix6-25.2.1.json
index 473f6a8..1397da1 100644
--- a/vendordeps/Phoenix6-25.1.0.json
+++ b/vendordeps/Phoenix6-25.2.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.1.0.json",
+    "fileName": "Phoenix6-25.2.1.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.2.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +351,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,

From 7546002a57f5e3a93b80ab5cc44c27524fa667d5 Mon Sep 17 00:00:00 2001
From: Ronan-B <rdavidbouza@gmail.com>
Date: Sun, 26 Jan 2025 15:11:07 -0800
Subject: [PATCH 5/9] Made basic command (Needs testing)

---
 src/main/java/frc/robot/RobotContainer.java      |  7 +++++--
 .../frc/robot/commands/RunEndEffectorIntake.java | 16 ++++++++--------
 src/main/java/frc/robot/constants/CAN.java       |  2 +-
 .../java/frc/robot/subsystems/EndEffector.java   |  2 +-
 4 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ba1826e..39dc2a1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -21,6 +21,7 @@
 import frc.robot.commands.ResetGyro;
 import frc.robot.commands.RunAlgaeIntake;
 import frc.robot.commands.RunCoralOuttake;
+import frc.robot.commands.RunEndEffectorIntake;
 import frc.robot.commands.SwerveCharacterization;
 import frc.robot.constants.ROBOT;
 import frc.robot.constants.SWERVE;
@@ -30,6 +31,7 @@
 import frc.robot.subsystems.AlgaeIntake;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.CoralOuttake;
+import frc.robot.subsystems.EndEffector;
 import frc.robot.utils.SysIdUtils;
 import frc.robot.utils.Telemetry;
 
@@ -45,7 +47,7 @@ public class RobotContainer {
   private final Telemetry m_telemetry = new Telemetry();
   private final CoralOuttake m_coralOuttake = new CoralOuttake();
   private final AlgaeIntake m_algaeIntake = new AlgaeIntake();
-
+  private final EndEffector m_endEffector = new EndEffector();
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final Joystick leftJoystick = new Joystick(USB.leftJoystick);
   private final SendableChooser<Command> m_sysidChooser = new SendableChooser<>();
@@ -173,8 +175,9 @@ private void configureBindings() {
     m_driverController
         .rightBumper()
         .whileTrue(new RunCoralOuttake(m_coralOuttake, -0.15)); // intake
-    m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake
+    m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // ou,ttake
     m_driverController.y().whileTrue(new RunAlgaeIntake(m_algaeIntake, -0.5)); // intake
+    m_driverController.leftTrigger().whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); //
   }
 
   /**
diff --git a/src/main/java/frc/robot/commands/RunEndEffectorIntake.java b/src/main/java/frc/robot/commands/RunEndEffectorIntake.java
index 078eb97..a9530ca 100644
--- a/src/main/java/frc/robot/commands/RunEndEffectorIntake.java
+++ b/src/main/java/frc/robot/commands/RunEndEffectorIntake.java
@@ -5,20 +5,20 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.AlgaeIntake;
+import frc.robot.subsystems.EndEffector;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
-public class RunAlgaeIntake extends Command {
+public class RunEndEffectorIntake extends Command {
 
-  private final AlgaeIntake m_algaeIntake;
+  private final EndEffector m_EndEffector;
   private final double m_speed;
 
   /** Creates a new SetAlgaeIntakeSpeed. */
-  public RunAlgaeIntake(AlgaeIntake algae, double speed) {
-    m_algaeIntake = algae;
+  public RunEndEffectorIntake(EndEffector endeffector, double speed) {
+    m_EndEffector = endeffector;
     m_speed = speed;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(m_algaeIntake);
+    addRequirements(m_EndEffector);
   }
 
   // Called when the command is initially scheduled.
@@ -28,13 +28,13 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_algaeIntake.setPercentOutput(m_speed);
+    m_EndEffector.setPercentOutput(m_speed);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_algaeIntake.setPercentOutput(0);
+    m_EndEffector.setPercentOutput(0);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/constants/CAN.java b/src/main/java/frc/robot/constants/CAN.java
index b6609c6..b04a98b 100644
--- a/src/main/java/frc/robot/constants/CAN.java
+++ b/src/main/java/frc/robot/constants/CAN.java
@@ -22,5 +22,5 @@ public class CAN {
 
   public static final int coralOuttakeMotor = 30;
   public static final int algaeMotor = 31;
-  public static final int EndEffector = 35;
+  public static final int endEffector = 35;
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
index 03a70a7..b489f62 100644
--- a/src/main/java/frc/robot/subsystems/EndEffector.java
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -13,7 +13,7 @@
 import frc.robot.utils.CtreUtils;
 
 public class EndEffector extends SubsystemBase {
-  private final TalonFX m_endEffectorMotor = new TalonFX(CAN.EndEffector);
+  private final TalonFX m_endEffectorMotor = new TalonFX(CAN.endEffector);
 
   /** Creates a new EndEffector. */
   public EndEffector() {

From d17578654fd76ce41c217a295b65eef44c5f94aa Mon Sep 17 00:00:00 2001
From: Gabriel Chavez <specter.assassin.e2o4@gmail.com>
Date: Sun, 26 Jan 2025 15:35:39 -0800
Subject: [PATCH 6/9] WIP on 20-arm-pivot-outline

---
 .../java/frc/robot/constants/ENDEFFECTOR.java | 11 +++++
 src/main/java/frc/robot/constants/V2CAN.java  |  2 +-
 .../frc/robot/subsystems/EndEffector.java     |  4 +-
 .../robot/subsystems/EndEffectorWrist.java    | 45 +++++++++++++++++++
 4 files changed, 59 insertions(+), 3 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/EndEffectorWrist.java

diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
index 64db4de..5462f6a 100644
--- a/src/main/java/frc/robot/constants/ENDEFFECTOR.java
+++ b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
@@ -5,4 +5,15 @@ public class ENDEFFECTOR {
   public static final double kI = 0.0;
   public static final double kD = 0.0;
   public static final double endEffectorGearRatio = 1.0;
+
+  //Pivot motor stuff
+  public static final double kPivotP = 0.0;
+  public static final double kPivotI = 0.0;
+  public static final double kPivotD = 0.0;
+
+  public static final double endEffectorPivotGearRatio = 1.0;
+
+  public enum STATE {
+    
+  }
 }
diff --git a/src/main/java/frc/robot/constants/V2CAN.java b/src/main/java/frc/robot/constants/V2CAN.java
index d5d6b8c..e45a9bb 100644
--- a/src/main/java/frc/robot/constants/V2CAN.java
+++ b/src/main/java/frc/robot/constants/V2CAN.java
@@ -25,7 +25,7 @@ public class V2CAN {
 
   public static final int endEffectorPivotMotor = 32;
   public static final int endEffectorOuttakeMotor = 35;
-  public static final int carriagePivotCanCoder = 36;
+  public static final int endEffecotrPivotCanCoder = 36;
 
   public static final int elevatorMotor1 = 33;
   public static final int elevatorMotor2 = 34;
diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
index 03a70a7..bd28e00 100644
--- a/src/main/java/frc/robot/subsystems/EndEffector.java
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -8,12 +8,12 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.CAN;
+import frc.robot.constants.V2CAN;
 import frc.robot.constants.ENDEFFECTOR;
 import frc.robot.utils.CtreUtils;
 
 public class EndEffector extends SubsystemBase {
-  private final TalonFX m_endEffectorMotor = new TalonFX(CAN.EndEffector);
+  private final TalonFX m_endEffectorMotor = new TalonFX(V2CAN.endEffectorOuttakeMotor);
 
   /** Creates a new EndEffector. */
   public EndEffector() {
diff --git a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
new file mode 100644
index 0000000..f0da8de
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ENDEFFECTOR;
+import frc.robot.constants.V2CAN;
+
+public class EndEffectorWrist extends SubsystemBase {
+
+  private final TalonFX m_pivotMotor = new TalonFX(V2CAN.endEffectorPivotMotor);
+  private final CANcoder m_pivotEncoder = new CANcoder(V2CAN.endEffecotrPivotCanCoder);
+
+  private final NeutralModeValue m_neutralMode = NeutralModeValue.Brake;
+
+  private final MotionMagicTorqueCurrentFOC m_request = new MotionMagicTorqueCurrentFOC(0);
+  // private doulbe m_desiredRotations = 
+  /** Creates a nepw EndEffectorWrist. */
+
+  public EndEffectorWrist() {
+    TalonFXConfiguration configuration = new TalonFXConfiguration();
+      configuration.Slot0.kP = ENDEFFECTOR.kP;
+      configuration.Slot0.kI = ENDEFFECTOR.kI;
+      configuration.Slot0.kD = ENDEFFECTOR.kD;
+      configuration.MotorOutput.NeutralMode = m_neutralMode;
+      configuration.Feedback.RotorToSensorRatio = ENDEFFECTOR.endEffectorGearRatio;
+    
+
+
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From 8c73218aa85464049035d983a8bc1ada6b2a7fcd Mon Sep 17 00:00:00 2001
From: Ronan-B <114883633+Ronan-B@users.noreply.github.com>
Date: Sun, 26 Jan 2025 23:43:07 +0000
Subject: [PATCH 7/9] Automatically applied spotless

---
 .../java/frc/robot/constants/ENDEFFECTOR.java |  6 ++----
 .../frc/robot/subsystems/EndEffector.java     |  2 +-
 .../robot/subsystems/EndEffectorWrist.java    | 19 +++++++------------
 3 files changed, 10 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
index 5462f6a..1a166a1 100644
--- a/src/main/java/frc/robot/constants/ENDEFFECTOR.java
+++ b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
@@ -6,14 +6,12 @@ public class ENDEFFECTOR {
   public static final double kD = 0.0;
   public static final double endEffectorGearRatio = 1.0;
 
-  //Pivot motor stuff
+  // Pivot motor stuff
   public static final double kPivotP = 0.0;
   public static final double kPivotI = 0.0;
   public static final double kPivotD = 0.0;
 
   public static final double endEffectorPivotGearRatio = 1.0;
 
-  public enum STATE {
-    
-  }
+  public enum STATE {}
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
index bd28e00..58afe04 100644
--- a/src/main/java/frc/robot/subsystems/EndEffector.java
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -8,8 +8,8 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.V2CAN;
 import frc.robot.constants.ENDEFFECTOR;
+import frc.robot.constants.V2CAN;
 import frc.robot.utils.CtreUtils;
 
 public class EndEffector extends SubsystemBase {
diff --git a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
index f0da8de..7687456 100644
--- a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
+++ b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
@@ -5,12 +5,10 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ENDEFFECTOR;
 import frc.robot.constants.V2CAN;
@@ -23,19 +21,16 @@ public class EndEffectorWrist extends SubsystemBase {
   private final NeutralModeValue m_neutralMode = NeutralModeValue.Brake;
 
   private final MotionMagicTorqueCurrentFOC m_request = new MotionMagicTorqueCurrentFOC(0);
-  // private doulbe m_desiredRotations = 
-  /** Creates a nepw EndEffectorWrist. */
 
+  // private doulbe m_desiredRotations =
+  /** Creates a nepw EndEffectorWrist. */
   public EndEffectorWrist() {
     TalonFXConfiguration configuration = new TalonFXConfiguration();
-      configuration.Slot0.kP = ENDEFFECTOR.kP;
-      configuration.Slot0.kI = ENDEFFECTOR.kI;
-      configuration.Slot0.kD = ENDEFFECTOR.kD;
-      configuration.MotorOutput.NeutralMode = m_neutralMode;
-      configuration.Feedback.RotorToSensorRatio = ENDEFFECTOR.endEffectorGearRatio;
-    
-
-
+    configuration.Slot0.kP = ENDEFFECTOR.kP;
+    configuration.Slot0.kI = ENDEFFECTOR.kI;
+    configuration.Slot0.kD = ENDEFFECTOR.kD;
+    configuration.MotorOutput.NeutralMode = m_neutralMode;
+    configuration.Feedback.RotorToSensorRatio = ENDEFFECTOR.endEffectorGearRatio;
   }
 
   @Override

From f0dffffd892e2a8f65c5fa8bed083b0a80e0b3f0 Mon Sep 17 00:00:00 2001
From: Gabriel Chavez <specter.assassin.e2o4@gmail.com>
Date: Tue, 28 Jan 2025 09:41:18 -0800
Subject: [PATCH 8/9] WIP on End-Effector

---
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../java/frc/robot/constants/ALPHABOTCAN.java |  1 -
 .../java/frc/robot/constants/ENDEFFECTOR.java | 26 ++++++++++++-
 .../robot/subsystems/EndEffectorWrist.java    | 38 +++++++++++++++++--
 4 files changed, 60 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 39dc2a1..9109cb3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -175,7 +175,7 @@ private void configureBindings() {
     m_driverController
         .rightBumper()
         .whileTrue(new RunCoralOuttake(m_coralOuttake, -0.15)); // intake
-    m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // ou,ttake
+    m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake
     m_driverController.y().whileTrue(new RunAlgaeIntake(m_algaeIntake, -0.5)); // intake
     m_driverController.leftTrigger().whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); //
   }
diff --git a/src/main/java/frc/robot/constants/ALPHABOTCAN.java b/src/main/java/frc/robot/constants/ALPHABOTCAN.java
index 53c69c9..69a9b04 100644
--- a/src/main/java/frc/robot/constants/ALPHABOTCAN.java
+++ b/src/main/java/frc/robot/constants/ALPHABOTCAN.java
@@ -22,5 +22,4 @@ public class ALPHABOTCAN {
 
   public static final int coralOuttakeMotor = 30;
   public static final int algaeMotor = 31;
-  public static final int endEffector = 35;
 }
diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
index 1a166a1..ba25240 100644
--- a/src/main/java/frc/robot/constants/ENDEFFECTOR.java
+++ b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
@@ -7,11 +7,33 @@ public class ENDEFFECTOR {
   public static final double endEffectorGearRatio = 1.0;
 
   // Pivot motor stuff
+  public static final double minAngleDegrees = 270;
+  public static final double maxAngleDegrees = 3;
+  
   public static final double kPivotP = 0.0;
   public static final double kPivotI = 0.0;
   public static final double kPivotD = 0.0;
 
-  public static final double endEffectorPivotGearRatio = 1.0;
+  public static final double endEffectorPivotGearRatio = 70.0;
 
-  public enum STATE {}
+  public enum WRIST_SETPOINT{
+    STOWED(0),
+    L4(0),
+    L3_L2(0);
+
+    private final double angle;
+
+    WRIST_SETPOINT(double angle){
+      this.angle = angle;
+    }
+
+    public double get() {
+      return angle;
+    }
+  }
+
+  public enum STATE {
+    STILL,
+    MOVING
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
index 7687456..920085f 100644
--- a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
+++ b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
@@ -9,6 +9,8 @@
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ENDEFFECTOR;
 import frc.robot.constants.V2CAN;
@@ -20,9 +22,11 @@ public class EndEffectorWrist extends SubsystemBase {
 
   private final NeutralModeValue m_neutralMode = NeutralModeValue.Brake;
 
-  private final MotionMagicTorqueCurrentFOC m_request = new MotionMagicTorqueCurrentFOC(0);
+  // private final MotionMagicTorqueCurrentFOC m_request = new MotionMagicTorqueCurrentFOC();
+
+  private double m_desiredRotations;
+  private boolean m_pivotState;
 
-  // private doulbe m_desiredRotations =
   /** Creates a nepw EndEffectorWrist. */
   public EndEffectorWrist() {
     TalonFXConfiguration configuration = new TalonFXConfiguration();
@@ -30,7 +34,35 @@ public EndEffectorWrist() {
     configuration.Slot0.kI = ENDEFFECTOR.kI;
     configuration.Slot0.kD = ENDEFFECTOR.kD;
     configuration.MotorOutput.NeutralMode = m_neutralMode;
-    configuration.Feedback.RotorToSensorRatio = ENDEFFECTOR.endEffectorGearRatio;
+    configuration.Feedback.RotorToSensorRatio = ENDEFFECTOR.endEffectorPivotGearRatio;
+  }
+
+  public void setState(boolean state) {
+    m_pivotState = state;
+  }
+
+  public boolean getState() {
+    return m_pivotState;
+  }
+
+  public void setPosition(double rotations) {
+    m_desiredRotations = MathUtil.clamp(
+      rotations, 
+      ENDEFFECTOR.minAngleDegrees, 
+      ENDEFFECTOR.maxAngleDegrees
+    );
+  }  
+
+  public double getPosition() {
+    return m_desiredRotations;
+  }
+
+  public void setPercentOutput(double speed) {
+    m_pivotMotor.set(speed);
+  }
+
+  public double getPercentOutput() {
+    return m_pivotMotor.get();
   }
 
   @Override

From 924710b22c4a0bf3a5e502b3c6d9d37a0801b2f5 Mon Sep 17 00:00:00 2001
From: Gener1cU5ername <114034560+Gener1cU5ername@users.noreply.github.com>
Date: Tue, 28 Jan 2025 17:42:35 +0000
Subject: [PATCH 9/9] Automatically applied spotless

---
 src/main/java/frc/robot/constants/ENDEFFECTOR.java    |  6 +++---
 .../java/frc/robot/subsystems/EndEffectorWrist.java   | 11 +++--------
 2 files changed, 6 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
index ba25240..0cb7a83 100644
--- a/src/main/java/frc/robot/constants/ENDEFFECTOR.java
+++ b/src/main/java/frc/robot/constants/ENDEFFECTOR.java
@@ -9,21 +9,21 @@ public class ENDEFFECTOR {
   // Pivot motor stuff
   public static final double minAngleDegrees = 270;
   public static final double maxAngleDegrees = 3;
-  
+
   public static final double kPivotP = 0.0;
   public static final double kPivotI = 0.0;
   public static final double kPivotD = 0.0;
 
   public static final double endEffectorPivotGearRatio = 70.0;
 
-  public enum WRIST_SETPOINT{
+  public enum WRIST_SETPOINT {
     STOWED(0),
     L4(0),
     L3_L2(0);
 
     private final double angle;
 
-    WRIST_SETPOINT(double angle){
+    WRIST_SETPOINT(double angle) {
       this.angle = angle;
     }
 
diff --git a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
index 920085f..012bdf9 100644
--- a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
+++ b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java
@@ -5,11 +5,9 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ENDEFFECTOR;
@@ -46,12 +44,9 @@ public boolean getState() {
   }
 
   public void setPosition(double rotations) {
-    m_desiredRotations = MathUtil.clamp(
-      rotations, 
-      ENDEFFECTOR.minAngleDegrees, 
-      ENDEFFECTOR.maxAngleDegrees
-    );
-  }  
+    m_desiredRotations =
+        MathUtil.clamp(rotations, ENDEFFECTOR.minAngleDegrees, ENDEFFECTOR.maxAngleDegrees);
+  }
 
   public double getPosition() {
     return m_desiredRotations;