Added Endeffector and Gear Ratios

Created Endeffector Subsystem and added Gear Ratios.

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

@@ -330,11 +330,12 @@ public final class Constants {
     public static final class ElevatorConstants {
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 724);
         
-        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;
+        public static final double GEAR_RATIO = 100.0;
+        public static final double LEVEL_1 = 123 * GEAR_RATIO;
+        public static final double LEVEL_2 = 123 * GEAR_RATIO;
+        public static final double ELEVATOR_MAX_HEIGHT = 123 * GEAR_RATIO;
+        public static final double ELEVATOR_SPEED_UP = 123 * GEAR_RATIO;
+        public static final double ELEVATOR_SPEED_DOWN = -123 * GEAR_RATIO;
 
 
         public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
@@ -342,4 +343,19 @@ public final class Constants {
             .withKI(0)
             .withKD(0);
     }
+
+    public static class EndeffectorConstants {
+        public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 42);
+        
+        public static final double GEAR_RATIO = 100.0;
+        public static final double TOP = 8.0 * GEAR_RATIO;
+        public static final double MIDDLE = 6.0 * GEAR_RATIO;
+        public static final double BOTTOM = 4.0 * GEAR_RATIO;
+
+        public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs()
+            .withKP(0)
+            .withKI(0)
+            .withKD(0);
+        
+    }
 }

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

@@ -40,6 +40,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 // import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Elevator;
+import frc4388.robot.subsystems.Endeffector;
 import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
@@ -63,6 +64,7 @@ public class RobotContainer {
     public final Vision m_vision = new Vision(m_robotMap.camera);
 
     public final Elevator m_robotELevator= new Elevator(m_robotMap.elevator);
+    public final Endeffector m_robotEndeffector = new Endeffector(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);
 
@@ -211,6 +213,17 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotELevator.elevatorUp()))
             .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
+
+            /*Endeffector Controls*/
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotEndeffector.endeffectorTop()));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotEndeffector.endeffectorMiddle()));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotEndeffector.endeffectorBottom()));
     }
     
     /**

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

@@ -19,6 +19,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
 import frc4388.robot.Constants.ElevatorConstants;
+import frc4388.robot.Constants.EndeffectorConstants;
 // import edu.wpi.first.wpilibj.motorcontrol.Spark;
 // import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -52,6 +53,9 @@ public class RobotMap {
 
     /* Elevator Subsystem */
     public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
+
+    /*Endeffector Subsystem*/
+    public final TalonFX endeffector = new TalonFX(EndeffectorConstants.ENDEFFECTOR_ID.id);
     void configureDriveMotorControllers() {}
         
 }

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

@@ -0,0 +1,56 @@
+// 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.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.EndeffectorConstants;
+
+public class Endeffector extends SubsystemBase {
+  /** Creates a new Endefector. */
+  private TalonFX endeffectorMotor;
+  public Endeffector(TalonFX endffectorTalonFX) {
+    endeffectorMotor = endffectorTalonFX;
+
+    endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
+  }
+
+  public void PIDPosition(double position) {
+    var request = new PositionVoltage(position);
+    endeffectorMotor.setControl(request);
+  }
+
+  public void PIDTop() {
+    PIDPosition(EndeffectorConstants.TOP);
+  }
+
+  public void PIDMiddle() {
+    PIDPosition(EndeffectorConstants.MIDDLE);
+  }
+
+  public void PIDBottom() {
+    PIDPosition(EndeffectorConstants.BOTTOM);
+  }
+
+  public void endeffectorTop() {
+    endeffectorMotor.set(EndeffectorConstants.TOP);
+  }
+  
+  public void endeffectorMiddle() {
+    endeffectorMotor.set(EndeffectorConstants.MIDDLE);
+  }
+
+  public void endeffectorBottom() {
+    endeffectorMotor.set(EndeffectorConstants.BOTTOM);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}