Merge remote-tracking branch 'origin/20-arm-pivot-outline' into End-E…

…ffector

src/main/java/frc/robot/constants/CAN.java → src/main/java/frc/robot/constants/ALPHABOTCAN.java

@@ -1,6 +1,6 @@
 package frc.robot.constants;
 
-public class CAN {
+public class ALPHABOTCAN {
   public static final String rioCanbus = "rio";
   public static String driveBaseCanbus = "drivebase";
 

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

@@ -0,0 +1,40 @@
+package frc.robot.constants;
+
+public class V2CAN {
+  public static final String rioCanbus = "rio";
+  public static String driveBaseCanbus = "drivebase";
+
+  public static final int pigeon = 9;
+
+  public static final int frontLeftCanCoder = 10;
+  public static final int frontRightCanCoder = 11;
+  public static final int backLeftCanCoder = 12;
+  public static final int backRightCanCoder = 13;
+
+  public static final int frontLeftDriveMotor = 20;
+  public static final int frontLeftTurnMotor = 21;
+  public static final int frontRightDriveMotor = 22;
+  public static final int frontRightTurnMotor = 23;
+  public static final int backLeftDriveMotor = 24;
+  public static final int backLeftTurnMotor = 25;
+  public static final int backRightDriveMotor = 26;
+  public static final int backRightTurnMotor = 27;
+
+  public static final int hopperIntakeMotor1 = 30;
+  public static final int hopperIntakeMotor2 = 31;
+
+  public static final int endEffectorPivotMotor = 32;
+  public static final int endEffectorOuttakeMotor = 35;
+  public static final int endEffecotrPivotCanCoder = 36;
+
+  public static final int elevatorMotor1 = 33;
+  public static final int elevatorMotor2 = 34;
+
+  public static final int groundRollerMotor = 40;
+  public static final int groundPivotMotor = 41;
+  public static final int groundPivotCanCoder = 42;
+
+  public static final int cageGrabMotor = 50;
+  public static final int cageArmMotor = 51;
+  public static final int cageArmCanCoder = 52;
+}

src/main/java/frc/robot/generated/TunerConstants.java

@@ -12,7 +12,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.*;
-import frc.robot.constants.CAN;
+import frc.robot.constants.ALPHABOTCAN;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 
 // Generated by the Tuner X Swerve Project Generator for Software Bot/AlphaBot as of 1/6/2025
@@ -140,9 +140,9 @@ public class TunerConstants {
               .withDriveFrictionVoltage(kDriveFrictionVoltage);
 
   // Front Left
-  private static final int kFrontLeftDriveMotorId = CAN.frontLeftDriveMotor;
-  private static final int kFrontLeftSteerMotorId = CAN.frontLeftTurnMotor;
-  private static final int kFrontLeftEncoderId = CAN.frontLeftCanCoder;
+  private static final int kFrontLeftDriveMotorId = ALPHABOTCAN.frontLeftDriveMotor;
+  private static final int kFrontLeftSteerMotorId = ALPHABOTCAN.frontLeftTurnMotor;
+  private static final int kFrontLeftEncoderId = ALPHABOTCAN.frontLeftCanCoder;
   private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.263916015625);
   private static final boolean kFrontLeftSteerMotorInverted = false;
   private static final boolean kFrontLeftEncoderInverted = false;
@@ -151,9 +151,9 @@ public class TunerConstants {
   private static final Distance kFrontLeftYPos = Inches.of(13.75);
 
   // Front Right
-  private static final int kFrontRightDriveMotorId = CAN.frontRightDriveMotor;
-  private static final int kFrontRightSteerMotorId = CAN.frontRightTurnMotor;
-  private static final int kFrontRightEncoderId = CAN.frontRightCanCoder;
+  private static final int kFrontRightDriveMotorId = ALPHABOTCAN.frontRightDriveMotor;
+  private static final int kFrontRightSteerMotorId = ALPHABOTCAN.frontRightTurnMotor;
+  private static final int kFrontRightEncoderId = ALPHABOTCAN.frontRightCanCoder;
   private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.485595703125);
   private static final boolean kFrontRightSteerMotorInverted = false;
   private static final boolean kFrontRightEncoderInverted = false;
@@ -162,9 +162,9 @@ public class TunerConstants {
   private static final Distance kFrontRightYPos = Inches.of(-13.75);
 
   // Back Left
-  private static final int kBackLeftDriveMotorId = CAN.backLeftDriveMotor;
-  private static final int kBackLeftSteerMotorId = CAN.backLeftTurnMotor;
-  private static final int kBackLeftEncoderId = CAN.backLeftCanCoder;
+  private static final int kBackLeftDriveMotorId = ALPHABOTCAN.backLeftDriveMotor;
+  private static final int kBackLeftSteerMotorId = ALPHABOTCAN.backLeftTurnMotor;
+  private static final int kBackLeftEncoderId = ALPHABOTCAN.backLeftCanCoder;
   private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0556640625);
   private static final boolean kBackLeftSteerMotorInverted = false;
   private static final boolean kBackLeftEncoderInverted = false;
@@ -173,9 +173,9 @@ public class TunerConstants {
   private static final Distance kBackLeftYPos = Inches.of(13.75);
 
   // Back Right
-  private static final int kBackRightDriveMotorId = CAN.backRightDriveMotor;
-  private static final int kBackRightSteerMotorId = CAN.backRightTurnMotor;
-  private static final int kBackRightEncoderId = CAN.backRightCanCoder;
+  private static final int kBackRightDriveMotorId = ALPHABOTCAN.backRightDriveMotor;
+  private static final int kBackRightSteerMotorId = ALPHABOTCAN.backRightTurnMotor;
+  private static final int kBackRightEncoderId = ALPHABOTCAN.backRightCanCoder;
   private static final Angle kBackRightEncoderOffset = Rotations.of(-0.201171875);
   private static final boolean kBackRightSteerMotorInverted = false;
   private static final boolean kBackRightEncoderInverted = false;

src/main/java/frc/robot/subsystems/AlgaeIntake.java

@@ -9,12 +9,12 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ALGAE;
-import frc.robot.constants.CAN;
+import frc.robot.constants.ALPHABOTCAN;
 import frc.robot.utils.CtreUtils;
 
 public class AlgaeIntake extends SubsystemBase {
 
-  private final TalonFX m_algaeMotor = new TalonFX(CAN.algaeMotor);
+  private final TalonFX m_algaeMotor = new TalonFX(ALPHABOTCAN.algaeMotor);
 
   /** Creates a new Algae. */
   public AlgaeIntake() {

src/main/java/frc/robot/subsystems/CoralOuttake.java

@@ -25,14 +25,14 @@
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.CAN;
+import frc.robot.constants.ALPHABOTCAN;
 import frc.robot.constants.CORALOUTTAKE;
 import frc.robot.utils.CtreUtils;
 
 public class CoralOuttake extends SubsystemBase {
   private boolean m_isOuttaking = false;
   private LinearSystem<N2, N1, N2> outtakePlant = LinearSystemId.createDCMotorSystem(1, 1);
-  private final TalonFX outtakeMotor = new TalonFX(CAN.coralOuttakeMotor);
+  private final TalonFX outtakeMotor = new TalonFX(ALPHABOTCAN.coralOuttakeMotor);
   private final DCMotorSim outtakeMotorSim =
       new DCMotorSim(outtakePlant, CORALOUTTAKE.gearbox); // TODO implement sim code
   private double m_desiredPercentOutput;

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

src/main/java/frc/robot/utils/CtreUtils.java

@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
-import frc.robot.constants.CAN;
+import frc.robot.constants.ALPHABOTCAN;
 
 // TODO: This class is also a mess
 public final class CtreUtils {
@@ -23,7 +23,7 @@ public static void initPhoenixServer() {
         new Alert("Starting Phoenix Server at: " + Timer.getFPGATimestamp(), AlertType.kInfo);
     alert.set(true);
     if (RobotBase.isReal()) {
-      TalonFX dummy = new TalonFX(0, CAN.driveBaseCanbus);
+      TalonFX dummy = new TalonFX(0, ALPHABOTCAN.driveBaseCanbus);
       Timer.delay(5);
       dummy.close();
       dummy = null;