diff --git a/README.md b/README.md new file mode 100644 index 0000000..78925fd --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +The version was made by Levi L (or CNrailfaner4039 on YouTube ©) +Other people assisted in the creation on this file (Located in the copyright and colaberators) diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 36c709f..eb3d5f2 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -4,13 +4,13 @@ * Automatically generated file containing build version information. */ public final class BuildConstants { - public static final String MAVEN_GROUP = ""; + public static final String MAVEN_GROUP = "2530"; public static final String MAVEN_NAME = "RobotCode2026"; - public static final String VERSION = "unspecified"; + public static final String VERSION = "ThE mOsT rEcEnT oNe"; public static final int GIT_REVISION = 5; public static final String GIT_SHA = "bccd7eb881eba8a2c445853e5af72829a1aaac19"; - public static final String GIT_DATE = "2026-01-10 09:47:49 CST"; - public static final String GIT_BRANCH = "updating"; + public static final String GIT_DATE = "2026-01-30 09:14:49 CST"; + public static final String GIT_BRANCH = "Climber"; public static final String BUILD_DATE = "2026-01-12 20:55:55 CST"; public static final long BUILD_UNIX_TIME = 1768272955275L; public static final int DIRTY = 1; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 246c8d9..a5f32fa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,21 +66,111 @@ public static Alliance getAlliance() { } } + public static class SwerveModuleConstants { + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double WHEEL_DIAMETER = Units.inchesToMeters(3.85); // ~4 in + public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d); + + // TODO: UPDATE BASED ON REAL ROBOT + // This is for L2 modules with 16T pinions + public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d); + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double DRIVE_ROTATION_TO_METER = DRIVE_GEAR_RATIO * Math.PI * WHEEL_DIAMETER; + public static final double STEER_ROTATION_TO_RADIANS = STEERING_GEAR_RATIO * Math.PI * 2d; + public static final double DRIVE_METERS_PER_MINUTE = DRIVE_ROTATION_TO_METER / 60d; + public static final double STEER_RADIANS_PER_MINUTE = STEER_ROTATION_TO_RADIANS / 60d; + public static final double STEER_MAX_RAD_SEC = 0.8 * STEERING_GEAR_RATIO * ((5880.f * 2.f * Math.PI) / 60.f); + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double WHEEL_FRICTION_COEFFICIENT = 1.2; + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double MODULE_KP = 0.46368; + public static final double MODULE_KD = 0.0050806; + + // --------- Front Left Module --------- \\ + public static final int FL_DRIVE_ID = 10; + public static final int FL_STEER_ID = 11; + public static final int FL_ABSOLUTE_ENCODER_PORT = 30; + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(0); + public static final boolean FL_ABSOLUTE_ENCODER_REVERSED = false; + public static final boolean FL_MOTOR_REVERSED = true; + public static final boolean FL_STEERING_MOTOR_REVERSED = true; + + // --------- Front Right Module --------- \\ + public static final int FR_DRIVE_ID = 12; + public static final int FR_STEER_ID = 13; + public static final int FR_ABSOLUTE_ENCODER_PORT = 31; + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(0); + public static final boolean FR_ABSOLUTE_ENCODER_REVERSED = false; + public static final boolean FR_MOTOR_REVERSED = true; + public static final boolean FR_STEERING_MOTOR_REVERSED = true; + + // --------- Back Right Module --------- \\ + public static final int BR_DRIVE_ID = 14; + public static final int BR_STEER_ID = 15; + public static final int BR_ABSOLUTE_ENCODER_PORT = 32; + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(0); + public static final boolean BR_ABSOLUTE_ENCODER_REVERSED = false; + public static final boolean BR_MOTOR_REVERSED = true; + public static final boolean BR_STEERING_MOTOR_REVERSED = true; + + // --------- Back Left Module --------- \\ + public static final int BL_DRIVE_ID = 16; + public static final int BL_STEER_ID = 17; + public static final int BL_ABSOLUTE_ENCODER_PORT = 33; + + // TODO: UPDATE BASED ON REAL ROBOT + public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(0); + public static final boolean BL_ABSOLUTE_ENCODER_REVERSED = false; + public static final boolean BL_MOTOR_REVERSED = true; + public static final boolean BL_STEERING_MOTOR_REVERSED = true; + } + + + /*|-----------WARNING-----------------| *|edit with caution, used in all subs| *|-----------------------------------| *|Motor Config (YOU HAVE BEEN WARNED)| *|-----------------------------------| - *|edits can have catastafic falure---| + *|edits can have catastafic falures--| *///|-----------WARNING-----------------| //---Intake Motors---\\ public static class IntakeMotors { + //Intake motor 1 + public static final int INTAKESUBSYSTEM_MOTOR = 4;//TODO: IDs and speeds need to be changed public static final int INTAKESUBSYSTEM_MOTOR = 4;//TODO: IDs and speeds need to be changed. DONE: FALSE public static final double INTAKE_SPEED = 0.6; - public static final int INTAKE_MOTOR1 = 5; - public static final double INTAKE_SPEED1 = 0.6; + //Pivot Motor for intake + public static final int INTAKE_PIVOT_MOTOR = 5; + public static final double INTAKE_PIVOT_SPEED = 0.6; + } + //----Climb Motors---\\ + public static class ClimbMotors { + public static final int CLIMBSUBSYSTEM_MOTOR = 6;//TODO: IDs and speeds need to be changed + public static final double CLIMB_SPEED = 0.6; } + //---------Speed Def---------\\ + //time = seconds doing nothing\\ + public static class Sleep { + public static final double up = 3; //TODO: time ajustment. (Time is Seconds) + public static final double finishUP = 1; + public static final double down = 1; + public static final double finishDN = 3; + } + + + public static class DriveConstants { public static final class DriveConstants { // TODO: Make sure that this is correct - this is from the SDS website but needs // empirical verification @@ -268,6 +358,11 @@ public static final class PoseConstants { put(i, TAG_LAYOUT.getTagPose(i + 1).get().toPose2d()); } }}; - } -} + public static final class TurretConstants { + // TODO: UPDATE BASED ON REAL ROBOT + public static final int FlyWheelID = 20; + public static final double FlyWheelSpeed = 0.3; + public static final double FlyWheelDiameterMeters = Units.inchesToMeters(4); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ed190e3..e0dea78 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import java.util.function.BooleanSupplier; + import org.littletonrobotics.urcl.URCL; import com.ctre.phoenix6.SignalLogger; @@ -129,6 +131,8 @@ public void disabledPeriodic() { } + + /** * This autonomous runs the autonomous command selected by your * {@link RobotContainer} class. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18ed039..6d12f30 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,11 +16,17 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; +import frc.robot.commands.ClimberCommand; import frc.robot.commands.DriveCommand; +import frc.robot.commands.IntakeCommand; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.LauncherSubsystem; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.util.AllianceFlipUtil; import frc.robot.subsystems.Limelight.LimelightType; @@ -47,6 +53,110 @@ public class RobotContainer { //initalizing limelight container (Group) public static final LimelightContainer LLContainer = new LimelightContainer(LL_BF, LL_BL, LL_BR, LL_FR); // @Logged + // public static final LimelightContainer LLContainer = new LimelightContainer(LL_name1, LL_name2, LL_name3); + + // @Logged + public final static CommandXboxController driverXbox = new CommandXboxController( + ControllerConstants.DRIVER_CONTROLLER_PORT); + // @Logged + public final CommandXboxController operatorXbox = new CommandXboxController( + ControllerConstants.OPERATOR_CONTROLLER_PORT); + + private final SendableChooser autoChooser; + + @Logged + public final SwerveSubsystem swerveDriveSubsystem = new SwerveSubsystem(); + + // private final LimeLightSubsystem limeLightSubsystem = new + // LimeLightSubsystem(); + @Logged + private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID()); + + /* + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the trigger bindings + configureBindings(); + + DataLogManager.logNetworkTables(true); + DataLogManager.start(); + + + + swerveDriveSubsystem.setDefaultCommand(normalDrive); + + // NamedCommands.registerCommand(null, getAutonomousCommand()); + + swerveDriveSubsystem.configurePathplanner(); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + autoChooser.onChange(new Consumer() { + @Override + public void accept(Command t) { + if (t instanceof PathPlannerAuto) { + PathPlannerAuto auto = (PathPlannerAuto) t; + swerveDriveSubsystem.setAutoStartingPose(auto.getStartingPose()); + } + } + }); + } + + + + + /** + * Use this method to define your trigger->command mappings. Triggers can be + * created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with + * an arbitrary + * predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for + * {@link + * CommandXboxController + * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or + * {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. + */ + boolean isXstance; + + private void configureBindings() { // Using binary send and recive + + // Firing + operatorXbox.x().whileTrue(new InstantCommand(() -> { + LauncherSubsystem.setOutput(1); + })); + operatorXbox.x().whileFalse(new InstantCommand(() -> { + LauncherSubsystem.setOutput(0); + })); + + // Climb + operatorXbox.rightBumper().onTrue(new InstantCommand(() -> { + ClimberCommand.setOutput(1); + })); + // Start Intaking + driverXbox.x().whileTrue(new InstantCommand(() -> { + IntakeCommand.setStart(1); + })); + driverXbox.x().whileFalse(new InstantCommand(() -> { + IntakeCommand.setStart(0); + })); + + // Drop Intake (Light tap needed, no need for hold. Hold until ready to intake, driver holds for the rest, reset automaticly) + // An auto system will be made to remove this for faster gameplay. (all you got to do is replace the check for down with start :>) + operatorXbox.a().onTrue(new InstantCommand(() -> { + IntakeCommand.setDown(1); + })); + + // EMS for Intake (Emergency Use, Reverses Intake to clear) + operatorXbox.y().whileTrue(new InstantCommand(() -> { + IntakeCommand.setEMS(1); + })); + operatorXbox.y().whileFalse(new InstantCommand(() -> { + IntakeCommand.setEMS(0); + })); public final CommandXboxController driverXbox = new CommandXboxController(ControllerConstants.DRIVER_CONTROLLER_PORT); // @Logged public final CommandXboxController operatorXbox = new CommandXboxController(ControllerConstants.OPERATOR_CONTROLLER_PORT); @@ -77,28 +187,10 @@ public RobotContainer() { } - - - /** - * Use this method to define your trigger->command mappings. Triggers can be - * created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with - * an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for - * {@link - * CommandXboxController - * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or - * {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ - private void configureBindings() { - //This is ment for operator controls } - - /** + + /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java new file mode 100644 index 0000000..ee846ff --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -0,0 +1,97 @@ +package frc.robot.commands; + +// import com.revrobotics.spark.SparkFlex; + +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandScheduler; +//import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +//import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +//import frc.robot.commands.DriveCommand; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.ClimberSubsystem; +//import frc.robot.RobotContainer; + +/* + * FILE POSSIBLE WILL BE DEPRICATED FOR TOURNAMENT + * -PRIORITY SWITCH, HOLD TILL FURTHER NOTICE- + */ +public class ClimberCommand extends Command { + private final ClimberSubsystem subsystem1; + + + private ClimbPresets climbPreset; + + public ClimberCommand(ClimberSubsystem subsystem1) { + this.subsystem1 = subsystem1; + addRequirements(subsystem1); + } + + + @Override + public void end(boolean interrupted) { + subsystem1.setClimbPreset(ClimbPresets.IDLE); + SmartDashboard.putString("Climb Ended", "End Fail"); + } + + @Override + public void initialize() { + if (output == 1) { + subsystem1.setClimbPreset(ClimbPresets.CLIMBINGUP); //TODO: Sequence command group this, I'm not sure how + SmartDashboard.putString("Climb Command", "Started Part 1 Climb"); + new WaitCommand(Constants.Sleep.up); + SmartDashboard.putString("Climb Command", "Finished Part 1 Climb"); + subsystem1.setClimbPreset(ClimbPresets.CLIMBINGDOWN); + new WaitCommand(Constants.Sleep.finishUP); + SmartDashboard.putString("Climb Command", "Finished Climb"); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Climbed", true); + } + new WaitUntilCommand(driveXbox.a()); + subsystem1.setClimbPreset(ClimbPresets.CLIMBINGUP); + SmartDashboard.putString("Climb Command", "Started Part 1 Going Down"); + new WaitCommand(Constants.Sleep.down); + SmartDashboard.putString("Climb Command", "Started Part 2 Going Down, Please Reverse to clear the bar!!!!!!!!!!!"); + subsystem1.setClimbPreset(ClimbPresets.CLIMBINGDOWN); + new WaitCommand(Constants.Sleep.finishDN); + subsystem1.setClimbPreset(ClimbPresets.IDLE); + SmartDashboard.putString("Climb Command", "Congrats, you successfully unclimbed from the bar! You can return with RB."); + ClimberCommand.setOutput(0); + } + if (output == 0) { + SmartDashboard.putString("Climb Command", "Idle"); + } + } + + public ClimbPresets getClimbPreset() { + return this.climbPreset; + } + + public enum ClimbPresets { + CLIMBINGUP, + CLIMBINGUPF, + CLIMBINGDOWN, + IDLE, + } + public CommandXboxController getDriverXbox() { + return getDriverXbox(); + } + private CommandXboxController driveXbox; + + + private static double output; + + public static void setOutput(double output) { + ClimberCommand.output = output; + } + public double getOutput() { + return output; + } +} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 216314e..ef49bc0 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -5,6 +5,7 @@ import frc.robot.subsystems.IntakeSubsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class IntakeCommand extends Command { @@ -16,7 +17,7 @@ public IntakeCommand(IntakeSubsystem subsystem) { this.subsystem = subsystem; addRequirements(subsystem); } - + @Override public void end(boolean interrupted) { subsystem.setIntakePreset(IntakePresets.IDLE); @@ -25,13 +26,60 @@ public void end(boolean interrupted) { @Override public void initialize() { - subsystem.setIntakePreset(IntakePresets.INTAKE); - SmartDashboard.putString("Intake Command", "Started"); - + if (down == 1) { + subsystem.setIntakePreset(IntakePresets.OUT); + SmartDashboard.putString("Intake Command", "Started"); + while (start == 1) { + subsystem.setIntakePreset(IntakePresets.INTAKE); + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Collecting", true); + + } + if (start == 0) { + setDown(0); + subsystem.setIntakePreset(IntakePresets.IDLE); + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Collecting", false); + } + } - if (Robot.isSimulation()) { - SmartDashboard.putBoolean("[SIM] Collected", true); - + } + } + if (EMS == 1) { + subsystem.setIntakePreset(IntakePresets.INTAKECLEAR); + if (EMS == 0) { + subsystem.setIntakePreset(IntakePresets.IDLE); + } } + + } + private static double start; + + public static void setStart(double start) { + IntakeCommand.start = start; + } + public double getstart() { + return start; + } +/* + // DOWN IS BEING DEPRECATED WHEN IT IS NECESSARY \\ + \\ REMOVE AND CHANGE DOWN ( REPLACED BY START ) // +*/ + private static double down; + + public static void setDown(double down) { + IntakeCommand.down = down; + } + public double getdown() { + return down; + } + //EMS (Emergency Management system) + private static double EMS; + + public static void setEMS(double ems) { + IntakeCommand.EMS = ems; + } + public double getEMG() { + return EMS; } } diff --git a/src/main/java/frc/robot/commands/TurretCommand.java b/src/main/java/frc/robot/commands/TurretCommand.java new file mode 100644 index 0000000..4891af2 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurretCommand.java @@ -0,0 +1,52 @@ +package frc.robot.commands; + +import frc.robot.subsystems.LauncherSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +/** + * A command that runs the intake at a set speed for collection or ejection. + */ +public class TurretCommand extends Command { + private final LauncherSubsystem m_LauncherSubsystem; + + /** + * Creates a new RunIntakeCommand. + * + * @param intakeSubsystem The subsystem this command will run on. + * @param isReversed If true, the motor will run in reverse (eject). + */ + public TurretCommand(LauncherSubsystem LauncherSubsystem) { + m_LauncherSubsystem = LauncherSubsystem; + // Require the subsystem to prevent other commands from running on it concurrently + addRequirements(LauncherSubsystem); + } + + // Called once when the command is initially scheduled. + @Override + public void initialize() { + // Motor action is performed in execute() for continuous speed control, + // but we'll call the method here too for clarity. + } + + // Called repeatedly while the command is scheduled. + @Override + public void execute() { + m_LauncherSubsystem.runLauncher(); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + // This is crucial: stop the motor when the command ends (i.e., button is released) + m_LauncherSubsystem.stopLauncher(); + } + + // Returns true when the command should end. + // We set this to false so the command only stops when interrupted by the button release. + @Override + public boolean isFinished() { + return false; + } +} + diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..88d9a20 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.commands.ClimberCommand.ClimbPresets; + +public class ClimberSubsystem extends SubsystemBase { + + private final SparkFlex m_climbsubsystemMotor; + @SuppressWarnings("unused") + private ClimbPresets climbPreset; + + public ClimberSubsystem() { + m_climbsubsystemMotor = new SparkFlex(Constants.ClimbMotors.CLIMBSUBSYSTEM_MOTOR, MotorType.kBrushless);//TODO: Change to ACTUAL motor type + + } + // -- Warning: Irreverable when climbing -- \\ + public void setClimbPreset(ClimbPresets climb) { + this.climbPreset = climb; + + if (ClimbPresets.CLIMBINGUP == climb) { + + m_climbsubsystemMotor.set(Constants.ClimbMotors.CLIMB_SPEED); + SmartDashboard.putString("Climbing Subsystem", "Begin"); + + if (ClimbPresets.CLIMBINGDOWN == climb) { + m_climbsubsystemMotor.set(0); + if (DriverStation.isAutonomousEnabled()==false) { + //Button press + } + m_climbsubsystemMotor.set(-Constants.ClimbMotors.CLIMB_SPEED); + SmartDashboard.putString("Climbing Command", "Lowering to finish Climb"); + if (ClimbPresets.CLIMBINGUPF == climb) { + m_climbsubsystemMotor.set(0); + } + } + + + SmartDashboard.putString("Climbing Command", "Finished"); + + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Climbing", false); + + } + } + if (SmartDashboard.putString("Climbing Command", "Finished")) { + m_climbsubsystemMotor.set(0); + SmartDashboard.putString("Climbing Command", "Ended"); + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Climbing", false); + + } + } + if (ClimbPresets.CLIMBINGDOWN == climb) { + m_climbsubsystemMotor.set(Constants.ClimbMotors.CLIMB_SPEED); + + } + } + + public void runClimb() { + m_climbsubsystemMotor.set(Constants.ClimbMotors.CLIMB_SPEED); + + } + +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e4ad15f..41055f3 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -2,7 +2,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.spark.SparkFlex; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; import frc.robot.Constants; import frc.robot.Robot; @@ -12,25 +15,28 @@ public class IntakeSubsystem extends SubsystemBase {// Cloned From MT (NOT FINAL - private final SparkFlex m_intakesubsystemMotor; + private final TalonFX m_intakesubsystemMotor; + private final SparkMax m_intakerotationMotor; private IntakePresets intakePreset; public IntakeSubsystem() { - m_intakesubsystemMotor = new SparkFlex(Constants.IntakeMotors.INTAKESUBSYSTEM_MOTOR, MotorType.kBrushless);//TODO: Change to ACTUAL motor type - + m_intakesubsystemMotor = new TalonFX(Constants.IntakeMotors.INTAKESUBSYSTEM_MOTOR); + m_intakerotationMotor = new SparkMax(Constants.IntakeMotors.INTAKE_PIVOT_MOTOR,MotorType.kBrushless); } public void runIntake() { m_intakesubsystemMotor.set(Constants.IntakeMotors.INTAKE_SPEED); - }//TODO: Update m_intakeMotor when fixed - //After intake is touching the floor, activate the intake entry motor. - // When released, Lift Intake and reverse motor for 2 secs at x2 speed. (NOT DONE) + } public void setIntakePreset(IntakePresets intake) { this.intakePreset = intake; - boolean intakeActive = intake == IntakePresets.INTAKE; - boolean intakeInactive = intake == IntakePresets.IDLE; - if (IntakePresets.INTAKE == intake) { + + if (IntakePresets.OUT == intake) { + m_intakerotationMotor.set(Constants.IntakeMotors.INTAKE_PIVOT_SPEED); + SmartDashboard.putString("Intake Subsystem", "Deployed"); + new WaitCommand(1);// TODO: Change so that it got to the right position + m_intakerotationMotor.set(0); + if (IntakePresets.INTAKE == intake) { m_intakesubsystemMotor.set(Constants.IntakeMotors.INTAKE_SPEED); SmartDashboard.putString("Intake Subsystem", "Begin"); @@ -39,17 +45,23 @@ public void setIntakePreset(IntakePresets intake) { SmartDashboard.putBoolean("[SIM] Intaking", true); } + } } if (IntakePresets.IDLE == intake) { m_intakesubsystemMotor.set(0); SmartDashboard.putString("Intake Subsystem", "Ended"); - + m_intakerotationMotor.set(-Constants.IntakeMotors.INTAKE_PIVOT_SPEED); + new WaitCommand(1); + m_intakerotationMotor.set(0); if (Robot.isSimulation()) { SmartDashboard.putBoolean("[SIM] Intaking", false); } } + if (IntakePresets.INTAKECLEAR == intake) { + m_intakesubsystemMotor.set(-Constants.IntakeMotors.INTAKE_SPEED); + } } @@ -58,6 +70,8 @@ public IntakePresets getIntakePreset() { } public enum IntakePresets { + OUT, + INTAKECLEAR, INTAKE, IDLE, } diff --git a/src/main/java/frc/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/robot/subsystems/LauncherSubsystem.java new file mode 100644 index 0000000..9d43cf0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LauncherSubsystem.java @@ -0,0 +1,66 @@ +// 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 frc.robot.subsystems; + +import edu.wpi.first.wpilibj.CAN; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; +import com.revrobotics.spark.config.SparkMaxConfig; + +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import frc.robot.Robot; +import frc.robot.commands.ClimberCommand; +import frc.robot.Constants; +import com.ctre.phoenix6.hardware.TalonFX; + +public class LauncherSubsystem extends SubsystemBase { + private static final int LauncherID = Constants.TurretConstants.FlyWheelID; + private static final double LauncherSpeed = Constants.TurretConstants.FlyWheelSpeed; + public static SwerveSubsystem SwerveSubsystem; + + private final TalonFX m_Motor; + + + + /** Creates a new ExampleSubsystem. */ + public LauncherSubsystem() { + m_Motor = new TalonFX(LauncherID); + } + + + void setCoastMode() { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(IdleMode.kCoast); + } + + public void runLauncher() { + m_Motor.set(LauncherSpeed); + } + + public void stopLauncher() { + m_Motor.set(0.0); + } + + public void initalize() { //Helps make trigger keys + if (output == 1) { + runLauncher(); + } + if (output == 0) { + stopLauncher(); + } + } + + private static double output; + + public static void setOutput(double output) { + LauncherSubsystem.output = output; + } + public double getOutput() { + return output; + } + +}