diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b4539a..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; @@ -16,20 +17,25 @@ import frc.robot.subsystems.Gratte; import frc.robot.subsystems.bras.BrasTelescopique; import frc.robot.subsystems.bras.Pince; import frc.robot.subsystems.bras.Pivot; +import pabeles.concurrency.ConcurrencyOps.Reset; 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.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); @@ -53,7 +59,10 @@ 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()); +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(); @@ -66,7 +75,9 @@ public RobotContainer() { private void configureBindings() { manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable)); - + manette1.y().whileTrue(gyro); + manette1.b().toggleOnTrue(Commands.startEnd()); + manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable)); } 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/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/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/GratteMonte.java b/src/main/java/frc/robot/commands/GratteMonte.java index 950dcd2..b460277 100644 --- a/src/main/java/frc/robot/commands/GratteMonte.java +++ b/src/main/java/frc/robot/commands/GratteMonte.java @@ -4,6 +4,8 @@ package frc.robot.commands; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import frc.robot.Constants; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Gratte; @@ -24,7 +26,7 @@ public class GratteMonte extends CommandBase { @Override public void execute() { if(gratte.hautd()){ - gratte.Lever(0); + gratte.Lever(0.5); } else{ gratte.Lever(0.5); @@ -43,7 +45,7 @@ public class GratteMonte extends CommandBase { // Returns true when the command should end. @Override - public boolean isFinished() { + public boolean isFinished(){ return false; - } } + } \ No newline at end of file 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index f749d8c..6ed4487 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -8,6 +8,7 @@ import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -59,6 +60,10 @@ public void BrakeFerme(){ brakedroit.set(Value.kReverse); brakegauche.set(Value.kReverse); } +public void resetGyro(){ + try {gyroscope.reset();} catch(Exception e){DriverStation.reportError("bye bye",true); + } + } /** Creates a new BasePilotable. */ public BasePilotable() { droit.setInverted(true); diff --git a/src/main/java/frc/robot/subsystems/Gratte.java b/src/main/java/frc/robot/subsystems/Gratte.java index 3aa09ac..4cefbae 100644 --- a/src/main/java/frc/robot/subsystems/Gratte.java +++ b/src/main/java/frc/robot/subsystems/Gratte.java @@ -17,6 +17,7 @@ public class Gratte extends SubsystemBase { private DigitalInput limithg = new DigitalInput(Constants.limithg); private DigitalInput limitbd = new DigitalInput(Constants.limitbd); private DigitalInput limitbg = new DigitalInput(Constants.limitbg); + public boolean hautd(){ return limithd.get(); } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 7e88f9b..a17900b 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -8,6 +8,7 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 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; @@ -40,6 +41,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() { CameraServer.startAutomaticCapture(); 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