diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cd7cb9c..25d3767 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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); + + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a51c2af..e211436 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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()));; } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3541a58..c939ca5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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() {} } 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..ef24876 --- /dev/null +++ b/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 + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Endeffector.java b/src/main/java/frc4388/robot/subsystems/Endeffector.java new file mode 100644 index 0000000..ed7d3b5 --- /dev/null +++ b/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 + } +}