Merge pull request #18 from Team4388/Elevator

Add elevator and endefector

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

@@ -386,4 +386,36 @@ public final class Constants {
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }
+
+    public static final class ElevatorConstants {
+        public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
+        
+        public static final double GEAR_RATIO = 36.0;
+        public static final double LEVEL_1 = 0 * GEAR_RATIO;
+        public static final double LEVEL_2 = 5 * GEAR_RATIO;
+        public static final double ELEVATOR_MAX_HEIGHT = 5 * GEAR_RATIO;
+        public static final double ELEVATOR_SPEED_UP = 1 * GEAR_RATIO;
+        public static final double ELEVATOR_SPEED_DOWN = -1 * GEAR_RATIO;
+
+
+        public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
+            .withKP(1)
+            .withKI(0)
+            .withKD(0);
+    }
+
+    public static class EndeffectorConstants {
+        public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
+        
+        public static final double GEAR_RATIO = 100.0;
+        public static final double TOP = 0.25 * GEAR_RATIO;
+        public static final double MIDDLE = 0.0 * GEAR_RATIO;
+        public static final double BOTTOM = -0.25 * GEAR_RATIO;
+
+        public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs()
+            .withKP(1)
+            .withKI(0)
+            .withKD(0);
+        
+    }
 }

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

@@ -40,6 +40,8 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 // import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.Elevator;
+import frc4388.robot.subsystems.Endeffector;
 import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
@@ -64,6 +66,8 @@ public class RobotContainer {
     // private final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.camera);
     public final Lidar m_lidar = new Lidar();
+    public final Elevator m_robotELevator= new Elevator(m_robotMap.elevator);
+    public final Endeffector m_robotEndeffector = new Endeffector(m_robotMap.endeffector);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
@@ -195,6 +199,38 @@ public class RobotContainer {
             new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
             true, false))
             .onFalse(new InstantCommand());
+
+            /*DPad for Level 1 and 2*/
+        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.9)
+            .onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel2()))
+            .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_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()));
+
+            /*Endeffector Controls*/
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDTop()))
+            .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDMiddle()))
+            .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDBottom()))
+            .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));;
     }
     
     /**

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

@@ -18,6 +18,8 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 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;
@@ -49,7 +51,11 @@ public class RobotMap {
         Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT
     );
 
-    
+    /* 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/Elevator.java

@@ -0,0 +1,60 @@
+// 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.Brake);
+    
+    
+    elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
+  }
+
+  //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
+  }
+}

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

@@ -0,0 +1,51 @@
+// 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.ElevatorConstants;
+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);
+    endeffectorMotor.getConfigurator().apply(EndeffectorConstants.ENDEFECTOR_PID);
+
+  }
+
+  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 endEffectorStop() {
+    endeffectorMotor.set(0);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}