WIP on 20-arm-pivot-outline

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 {
+
+  }
 }

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;

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() {

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
+  }
+}