diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 249a881..53d64a9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; + + + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ecbc21b..22f0f19 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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())); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3541a58..5ae12c4 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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() {} } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java new file mode 100644 index 0000000..b69a720 --- /dev/null +++ b/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 + } +}