diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52434cf..b8e46a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,9 @@ import frc.robot.commands.DriveReverse; import frc.robot.commands.DriveTank; import frc.robot.commands.ExtendArm; +import frc.robot.commands.InCubeOutCone; import frc.robot.commands.ManualArm; +import frc.robot.commands.OutCubeInCone; import frc.robot.commands.ResetDrivePose; import frc.robot.commands.RetractArm; import frc.robot.commands.StartLeavingCommunity; @@ -94,9 +96,8 @@ public class RobotContainer { private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); /* ARM COMMANDS */ - // TODO: THIS - // private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); - // private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); + private final InCubeOutCone inCubeOutCone = new InCubeOutCone(armClaw); + private final OutCubeInCone outCubeInCone = new OutCubeInCone(armClaw); private final ExtendArm extend = new ExtendArm(armExtender); private final RetractArm retract = new RetractArm(armExtender); /* TRAJECTORY COMMAND */ @@ -167,6 +168,12 @@ private void configureBindings() { .whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); joystick.povUp().whileTrue(extend); joystick.povDown().whileTrue(retract); + // in cube out cube + joystick.button(3).whileTrue(inCubeOutCone); + joystick.button(4).whileTrue(outCubeInCone); + // in cone out cone + joystick.button(5).whileTrue(outCubeInCone); + joystick.button(6).whileTrue(inCubeOutCone); // joystick.button(2).whileTrue(closeClawCube); /*joystick diff --git a/src/main/java/frc/robot/commands/InCubeOutCone.java b/src/main/java/frc/robot/commands/InCubeOutCone.java new file mode 100644 index 0000000..7b9bbe6 --- /dev/null +++ b/src/main/java/frc/robot/commands/InCubeOutCone.java @@ -0,0 +1,42 @@ +// 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.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmIntake; + +/** Command to intake a cube or throw a cone on the intake. */ +public class InCubeOutCone extends CommandBase { + private final ArmIntake intake; + + /** Creates a new InCubeOutCone. */ + public InCubeOutCone(ArmIntake intake) { + this.intake = intake; + addRequirements(intake); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.intake.set(Constants.Arm.Intake.CUBE_IN_CONE_OUT_SPEED); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/OutCubeInCone.java b/src/main/java/frc/robot/commands/OutCubeInCone.java new file mode 100644 index 0000000..507a88c --- /dev/null +++ b/src/main/java/frc/robot/commands/OutCubeInCone.java @@ -0,0 +1,42 @@ +// 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.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmIntake; + +/** Command to intake a cube or throw a cone on the intake. */ +public class OutCubeInCone extends CommandBase { + private final ArmIntake intake; + + /** Creates a new OutCubeInCone. */ + public OutCubeInCone(ArmIntake intake) { + this.intake = intake; + addRequirements(intake); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.intake.set(Constants.Arm.Intake.CUBE_OUT_CONE_IN_SPEED); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}