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] 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 + } +}