diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 390c2a1..d36da33 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.Limelight; // command import frc.robot.commands.BrakeFerme; import frc.robot.commands.BrakeOuvre; +import frc.robot.commands.Cone; import frc.robot.commands.GratteBaisser; import frc.robot.commands.GratteMonte; import frc.robot.commands.Gyro; @@ -52,7 +53,7 @@ PivotBrasRentre pivotBrasRentre = new PivotBrasRentre(brasTelescopique, pivot); 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()); public RobotContainer() { configureBindings(); diff --git a/src/main/java/frc/robot/commands/Cone.java b/src/main/java/frc/robot/commands/Cone.java new file mode 100644 index 0000000..d780293 --- /dev/null +++ b/src/main/java/frc/robot/commands/Cone.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 Cone extends CommandBase { + private Limelight limelight; + private BasePilotable basePilotable; + private DoubleSupplier doubleSupplier; + /** Creates a new ConeCube. */ + public Cone(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.cone(); + } + + // 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/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 4ed9d06..272c321 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -32,7 +32,7 @@ public class BasePilotable extends SubsystemBase { return gyroscope.getPitch(); } - public void drive(double xSpeed, double zRotation, double d){ + public void drive(double xSpeed, double zRotation){ drive.arcadeDrive(xSpeed, zRotation); } public double distance(){ diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index ac1d815..2f8bcf4 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -6,6 +6,7 @@ package frc.robot.subsystems; import org.photonvision.PhotonCamera; import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,6 +39,18 @@ public class Limelight extends SubsystemBase { limelight.setPipelineIndex(1); } + public double getYaw() { + var result = limelight.getLatestResult(); + if(result.hasTargets()){ + return result.getBestTarget().getYaw(); + } + return 0; + + } + public void pilote(){ + limelight.setLED(VisionLEDMode.kOff); + limelight.setDriverMode(true); + } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 4f378c4..dad3105 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2023.3.0", + "version": "v2023.4.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-cpp", - "version": "v2023.3.0", + "version": "v2023.4.2", "libName": "Photon", "headerClassifier": "headers", "sharedLibrary": true, @@ -30,12 +30,12 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-java", - "version": "v2023.3.0" + "version": "v2023.4.2" }, { "groupId": "org.photonvision", "artifactId": "PhotonTargeting-java", - "version": "v2023.3.0" + "version": "v2023.4.2" } ] } \ No newline at end of file