diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..98b3f83 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); + m_compressor.enableAnalog(90, 100); } @Override @@ -30,7 +31,7 @@ public void disabledPeriodic() {} @Override public void disabledExit() {} - +//si @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b772969..6183431 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,16 +13,21 @@ import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.ShooterSubsystem; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.commands.IntakeSetCmd; public class RobotContainer { private final Joystick stick1 = new Joystick(0); private final Joystick stick2 = new Joystick(1); + private final Joystick stick3 = new Joystick(2); private final DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); private final DrivetrainDriveCommand drivetrainDriveCommand = new DrivetrainDriveCommand(drivetrainSubsystem, stick1); private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); private final ShooterToggleCommand shooterToggleCommand = new ShooterToggleCommand(shooterSubsystem, stick2); + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + public RobotContainer() { configureBindings(); @@ -30,6 +35,8 @@ public RobotContainer() { private void configureBindings() { CommandScheduler.getInstance().setDefaultCommand(drivetrainSubsystem, drivetrainDriveCommand); new JoystickButton(stick2, 1).onTrue(shooterToggleCommand); + new JoystickButton(stick3, 1).whileTrue(new IntakeSetCmd(intakeSubsystem, true)); + new JoystickButton(stick3, 2).whileTrue(new IntakeSetCmd(intakeSubsystem, false)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/IntakeToggleCommand.java b/src/main/java/frc/robot/commands/IntakeToggleCommand.java new file mode 100644 index 0000000..cf6bb65 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToggleCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IntakeSubsystem; + +public class IntakeSetCmd extends CommandBase { + private final Joystick stick; + + private final IntakeSubsystem intakeSubsystem; + private final boolean open; + + public IntakeSetCmd(IntakeSubsystem intakeSubsystem, boolean open) { + this.open = open; + this.intakeSubsystem = intakeSubsystem; + addRequirements(intakeSubsystem); + } + + @Override + public void initialize() { + System.out.println("Intake has started"); + } + + @Override + public void execute() { + intakeSubsystem.setPosition(open); + } + + @Override + public void end(boolean interrupted) { + intakeSubsystem.stop(); + System.out.println("Intake has ended"); + } + + @Override + public boolean isFinished() { + return false; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index a95b9a8..93d1d7a 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -17,6 +17,7 @@ public class DrivetrainSubsystem extends SubsystemBase{ private final SparkMax rightBack = new SparkMax(98, MotorType.kBrushless); private final SparkMax leftFront = new SparkMax(97, MotorType.kBrushless); private final SparkMax leftBack = new SparkMax(96, MotorType.kBrushless); + private final Compressor m_compressor = new Compressor(1, PneumaticsModuleType.REVPH); private final DifferentialDrive drive = new DifferentialDrive(rightFront, leftFront); public DrivetrainSubsystem() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..4dd0950 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticHub; +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.PneumaticsModuleType; + + +public class IntakeSubsystem extends SubsystemBase { + //Stablishment of Constant for code + public static final class Constants { + public static final double kOpenSpeed = 0.7; + public static final double kCloseSpeed = -0.7; + public static final int kLeftMotorPort = 5; + public static final int kRightMotorPort = 6; + public static final int kPneumaticHubID = 5; + public static final int kPneumaticHubID = 5; + public static final int kSolenoidChannelForward = 0; + public static final int kSolenoidChannelReverse = 1; + } + + //Intake Main Properties + private final PneumaticHub m_pneumaticHub = new PneumaticHub(5); + private Spark intakeLeftMotor = new Spark(5); + private Spark intakeRightMotor = new Spark(6); + private final Compressor m_compressor = new Compressor(5, PneumaticsModuleType.REVPH); + private final DoubleSolenoid IntakeDoubleSolenoid = new DoubleSolenoid( + Constants.kPneumaticHubID, + PneumaticsModuleType.REVPH, + Constants.kForwardChannel, + Constants.kReverseChannel + ); + + public IntakeSubsystem() { + m_compressor.enableAnalog(90, 120); + IntakeDoubleSolenoid.set(DoubleSolenoid.Value.kOff); + } + + @Override + public void periodic() { + } + + //Intake Functions (open or close) + public void setPosition(boolean open) { + if (open) { + intakeLeftMotor.set(IntakeConstants.kOpenSpeed); + intakeRightMotor.set(IntakeConstants.kOpenSpeed); + intakeDoubleSolenoid.set(DoubleSolenoid.Value.kForward); + } else { + intakeLeftMotor.set(IntakeConstants.kCloseSpeed); + intakeRightMotor.set(IntakeConstants.kCloseSpeed); + intakeDoubleSolenoid.set(DoubleSolenoid.Value.kReverse); + } + } + + //Intake Stop + public void stop() { + intakeLeftMotor.set(0); + intakeRightMotor.set(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeTest.java b/src/main/java/frc/robot/subsystems/IntakeTest.java new file mode 100644 index 0000000..aaf2962 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeTest.java @@ -0,0 +1,82 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PneumaticHub; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.TimedRobot; + +public class Robot extends TimedRobot { + + private final PneumaticHub m_pneumaticHub = new PneumaticHub(5); + + private final Compressor m_compressor = + new Compressor(5, PneumaticsModuleType.REVPH); + + private final DoubleSolenoid m_doubleSolenoid = + new DoubleSolenoid( + 5, + PneumaticsModuleType.REVPH, + 0, + 1 + ); + + // ✅ Stick en lugar de XboxController + private final Joystick m_stick = new Joystick(0); + + public Robot() { + // ✅ Compresor controlado SOLO por pressure switch digital + m_compressor.enableAnalog(90, 100); + } + + @Override + public void robotPeriodic() { + + // ✅ SOLO sensor digital (pin 0 del PH) + boolean pressureSwitch = m_pneumaticHub.getPressureSwitch(); + double current = m_pneumaticHub.getCompressorCurrent(); + boolean compressorRunning = m_compressor.isEnabled(); + + DoubleSolenoid.Value pistonState = m_doubleSolenoid.get(); + String pistonStatus; + + if (pistonState == DoubleSolenoid.Value.kForward) { + pistonStatus = "EXTENDIDO"; + } else if (pistonState == DoubleSolenoid.Value.kReverse) { + pistonStatus = "RETRAÍDO"; + } else { + pistonStatus = "APAGADO"; + } + + System.out.println("=== Sistema Neumático (Digital) ==="); + System.out.println("Pressure Switch (DIO 0): " + + (pressureSwitch ? "ACTIVADO" : "DESACTIVADO")); + System.out.println("Corriente del Compresor: " + + String.format("%.2f", current) + " A"); + System.out.println("Compresor Habilitado: " + + (compressorRunning ? "SÍ" : "NO")); + System.out.println("Estado del Pistón: " + pistonStatus); + System.out.println("==================================\n"); + } + + @Override + public void teleopInit() { + m_doubleSolenoid.set(DoubleSolenoid.Value.kReverse); + } + + @Override + public void teleopPeriodic() { + // ✅ Botón 1 del stick + if (m_stick.getRawButton(1)) { + m_doubleSolenoid.set(DoubleSolenoid.Value.kForward); + } else { + m_doubleSolenoid.set(DoubleSolenoid.Value.kReverse); + } + } + + @Override + public void disabledInit() { + m_doubleSolenoid.set(DoubleSolenoid.Value.kOff); + } +} \ No newline at end of file