diff --git a/src/main/java/frc/Subsystems/IntakeSubsystem.java b/src/main/java/frc/Subsystems/IntakeSubsystem.java index 7234c07..d43d79e 100644 --- a/src/main/java/frc/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/Subsystems/IntakeSubsystem.java @@ -16,21 +16,37 @@ public class IntakeSubsystem extends SubsystemBase { public TalonFX IntakeMotor; public double IntakeMotorSpeed; + public double OutakeMotorPower; + public boolean Intake = false; /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { IntakeMotor = new TalonFX(Constants.Intake.INTAKE_MOTOR_PORT); + } public void setIntakePower(double IntakeMotorSpeed){ + Intake = true; this.IntakeMotorSpeed = IntakeMotorSpeed; } + public void setOutakePower(double OutakeMotorPower){ + Intake = false; + this.OutakeMotorPower = OutakeMotorPower; + + } + + @Override public void periodic() { // This method will be called once per scheduler run - IntakeMotor.set(ControlMode.PercentOutput, IntakeMotorSpeed); + if(!Intake){ + IntakeMotor.set(ControlMode.PercentOutput, IntakeMotorSpeed); + } else{ + IntakeMotor.set(ControlMode.PercentOutput, OutakeMotorPower); + } + } } diff --git a/src/main/java/frc/robot/Commands/IntakeCommand.java b/src/main/java/frc/robot/Commands/IntakeCommand.java index c9388a3..13cf526 100644 --- a/src/main/java/frc/robot/Commands/IntakeCommand.java +++ b/src/main/java/frc/robot/Commands/IntakeCommand.java @@ -8,23 +8,18 @@ import frc.Subsystems.IntakeSubsystem; public class IntakeCommand extends CommandBase { - private final IntakeSubsystem subsystem; - /** Creates a new IntakeCommand. */ public IntakeCommand(IntakeSubsystem subsystem) { // Use addRequirements() here to declare subsystem dependencies. this.subsystem = subsystem; addRequirements(subsystem); - } // Called when the command is initially scheduled. @Override - public void initialize() { - - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override @@ -34,9 +29,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) { - subsystem.setIntakePower(0); - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/Commands/IntakeSequentailCommands.java b/src/main/java/frc/robot/Commands/IntakeSequentailCommands.java new file mode 100644 index 0000000..5a06d1e --- /dev/null +++ b/src/main/java/frc/robot/Commands/IntakeSequentailCommands.java @@ -0,0 +1,28 @@ +// 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.Commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.Subsystems.IntakeSubsystem; + + +public class IntakeSequentailCommands extends SequentialCommandGroup{ + + private final IntakeSubsystem subsystem; + + /** Creates a new IntakeCommand. */ + public IntakeSequentailCommands(IntakeSubsystem subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.subsystem = subsystem; + + addRequirements(subsystem); + + addCommands( + new IntakeCommand(subsystem), + new OutakeCommand(subsystem) + ); + + } +} diff --git a/src/main/java/frc/Commands/IntakeCommand.java b/src/main/java/frc/robot/Commands/OutakeCommand.java similarity index 65% rename from src/main/java/frc/Commands/IntakeCommand.java rename to src/main/java/frc/robot/Commands/OutakeCommand.java index bf83173..331b394 100644 --- a/src/main/java/frc/Commands/IntakeCommand.java +++ b/src/main/java/frc/robot/Commands/OutakeCommand.java @@ -2,14 +2,20 @@ // 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.Commands; +package frc.robot.Commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.Subsystems.IntakeSubsystem; -public class IntakeCommand extends CommandBase { - /** Creates a new IntakeCommand. */ - public IntakeCommand() { +public class OutakeCommand extends CommandBase { + private final IntakeSubsystem subsystem; + + /** Creates a new OutakeCommand. */ + public OutakeCommand(IntakeSubsystem subsystem) { // Use addRequirements() here to declare subsystem dependencies. + this.subsystem = subsystem; + + addRequirements(subsystem); } // Called when the command is initially scheduled. @@ -18,7 +24,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + subsystem.setOutakePower(-0.5); + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index da279f4..5f8be1f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,8 @@ import frc.Subsystems.ElevatorSubsystem; import frc.Subsystems.IntakeSubsystem; import frc.robot.Commands.IntakeCommand; +import frc.robot.Commands.IntakeSequentailCommands; +import frc.robot.Commands.OutakeCommand; /** @@ -49,6 +51,9 @@ public RobotContainer() { driverA.a().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 3d)); driverA.rightTrigger().whileTrue(new IntakeCommand(intakeSubsystem)); + driverA.rightBumper().whileTrue(new OutakeCommand(intakeSubsystem)); + + driverA.leftBumper().whileTrue(new IntakeSequentailCommands(intakeSubsystem)); }