From 2fd0092876a848fc93bb826ecf0d72205a487818 Mon Sep 17 00:00:00 2001 From: EdwardFaucher Date: Mon, 20 Feb 2023 19:30:11 -0500 Subject: [PATCH] utvkil --- src/main/java/frc/robot/RobotContainer.java | 6 +++ .../java/frc/robot/commands/Apriltag.java | 49 +++++++++++++++++++ src/main/java/frc/robot/commands/Cube.java | 49 +++++++++++++++++++ src/main/java/frc/robot/commands/Tape.java | 49 +++++++++++++++++++ 4 files changed, 153 insertions(+) create mode 100644 src/main/java/frc/robot/commands/Apriltag.java create mode 100644 src/main/java/frc/robot/commands/Cube.java create mode 100644 src/main/java/frc/robot/commands/Tape.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d64c88d..f04083d 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.apriltag.AprilTag; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -22,10 +23,12 @@ 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; @@ -56,6 +59,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; + } +}