From 32115fa9778a24f1eb6461f79ef46e860a5fbda6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <121452254+BallisticCrusader@users.noreply.github.com> Date: Fri, 17 Jan 2025 19:53:14 -0700 Subject: [PATCH 1/4] 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 | 14 +++++ .../java/frc4388/robot/RobotContainer.java | 20 ++++++ src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/Elevator.java | 63 +++++++++++++++++++ 4 files changed, 100 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Elevator.java 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 + } +} From faf17348c37c389838c56e1a0b6cf084ae22331f Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <121452254+BallisticCrusader@users.noreply.github.com> Date: Fri, 17 Jan 2025 20:09:14 -0700 Subject: [PATCH 2/4] Updated PID and Motor constants Added PID to constants Used michaels candevice type for motor --- src/main/java/frc4388/robot/Constants.java | 10 ++++++---- src/main/java/frc4388/robot/RobotMap.java | 2 +- src/main/java/frc4388/robot/subsystems/Elevator.java | 9 +++------ 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 53d64a9..91c5d34 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -328,9 +328,8 @@ public final class Constants { } public static final class ElevatorConstants { - public static final class IDs { - public static final int ELEVATOR_ID = 420; - } + 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; @@ -338,6 +337,9 @@ public final class Constants { public static final double ELEVATOR_SPEED_DOWN = -123; - + public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0); } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5ae12c4..1a689a7 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -51,7 +51,7 @@ public class RobotMap { ); /* Elevator Subsystem */ - public final TalonFX elevator = new TalonFX(ElevatorConstants.IDs.ELEVATOR_ID); + public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); void configureDriveMotorControllers() {} } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index b69a720..ef24876 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -19,13 +19,10 @@ public class Elevator extends SubsystemBase { public Elevator(TalonFX elevatorTalonFX) { elevatorMotor = elevatorTalonFX; - elevatorMotor.setNeutralMode(NeutralModeValue.Coast); + elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - var PIDConfigs = new Slot0Configs(); - PIDConfigs.kP = 0; - PIDConfigs.kI = 0; - PIDConfigs.kD = 0; - elevatorMotor.getConfigurator().apply(PIDConfigs); + + elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); } //PID methods From 453e0e54af219b574f59eb6a5684ee31a72782ed Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <121452254+BallisticCrusader@users.noreply.github.com> Date: Fri, 17 Jan 2025 20:55:53 -0700 Subject: [PATCH 3/4] Added Endeffector and Gear Ratios Created Endeffector Subsystem and added Gear Ratios. --- src/main/java/frc4388/robot/Constants.java | 26 +++++++-- .../java/frc4388/robot/RobotContainer.java | 13 +++++ src/main/java/frc4388/robot/RobotMap.java | 4 ++ .../frc4388/robot/subsystems/Endeffector.java | 56 +++++++++++++++++++ 4 files changed, 94 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Endeffector.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 91c5d34..f884ee9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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); + + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 22f0f19..6d6e88a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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())); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1a689a7..c939ca5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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() {} } 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..69d1d81 --- /dev/null +++ b/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 + } +} From d341b6a7d71346eadc8b97c71de80fad17661356 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:35:34 -0700 Subject: [PATCH 4/4] Tested elevator --- src/main/java/frc4388/robot/Constants.java | 26 +++++++++---------- .../java/frc4388/robot/RobotContainer.java | 17 +++++++----- .../frc4388/robot/subsystems/Endeffector.java | 15 ++++------- 3 files changed, 28 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f884ee9..1ebcd1a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -328,32 +328,32 @@ public final class Constants { } public static final class ElevatorConstants { - public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 724); + public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); - 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 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(0) + .withKP(1) .withKI(0) .withKD(0); } public static class EndeffectorConstants { - public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 42); + public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); 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 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(0) + .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 6d6e88a..8b6a0ba 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -64,7 +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 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); @@ -197,11 +197,11 @@ public class RobotContainer { .onFalse(new InstantCommand()); /*DPad for Level 1 and 2*/ - new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + 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.TOP_BOTTOM_DPAD_AXIS) > -0.9) + new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.9) .onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel1())) .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop())); @@ -217,13 +217,16 @@ public class RobotContainer { /*Endeffector Controls*/ new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotEndeffector.endeffectorTop())); + .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDTop())) + .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop())); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotEndeffector.endeffectorMiddle())); + .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDMiddle())) + .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotEndeffector.endeffectorBottom())); + 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/subsystems/Endeffector.java b/src/main/java/frc4388/robot/subsystems/Endeffector.java index 69d1d81..ed7d3b5 100644 --- a/src/main/java/frc4388/robot/subsystems/Endeffector.java +++ b/src/main/java/frc4388/robot/subsystems/Endeffector.java @@ -9,6 +9,7 @@ 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 { @@ -18,6 +19,8 @@ public class Endeffector extends SubsystemBase { endeffectorMotor = endffectorTalonFX; endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); + endeffectorMotor.getConfigurator().apply(EndeffectorConstants.ENDEFECTOR_PID); + } public void PIDPosition(double position) { @@ -37,16 +40,8 @@ public class Endeffector extends SubsystemBase { PIDPosition(EndeffectorConstants.BOTTOM); } - public void endeffectorTop() { - endeffectorMotor.set(EndeffectorConstants.TOP); - } - - public void endeffectorMiddle() { - endeffectorMotor.set(EndeffectorConstants.MIDDLE); - } - - public void endeffectorBottom() { - endeffectorMotor.set(EndeffectorConstants.BOTTOM); + public void endEffectorStop() { + endeffectorMotor.set(0); } @Override