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