diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0e01c85..2880b5b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,14 +68,15 @@ public class RobotContainer { Limelight3 limelight3 = new Limelight3(); Pose2d pose = new Pose2d(); private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2 + public double getAngle() { return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot } - public RobotContainer() { - autoChooser = AutoBuilder.buildAutoChooser("New Auto"); - SmartDashboard.putData("Auto Mode", autoChooser); - configureBindings(); - NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); + public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser("New Auto"); + SmartDashboard.putData("Auto Mode", autoChooser); + configureBindings(); + NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); } private void configureBindings() { elevateur.setDefaultCommand(new RunCommand(()->{ @@ -100,11 +101,11 @@ public class RobotContainer { manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); - manette1.a().whileTrue(new reset(elevateur)); + manette1.a().whileTrue(new reset(elevateur,pince)); manette1.b().whileTrue(new L2(elevateur, pince)); - manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); + // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); diff --git a/src/main/java/frc/robot/commands/reset.java b/src/main/java/frc/robot/commands/reset.java index f49fe97..9a61223 100644 --- a/src/main/java/frc/robot/commands/reset.java +++ b/src/main/java/frc/robot/commands/reset.java @@ -6,6 +6,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevateur; +import frc.robot.subsystems.Pince; @@ -13,10 +14,12 @@ import frc.robot.subsystems.Elevateur; public class reset extends Command { /** Creates a new reset. */ private Elevateur elevateur; - public reset(Elevateur elevateur) { + private Pince pince; + public reset(Elevateur elevateur, Pince pince) { // Use addRequirements() here to declare subsystem dependencies. this.elevateur = elevateur; - addRequirements(elevateur); + this.pince = pince; + addRequirements(elevateur,pince); } // Called when the command is initially scheduled. @@ -27,6 +30,7 @@ public class reset extends Command { @Override public void execute() { elevateur.reset(); + pince.reset(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index ef76d3f..7ef7851 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -16,9 +16,10 @@ public class Elevateur extends SubsystemBase { /** Creates a new Elevateur. */ public Elevateur() { teb.addDouble("encodeur elevateur",this::position); + teb.addBoolean("limit elevateur", this::limit2); } final SparkMax monte = new SparkMax(22, MotorType.kBrushless); - final DigitalInput limit2 = new DigitalInput(1); + final DigitalInput limit2 = new DigitalInput(0); public double position(){ return monte.getEncoder().getPosition(); diff --git a/src/main/java/frc/robot/subsystems/Pince.java b/src/main/java/frc/robot/subsystems/Pince.java index 8cea49b..125c103 100644 --- a/src/main/java/frc/robot/subsystems/Pince.java +++ b/src/main/java/frc/robot/subsystems/Pince.java @@ -10,19 +10,23 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Pince extends SubsystemBase { /** Creates a new Pince. */ - public Pince() {} + ShuffleboardTab teb = Shuffleboard.getTab("teb"); + public Pince() { + teb.addBoolean("limit pince",this::position); + teb.addDouble("encodeur pince", this::encodeurpivot); + } final SparkMax coral = new SparkMax(20, MotorType.kBrushless); final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless); final SparkMax algue1 = new SparkMax(16, MotorType.kBrushless); final SparkMax algue2 = new SparkMax(19, MotorType.kBrushless); - final DigitalInput limit6 = new DigitalInput(0); - GenericEntry teb = Shuffleboard.getTab("teb") - .add("pince encodeur",encodeurpivot()) - .getEntry(); + final DigitalInput limit6 = new DigitalInput(9); + + public void aspirecoral(double vitesse){ coral.set(vitesse); }