diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9fc957a..aff140e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -22,16 +23,19 @@ import frc.robot.subsystems.Limelight; import frc.robot.commands.BrakeFerme; import frc.robot.commands.BrakeOuvre; import frc.robot.commands.Cone; +import frc.robot.commands.Cube; import frc.robot.commands.GratteBaisser; import frc.robot.commands.GratteMonte; import frc.robot.commands.Gyro; import frc.robot.commands.Reculer; +import frc.robot.commands.Tape; import frc.robot.commands.bras.FermePince; import frc.robot.commands.bras.OuvrePince; import frc.robot.commands.bras.PivotBrasRentre; import frc.robot.commands.bras.PivoteBrasBas; import frc.robot.commands.bras.PivoteBrasHaut; import frc.robot.commands.bras.PivoteBrasMilieux; +import frc.robot.commands.Apriltag; public class RobotContainer { CommandXboxController manette1 = new CommandXboxController(0); @@ -56,6 +60,9 @@ PivoteBrasBas pivoteBrasBas = new PivoteBrasBas(brasTelescopique, pivot); PivoteBrasMilieux pivoteBrasMilieux = new PivoteBrasMilieux(brasTelescopique, pivot); PivoteBrasHaut pivoteBrasHaut = new PivoteBrasHaut(brasTelescopique, pivot); Cone cone = new Cone(limelight, basePilotable, ()->-manette1.getLeftY()); +Cube cube = new Cube(limelight, basePilotable, ()->-manette1.getLeftY()); +Apriltag aprilTag = new Apriltag(limelight, basePilotable, ()->-manette1.getLeftY()); +Tape tape = new Tape(limelight, basePilotable, ()->-manette1.getLeftY()); public RobotContainer() { configureBindings(); diff --git a/src/main/java/frc/robot/commands/Apriltag.java b/src/main/java/frc/robot/commands/Apriltag.java new file mode 100644 index 0000000..9ef54b3 --- /dev/null +++ b/src/main/java/frc/robot/commands/Apriltag.java @@ -0,0 +1,49 @@ +// 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 java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.BasePilotable; +import frc.robot.subsystems.Limelight; + +public class Apriltag extends CommandBase { + private Limelight limelight; + private BasePilotable basePilotable; + private DoubleSupplier doubleSupplier; + /** Creates a new Apriltag. */ + public Apriltag(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) { + this.basePilotable = basePilotable; + this.limelight = limelight; + this.doubleSupplier = doubleSupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(limelight,basePilotable); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + limelight.apriltag(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.pilote(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Cube.java b/src/main/java/frc/robot/commands/Cube.java new file mode 100644 index 0000000..d54839e --- /dev/null +++ b/src/main/java/frc/robot/commands/Cube.java @@ -0,0 +1,49 @@ +// 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 java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.BasePilotable; +import frc.robot.subsystems.Limelight; + +public class Cube extends CommandBase { + private Limelight limelight; + private BasePilotable basePilotable; + private DoubleSupplier doubleSupplier; + /** Creates a new cube. */ + public Cube(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) { + this.basePilotable = basePilotable; + this.limelight = limelight; + this.doubleSupplier = doubleSupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(limelight,basePilotable); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + limelight.cube(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.pilote(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Tape.java b/src/main/java/frc/robot/commands/Tape.java new file mode 100644 index 0000000..e1346f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/Tape.java @@ -0,0 +1,49 @@ +// 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 java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.BasePilotable; +import frc.robot.subsystems.Limelight; + +public class Tape extends CommandBase { + private Limelight limelight; + private BasePilotable basePilotable; + private DoubleSupplier doubleSupplier; + /** Creates a new Tape. */ + public Tape(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) { + this.basePilotable = basePilotable; + this.limelight = limelight; + this.doubleSupplier = doubleSupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(limelight,basePilotable); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + limelight.tape(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.pilote(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}