Created Elevator Subsystem

Added Elevator Subsystem with 2 modes. Use D-Pads for Top or Bottom and Bumpers for free mode.

src/main/java/frc4388/robot/Constants.java

@@ -325,5 +325,19 @@ public final class Constants {
         public static final int XBOX_PROGRAMMER_ID = 2;
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
+    }
+
+    public static final class ElevatorConstants {
+        public static final class IDs {
+            public static final int ELEVATOR_ID = 420;
+        }
+        public static final double LEVEL_1 = 123;
+        public static final double LEVEL_2 = 123;
+        public static final double ELEVATOR_MAX_HEIGHT = 123;
+        public static final double ELEVATOR_SPEED_UP = 123;
+        public static final double ELEVATOR_SPEED_DOWN = -123;
+
+
+        
     }
 }

src/main/java/frc4388/robot/RobotContainer.java

@@ -39,6 +39,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 // Subsystems
 // import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.Elevator;
 import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
@@ -61,6 +62,7 @@ public class RobotContainer {
     // private final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.camera);
 
+    public final Elevator m_robotELevator= new Elevator(m_robotMap.elevator);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
@@ -191,6 +193,24 @@ public class RobotContainer {
             new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
             true, false))
             .onFalse(new InstantCommand());
+
+            /*DPad for Level 1 and 2*/
+        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
+            .onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel2()))
+            .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
+            .onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel1()))
+            .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
+
+            /*Free Brid Mode With Bummpers*/
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotELevator.elevatorDown()))
+            .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotELevator.elevatorUp()))
+            .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
     }
     
     /**

src/main/java/frc4388/robot/RobotMap.java

@@ -18,6 +18,7 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
+import frc4388.robot.Constants.ElevatorConstants;
 // import edu.wpi.first.wpilibj.motorcontrol.Spark;
 // import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -49,7 +50,8 @@ public class RobotMap {
         Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT
     );
 
-    
+    /* Elevator Subsystem */
+    public final TalonFX elevator = new TalonFX(ElevatorConstants.IDs.ELEVATOR_ID);
     void configureDriveMotorControllers() {}
         
 }

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -0,0 +1,63 @@
+// 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 frc4388.robot.subsystems;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ElevatorConstants;
+
+public class Elevator extends SubsystemBase {
+  /** Creates a new Elevator. */
+  private TalonFX elevatorMotor;
+
+  public Elevator(TalonFX elevatorTalonFX) {
+    elevatorMotor = elevatorTalonFX;
+
+    elevatorMotor.setNeutralMode(NeutralModeValue.Coast);
+    
+    var PIDConfigs = new Slot0Configs();
+    PIDConfigs.kP = 0;
+    PIDConfigs.kI = 0;
+    PIDConfigs.kD = 0;
+    elevatorMotor.getConfigurator().apply(PIDConfigs);
+  }
+
+  //PID methods
+
+  public void PIDPosition(double position) {
+    var request = new PositionVoltage(position);
+    elevatorMotor.setControl(request);
+  }
+
+  public void PIDLevel1() {
+    PIDPosition(ElevatorConstants.LEVEL_1);
+  }
+
+  public void PIDLevel2() {
+    PIDPosition(ElevatorConstants.LEVEL_2);
+  }
+
+  public void elevatorUp() {
+    elevatorMotor.set(ElevatorConstants.ELEVATOR_SPEED_UP);
+  }
+
+  public void elevatorDown() {
+    elevatorMotor.set(ElevatorConstants.ELEVATOR_SPEED_UP);
+  }
+
+  public void elevatorStop() {
+    elevatorMotor.set(0);
+  }
+  
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}