From be9f3856f733de9f0ba718ec930ee1d0617dead2 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Sat, 15 Feb 2025 12:38:56 -0500 Subject: [PATCH] limelight marche pas --- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++++++------ .../java/frc/robot/commands/AprilTag3.java | 22 ++++++++++++------- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f683d5..b65ec6c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,10 +25,15 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.TunerConstants.TunerConstants; +import frc.robot.commands.AprilTag3; +import frc.robot.commands.AprilTag3G; +import frc.robot.commands.Forme3; import frc.robot.commands.RainBow; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Grimpeur; +import frc.robot.subsystems.Limelight3; +import frc.robot.subsystems.Limelight3G; public class RobotContainer { @@ -43,14 +48,14 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); private final CommandXboxController manette1 = new CommandXboxController(0); - private final CommandXboxController manette2 = new CommandXboxController(0); + private final CommandXboxController manette2 = new CommandXboxController(1); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private final SendableChooser autoChooser; Bougie bougie = new Bougie(); - - + Limelight3G limelight3g = new Limelight3G(); + Limelight3 limelight3 = new Limelight3(); public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("New Auto"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -63,16 +68,18 @@ public class RobotContainer { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward) - .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left) - .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left) + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.15)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.15)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left) ) ); // reset the field-centric heading on left bumper press manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - + manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain)); + manette1.rightTrigger().whileTrue(new Forme3(limelight3)); + manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g)); drivetrain.registerTelemetry(logger::telemeterize); } diff --git a/src/main/java/frc/robot/commands/AprilTag3.java b/src/main/java/frc/robot/commands/AprilTag3.java index 3a0282f..20a9de9 100644 --- a/src/main/java/frc/robot/commands/AprilTag3.java +++ b/src/main/java/frc/robot/commands/AprilTag3.java @@ -8,15 +8,15 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; - import edu.wpi.first.wpilibj2.command.Command; import frc.robot.TunerConstants.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Limelight3; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class AprilTag3 extends Command { private Limelight3 limelight3; - + private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity @@ -25,9 +25,10 @@ public class AprilTag3 extends Command { .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors /** Creates a new AprilTag3G. */ - public AprilTag3(Limelight3 limelight3) { + public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain) { this.limelight3 = limelight3; - addRequirements(limelight3); + this.drivetrain = drivetrain; + addRequirements(limelight3,drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -41,19 +42,24 @@ public class AprilTag3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + if(limelight3.getV() == true){ - drive.withRotationalRate(limelight3.getX()/48); + drivetrain.applyRequest(() -> + drive.withRotationalRate(limelight3.getX()/48)); + System.out.println(limelight3.getX()/48); } else{ - drive.withRotationalRate(0); + drivetrain.applyRequest(() -> + drive.withRotationalRate(0) + ); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drive.withRotationalRate(0); + drivetrain.applyRequest(() -> + drive.withRotationalRate(0)); } // Returns true when the command should end.